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")); } }