bool MyReactiveInterface::senseObstacles(CSimplePointsMap &obstacles){
cout << "senseObstacles" << endl;
CObservation2DRangeScan laserScan;
CPose2D rPose;
CPose3D rPose3D;
getCurrentMeasures(laserScan,rPose);
rPose3D=rPose;
obstacles.insertionOptions.minDistBetweenLaserPoints=0.005f;
obstacles.insertionOptions.also_interpolate=false;
obstacles.clear();
obstacles.insertObservation(&laserScan);
// Update data for plot thread
pthread_mutex_lock(&m_mutex);
path.AddVertex(rPose.x(),rPose.y());
laserCurrentMap.loadFromRangeScan(laserScan,&rPose3D);
newScan=true;
pthread_mutex_unlock(&m_mutex);
refreshPlot();
return true;
}
Widget::Widget(Model * model, QWidget *parent) :
QWidget(parent),
ui(new Ui::Widget),
model(model),
initializing(true),
acceptorEnabled(false),
donorEnabled(true),
plotVariant(PV_FERMI_DISTRIBUTION),
logYScale(false),
invertedXScale(true)
{
ui->setupUi(this);
QwtPlot* plotArea = findChild<QwtPlot*>("plotArea");
plotArea->setCanvasBackground(QColor(255,255,255));
configure_curve(&mainCurve, plotArea, "", QColor(50, 50, 200));
mainCurve->setRenderHint(QwtPlotCurve::RenderAntialiased);
configure_curve(&EvCurve, plotArea, "Ev", Qt::black);
configure_curve(&EcCurve, plotArea, "Ec", Qt::black);
configure_curve(&EdCurve, plotArea, "Ed", Qt::black, Qt::DashLine);
configure_curve(&EaCurve, plotArea, "Ea", Qt::black, Qt::DashLine);
configure_curve(&fermiLevelCurve, plotArea, "Fermi Level", Qt::red, Qt::DashDotLine);
copySiliconFromModel();
copyAdmixturesDefaultFromModel();
copyOthersDefaultFromModel();
findChild<QCheckBox*>("acceptorCheckBox")->setChecked(acceptorEnabled);
findChild<QCheckBox*>("donorCheckBox")->setChecked(donorEnabled);
findChild<QCheckBox*>("logScaleCheckBox")->setChecked(logYScale);
findChild<QCheckBox*>("invertedScaleCheckBox")->setChecked(invertedXScale);
QComboBox * plotVariantsComboBox = findChild<QComboBox*>("plotVariantsComboBox");
const QString plot_captions[_PV_COUNT] =
{
tr("Fermi distribution"),
tr("Fermi level"),
tr("Charged acceptors"),
tr("Charged donors"),
tr("Free electrons"),
tr("Free holes")
};
for(int i = 0; i < _PV_COUNT; ++i)
{
plotVariantsComboBox->addItem(plot_captions[i], i);
}
plotVariantsComboBox->setCurrentIndex(plotVariant);
initializing = false;
refreshPlot();
}
//Plot the GyrZ value
void MainWindow::plotGyrZ(uint8_t graph)
{
int x[plot_len], y[plot_len];
//Generate x index - ToDo optimize
for (int i=0; i<plot_len; ++i)
{
x[i] = i; // x goes from 0 to 1
}
//Get new datapoint from Stream:
update_plot_buf_gyrz(exec1.imu.z);
qCopy(plot_buf_gyrz, plot_buf_gyrz+plot_len, y);
refreshPlot(x, y, plot_len, graph);
}
//Test data, to showcase Plot when no COM port is available
void MainWindow::genTestData(uint8_t graph)
{
int x[plot_len], y[plot_len];
static int offset = 0;
offset+=1;
if(offset > 100)
offset = 0;
for (int i=0; i<plot_len; ++i)
{
x[i] = i; // x goes from 0 to 1
y[i] = i + (offset); // let's plot a quadratic function
}
refreshPlot(x, y, plot_len, graph);
}
void Widget::on_donorCheckBox_stateChanged(int check_state)
{
donorEnabled = (check_state == Qt::Checked);
update_Nd();
if(donorEnabled)
{
if(plotVariant == PV_FERMI_DISTRIBUTION || plotVariant == PV_FERMI_LEVEL)
EdCurve->attach(findChild<QwtPlot*>("plotArea"));
}
else
{
EdCurve->detach();
}
findChild<QDoubleSpinBox*>("EdSpinner")->setEnabled(donorEnabled);
findChild<QSlider*>("NdSlider")->setEnabled(donorEnabled);
findChild<QLineEdit*>("NdLineEdit")->setEnabled(donorEnabled);
refreshPlot();
}
void
LTMWindow::compareChanged()
{
if (!amVisible()) {
compareDirty = true;
return;
}
if (isCompare()) {
// refresh plot handles the compare case
refreshPlot();
} else {
// forced refresh back to normal
stackDirty = dirty = true;
filterChanged(); // forces reread etc
}
repaint();
}
void Widget::on_invertedScaleCheckBox_stateChanged(int state)
{
invertedXScale = (state == Qt::Checked);
refreshPlot();
}
void Widget::on_logScaleCheckBox_stateChanged(int state)
{
logYScale = (state == Qt::Checked);
refreshPlot();
}
void Widget::on_plotVariantsComboBox_currentIndexChanged(int index)
{
plotVariant = index;
reattach_level_curves();
refreshPlot();
}
void Widget::on_TstepSpinner_valueChanged(double value)
{
model->set_Tstep(value);
refreshPlot();
}
void Widget::on_NdSlider_valueChanged(int)
{
update_Nd();
refreshPlot();
}
void Widget::on_EdSpinner_valueChanged(double value)
{
model->set_Ed_eV(value);
refreshPlot();
}
void Widget::on_permittivitySpinner_valueChanged(double value)
{
model->set_permittivity(value);
refreshPlot();
}
void Widget::on_mvSpinner_valueChanged(double value)
{
model->set_mv_m0(value);
refreshPlot();
}
void MainWindow::timerPlotEvent(void)
{
uint8_t index = 0;
int y[PLOT_BUF_LEN];
//We go through the list and we update the appropriate data:
data_to_plot[0] = ui->cBoxvar1->currentIndex();
data_to_plot[1] = ui->cBoxvar2->currentIndex();
data_to_plot[2] = ui->cBoxvar3->currentIndex();
data_to_plot[3] = ui->cBoxvar4->currentIndex();
data_to_plot[4] = ui->cBoxvar5->currentIndex();
data_to_plot[5] = ui->cBoxvar6->currentIndex();
for(index = 0; index < VAR_NUM; index++)
{
//Update buffers with latest results:
switch(data_to_plot[index])
{
case 0: //"**Unused**"
update_graph_array(index, 0);
break;
case 1: //"Accel X"
update_graph_array(index, exec1.accel.x);
break;
case 2: //"Accel Y"
update_graph_array(index, exec1.accel.y);
break;
case 3: //"Accel Z"
update_graph_array(index, exec1.accel.z);
break;
case 4: //"Gyro X"
update_graph_array(index, exec1.gyro.x);
break;
case 5: //"Gyro Y"
update_graph_array(index, exec1.gyro.y);
break;
case 6: //"Gyro Z"
update_graph_array(index, exec1.gyro.z);
break;
case 7: //"Encoder"
update_graph_array(index, exec1.enc_display);
break;
case 8: //"Motor current"
update_graph_array(index, exec1.current);
break;
case 9: //"Analog[0]"
update_graph_array(index, (int) exec1.analog[0]);
break;
case 10: //"Strain"
update_graph_array(index, exec1.strain);
break;
case 11: //"+VB"
update_graph_array(index, exec1.volt_batt);
break;
case 12: //"+VG"
update_graph_array(index, exec1.volt_int);
break;
case 13: //"Temp"
update_graph_array(index, exec1.temp);
break;
case 14: //"Fake Data"
update_graph_array(index, gen_test_data());
break;
case 15: //"Setpoint (square)"
update_graph_array(index, ctrl_setpoint);
break;
case 16: //"Setpoint (trap)"
update_graph_array(index, ctrl_setpoint_trap);
break;
case 17: //"Strain ch1"
update_graph_array(index, strain[0].strain_filtered);
break;
case 18: //"Strain ch2"
update_graph_array(index, strain[1].strain_filtered);
break;
case 19: //"Strain ch3"
update_graph_array(index, strain[2].strain_filtered);
break;
case 20: //"Strain ch4"
update_graph_array(index, strain[3].strain_filtered);
break;
case 21: //"Strain ch5"
update_graph_array(index, strain[4].strain_filtered);
break;
case 22: //"Strain ch6"
update_graph_array(index, strain[5].strain_filtered);
break;
case 23: //"AS5047 (Mot.)"
update_graph_array(index, ricnu_1.ex.enc_commut);
break;
case 24: //"AS5048 (Joint)"
update_graph_array(index, ricnu_1.ex.enc_control);
break;
}
//Copy buffers and plot:
qCopy(graph_yarray[index], graph_yarray[index] + plot_len, y);
refreshPlot(graph_xarray, y, plot_len, index);
}
}
