void EmssController::emergencyStop() {
create->coil->directDrive(0, 0);
mode = EmssController::EmergencyStop;
emit signalChangedWheelSpeed(0, 0);
Debug::warning("[EmssController] emergency stop!");
}
void EmssController::process() {
// Determine wheel speeds
if (mode == EmssController::Idle || mode == EmssController::EmergencyStop) {
// Idle mode!
Lwheel = 0;
Rwheel = 0;
} else if (mode == EmssController::WheelDrive) {
// No change, just drive at current wheel values...
}
// Get all the sensor data...
if(sensorData) {
if(create->coil->getAllSensors(sensorData)) {
emit signalSensorDataUpdated();
} else {
// Return out of the process until Core can process the
// signal from COIL. This will help COIL disconnect
// faster...
return;
}
}
// Get movement...
double distanceDelta = create->coil->extractSensorFromData(sensorData,COIL::SENSOR_DISTANCE);
double angleDelta = create->coil->extractSensorFromData(sensorData,COIL::SENSOR_ANGLE);
// Emit signals for movement tracker
emit signalMovedDistance(distanceDelta);
emit signalChangedAngle(angleDelta);
// Get other sensor data
int sharpIRSensor = create->arduino->extractSensorFromData(create->arduinoController->sensorData, ArduinoCOIL::SENSOR_IR_2);
sharpIRSensor *= 25.4;
// int sharpIRSensor = create->coil->getIRSensorDistanceFromAnalogSignal(create->coil->extractSensorFromData(sensorData,COIL::SENSOR_ANALOG_SIGNAL));
int wallIRSensor = create->coil->getWallSensorDistanceFromSignal(create->coil->extractSensorFromData(sensorData,COIL::SENSOR_WALL_SIGNAL));
int bumpsWheelDrop = create->coil->extractSensorFromData(sensorData,COIL::SENSOR_BUMPS_AND_WHEEL_DROPS);
bool cliffLeft = create->coil->extractSensorFromData(sensorData,COIL::SENSOR_CLIFF_LEFT);
bool cliffFrontLeft = create->coil->extractSensorFromData(sensorData,COIL::SENSOR_CLIFF_FRONT_LEFT);
bool cliffFrontRight = create->coil->extractSensorFromData(sensorData,COIL::SENSOR_CLIFF_FRONT_RIGHT);
bool cliffRight = create->coil->extractSensorFromData(sensorData,COIL::SENSOR_CLIFF_RIGHT);
bool isForwardsDirection = (Lwheel > 0 && Rwheel > 0);
//ebug::print("[EmssController] sensor: %1", sharpIRSensor);
// Drop detected?
if (cliffLeft || cliffFrontLeft || cliffFrontRight || cliffRight) {
int angle;
if (cliffLeft){
angle = create->intSetting("Robot_SideCliffSensorPositionAngle");
emit signalObjectDetected(bumperCollisionOffset, angle, cliffCollisionOpacity, cliffCollisionSize );
} else if (cliffFrontLeft){
angle = create->intSetting("Robot_FrontCliffSensorPositionAngle");
emit signalObjectDetected(bumperCollisionOffset, angle, cliffCollisionOpacity, cliffCollisionSize );
} else if (cliffFrontRight){
angle = -create->intSetting("Robot_FrontCliffSensorPositionAngle");
emit signalObjectDetected(bumperCollisionOffset, angle, cliffCollisionOpacity, cliffCollisionSize );
} else if (cliffRight){
angle = -create->intSetting("Robot_SideCliffSensorPositionAngle");
emit signalObjectDetected(bumperCollisionOffset, angle, cliffCollisionOpacity, cliffCollisionSize );
}
if(isForwardsDirection && create->boolSetting("Controller_EmssController_EmergencyStopEnabled")) emergencyStop();
}
// Bumper Collision?
if (((COIL::BUMPWHEELDROP_BUMP_LEFT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_LEFT) || ((COIL::BUMPWHEELDROP_BUMP_RIGHT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_RIGHT)) {
int angle = 0;
// both bumper?
if (((COIL::BUMPWHEELDROP_BUMP_LEFT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_LEFT) && ((COIL::BUMPWHEELDROP_BUMP_RIGHT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_RIGHT)){
angle = 0;
}
// only left bumper?
else if ((COIL::BUMPWHEELDROP_BUMP_LEFT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_LEFT){
angle = 45;
}
// only right bumper?
else if ((COIL::BUMPWHEELDROP_BUMP_LEFT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_LEFT){
angle = -45;
}
emit signalObjectDetected(bumperCollisionOffset, angle, bumperCollisionOpacity, bumperCollisionSize );
if(isForwardsDirection && emergencyStopEnabled) emergencyStop();
}
// Wall detected?
if(wallIRSensor < robotWallSensorRange) {
emit signalObjectDetected(robotDiameter/2 + wallIRSensor, -90, wallCollisionOpacity, wallCollisionSize ); // Angle is -90 because the sensor points straight out to the right...
}
// Object detected?
if (sharpIRSensor < create->intSetting("Robot_SharpIRSensorCutoffValue")) {
emit signalObjectDetected(sharpIRSensor, 0, irCollisionOpacity, irCollisionSize ); // Angle is 0 because it is straight ahead always!
}
// Send wheel speeds to COIL
if(mode != EmssController::EmergencyStop) {
// Set new wheel speed
create->coil->directDrive(Lwheel, Rwheel);
emit signalChangedWheelSpeed((int)Lwheel, (int)Rwheel);
// Register on heat map as safe area
create->heatMap->registerHeat(ColorMap::OpenAreaChannel, create->tracker->getX(), create->tracker->getY(), openAreaOpacity, openAreaSize);
}
// Time for debug info?
if(debugInfoEnabled && lastDebugInfo.elapsed() > debugInfoInterval) {
lastDebugInfo.restart();
Debug::print("[EmssController] vl=%1\tvr=%2", Lwheel, Rwheel);
}
}
void EmssController::process() {
// Determine wheel speeds
if (mode == EmssController::Idle || mode == EmssController::EmergencyStop) {
// Idle mode!
Lwheel = 0;
Rwheel = 0;
} else if (mode == EmssController::WheelDrive) {
// No change, just drive at current wheel values...
}
// Get all the sensor data...
if(sensorData) {
core->coil->getAllSensors(sensorData);
emit signalSensorDataUpdated();
}
// Get movement...
double distanceDelta = core->coil->extractSensorFromData(sensorData,COIL::SENSOR_DISTANCE);
double angleDelta = core->coil->extractSensorFromData(sensorData,COIL::SENSOR_ANGLE);
// Emit signals for movement tracker
emit signalMovedDistance(distanceDelta);
emit signalChangedAngle(angleDelta);
// Get other sensor data
int sharpIRSensor = core->coil->getIRSensorDistanceFromAnalogSignal(core->coil->extractSensorFromData(sensorData,COIL::SENSOR_ANALOG_SIGNAL));
int wallIRSensor = core->coil->getWallSensorDistanceFromSignal(core->coil->extractSensorFromData(sensorData,COIL::SENSOR_WALL_SIGNAL));
int bumpsWheelDrop = core->coil->extractSensorFromData(sensorData,COIL::SENSOR_BUMPS_AND_WHEEL_DROPS);
bool cliffLeft = core->coil->extractSensorFromData(sensorData,COIL::SENSOR_CLIFF_LEFT);
bool cliffFrontLeft = core->coil->extractSensorFromData(sensorData,COIL::SENSOR_CLIFF_FRONT_LEFT);
bool cliffFrontRight = core->coil->extractSensorFromData(sensorData,COIL::SENSOR_CLIFF_FRONT_RIGHT);
bool cliffRight = core->coil->extractSensorFromData(sensorData,COIL::SENSOR_CLIFF_RIGHT);
bool isForwardsDirection = (Lwheel > 0 && Rwheel > 0);
// Log movement
distanceMoved += distanceDelta;
angleTurned += angleDelta;
// Drop detected?
if (cliffLeft || cliffFrontLeft || cliffFrontRight || cliffRight) {
int angle;
if (cliffLeft){
angle = core->intSetting("Robot_SideCliffSensorPositionAngle");
emit signalObjectDetected(core->intSetting("Robot_BumperCollisionOffset_mm"), angle,core->doubleSetting("Map_HeatMap_CliffCollisionOpacity"),core->intSetting("Map_HeatMap_CliffCollisionSize_mm") );
} else if (cliffFrontLeft){
angle = core->intSetting("Robot_FrontCliffSensorPositionAngle");
emit signalObjectDetected(core->intSetting("Robot_BumperCollisionOffset_mm"), angle,core->doubleSetting("Map_HeatMap_CliffCollisionOpacity"),core->intSetting("Map_HeatMap_CliffCollisionSize_mm") );
} else if (cliffFrontRight){
angle = -core->intSetting("Robot_FrontCliffSensorPositionAngle");
emit signalObjectDetected(core->intSetting("Robot_BumperCollisionOffset_mm"), angle,core->doubleSetting("Map_HeatMap_CliffCollisionOpacity"),core->intSetting("Map_HeatMap_CliffCollisionSize_mm") );
} else if (cliffRight){
angle = -core->intSetting("Robot_SideCliffSensorPositionAngle");
emit signalObjectDetected(core->intSetting("Robot_BumperCollisionOffset_mm"), angle,core->doubleSetting("Map_HeatMap_CliffCollisionOpacity"),core->intSetting("Map_HeatMap_CliffCollisionSize_mm") );
}
if(isForwardsDirection && core->boolSetting("Controller_EmssController_EmergencyStopEnabled")) _emergencyStop();
}
// Bumper Collision?
if (((COIL::BUMPWHEELDROP_BUMP_LEFT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_LEFT) || ((COIL::BUMPWHEELDROP_BUMP_RIGHT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_RIGHT)) {
int angle = 0;
// both bumper?
if (((COIL::BUMPWHEELDROP_BUMP_LEFT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_LEFT) && ((COIL::BUMPWHEELDROP_BUMP_RIGHT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_RIGHT)){
angle = 0;
}
// only left bumper?
else if ((COIL::BUMPWHEELDROP_BUMP_LEFT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_LEFT){
angle = 45;
}
// only right bumper?
else if ((COIL::BUMPWHEELDROP_BUMP_LEFT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_LEFT){
angle = -45;
}
emit signalObjectDetected(core->intSetting("Robot_BumperCollisionOffset_mm"), angle ,core->doubleSetting("Map_HeatMap_BumperCollisionOpacity"),core->intSetting("Map_HeatMap_BumperCollisionSize_mm") );
if(isForwardsDirection && core->boolSetting("Controller_EmssController_EmergencyStopEnabled")) _emergencyStop();
}
// Wall detected?
if(wallIRSensor < core->doubleSetting("Robot_WallSensorRange_mm")) {
emit signalObjectDetected(core->intSetting("Robot_Diameter_mm")/2 + wallIRSensor, -90, core->doubleSetting("Map_HeatMap_WallCollisionOpacity"),core->intSetting("Map_HeatMap_WallCollisionSize_mm") ); // Angle is -90 because the sensor points straight out to the right...
}
// Object detected?
if (sharpIRSensor < core->intSetting("Robot_SharpIRSensorCutoffValue")) {
emit signalObjectDetected(sharpIRSensor, 0, core->doubleSetting("Map_HeatMap_IRCollisionOpacity"),core->intSetting("Map_HeatMap_IRCollisionSize_mm") ); // Angle is 0 because it is straight ahead always!
//if(isForwardsDirection && core->boolSetting("Controller_EmssController_EmergencyStopEnabled") && sharpIRSensor < core->intSetting("Controller_EmssController_SharpIRSensorEmergencyStopBuffer_mm")) _emergencyStop();
}
// Send wheel speeds to COIL
if(mode != EmssController::EmergencyStop) {
// Set new wheel speed
core->coil->directDrive(Lwheel, Rwheel);
emit signalChangedWheelSpeed(Lwheel, Rwheel);
// Register on heat map as safe area
core->heatMap->registerHeat(ColorMap::OpenAreaChannel, core->tracker->getX(), core->tracker->getY(), core->doubleSetting("Map_HeatMap_OpenAreaOpacity"), core->intSetting("Map_HeatMap_OpenAreaSize_mm"));
}
}
