void LRoverNavigator::mainLoopAt20Hz() {
//perception
updateSensorReadings();
//localization
LGeoLocation currentLocation = _gps->location();
if (!currentLocation.isValid()) return;
double goalHeadingDeg = currentLocation.headingDegTo(_goalLocation);
_compass->setGoalHeadingDeg(goalHeadingDeg);
//path planning
if (isCurrentEqualToGoalLocation()) {
arrivedAtGoal();
return;
}
LObstacleDistances obstacleDistances = _sonics->obstacleDistances();
if (_state != LRoverStateWallFollow) {
if (obstacleDistances.isObstacleDetected() && !shouldForceCruiseAndIgnoreObstacle(obstacleDistances)) {
_state = LRoverStateWallFollow;
_wallFollowController->resetWallFollowParameters();
}
}
else if (obstacleDistances.isObstacleCleared() || shouldForceCruiseAndIgnoreObstacle(obstacleDistances)) {
_state = LRoverStateCruise;
_cruiseController->resetCruiseParameters();
}
LWheelSpeeds wheelSpeeds;
switch (_state) {
case LRoverStateWallFollow:
wheelSpeeds = _wallFollowController->wallFollowOutput(obstacleDistances);
break;
case LRoverStateCruise:
wheelSpeeds = _cruiseController->cruiseOutput(_compass->offsetFromGoalHeadingDeg());
break;
}
//path execution
#if DRIVE
_motorController->move(wheelSpeeds.leftWheelSpeed, wheelSpeeds.rightWheelSpeed);
#endif
}
LxQtSensors::LxQtSensors(ILxQtPanelPlugin *plugin, QWidget* parent):
QFrame(parent),
mPlugin(plugin),
mSettings(plugin->settings())
{
mDetectedChips = mSensors.getDetectedChips();
/**
* We have all needed data to initialize default settings, we have to do it here as later
* we are using them.
*/
initDefaultSettings();
// Add GUI elements
ProgressBar* pg = NULL;
mLayout = new QBoxLayout(QBoxLayout::LeftToRight, this);
mLayout->setSpacing(0);
mLayout->setContentsMargins(0, 0, 0, 0);
QString chipFeatureLabel;
mSettings->beginGroup("chips");
for (int i = 0; i < mDetectedChips.size(); ++i)
{
mSettings->beginGroup(mDetectedChips[i].getName());
const QList<Feature>& features = mDetectedChips[i].getFeatures();
for (int j = 0; j < features.size(); ++j)
{
if (features[j].getType() == SENSORS_FEATURE_TEMP)
{
chipFeatureLabel = features[j].getLabel();
mSettings->beginGroup(chipFeatureLabel);
pg = new ProgressBar(this);
pg->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred);
// Hide progress bar if it is not enabled
if (!mSettings->value("enabled").toBool())
{
pg->hide();
}
pg->setToolTip(chipFeatureLabel);
pg->setTextVisible(false);
QPalette pal = pg->palette();
QColor color(mSettings->value("color").toString());
pal.setColor(QPalette::Active, QPalette::Highlight, color);
pal.setColor(QPalette::Inactive, QPalette::Highlight, color);
pg->setPalette(pal);
mTemperatureProgressBars.push_back(pg);
mLayout->addWidget(pg);
mSettings->endGroup();
}
}
mSettings->endGroup();
}
mSettings->endGroup();
// Fit plugin to current panel
realign();
// Updated sensors readings to display actual values at start
updateSensorReadings();
// Run timer that will be updating sensor readings
mUpdateSensorReadingsTimer.setParent(this);
connect(&mUpdateSensorReadingsTimer, SIGNAL(timeout()), this, SLOT(updateSensorReadings()));
mUpdateSensorReadingsTimer.start(mSettings->value("updateInterval").toInt() * 1000);
// Run timer that will be showin warning
mWarningAboutHighTemperatureTimer.setParent(this);
connect(&mWarningAboutHighTemperatureTimer, SIGNAL(timeout()), this,
SLOT(warningAboutHighTemperature()));
if (mSettings->value("warningAboutHighTemperature").toBool())
{
mWarningAboutHighTemperatureTimer.start(mWarningAboutHighTemperatureTimerFreq);
}
}
void LxQtSensors::settingsChanged()
{
mUpdateSensorReadingsTimer.setInterval(mSettings->value("updateInterval").toInt() * 1000);
// Iterator for temperature progress bars
QList<ProgressBar*>::iterator temperatureProgressBarsIt =
mTemperatureProgressBars.begin();
mSettings->beginGroup("chips");
for (int i = 0; i < mDetectedChips.size(); ++i)
{
mSettings->beginGroup(mDetectedChips[i].getName());
const QList<Feature>& features = mDetectedChips[i].getFeatures();
for (int j = 0; j < features.size(); ++j)
{
if (features[j].getType() == SENSORS_FEATURE_TEMP)
{
mSettings->beginGroup(features[j].getLabel());
if (mSettings->value("enabled").toBool())
{
(*temperatureProgressBarsIt)->show();
}
else
{
(*temperatureProgressBarsIt)->hide();
}
QPalette pal = (*temperatureProgressBarsIt)->palette();
QColor color(mSettings->value("color").toString());
pal.setColor(QPalette::Active, QPalette::Highlight, color);
pal.setColor(QPalette::Inactive, QPalette::Highlight, color);
(*temperatureProgressBarsIt)->setPalette(pal);
mSettings->endGroup();
// Go to the next temperature progress bar
++temperatureProgressBarsIt;
}
}
mSettings->endGroup();
}
mSettings->endGroup();
if (mSettings->value("warningAboutHighTemperature").toBool())
{
// Update sensors readings to get the list of high temperature progress bars
updateSensorReadings();
mWarningAboutHighTemperatureTimer.start(mWarningAboutHighTemperatureTimerFreq);
}
else if (mWarningAboutHighTemperatureTimer.isActive())
{
mWarningAboutHighTemperatureTimer.stop();
// Update sensors readings to set progress bar values to "normal" height
updateSensorReadings();
}
realign();
update();
}