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"));
}
}
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::run() {
// Enter processing loop
stopRequested = false;
while (stopRequested == false) {
// Get movement...
double distanceDelta = coil_getDistance();
double angleDelta = coil_getAngle();
// Emit signals for movement tracker
emit signalMovedDistance(distanceDelta);
emit signalChangedAngle(angleDelta);
// Get other sensor data
int sharpIRSensor = coil_getAnalogSensorDistance();
int bumpsWheelDrop = coil_getBumpsAndWheelDrops();
// Collision?
if (((COIL::BUMPWHEELDROP_BUMP_LEFT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_LEFT) || ((COIL::BUMPWHEELDROP_BUMP_RIGHT & bumpsWheelDrop) == COIL::BUMPWHEELDROP_BUMP_RIGHT)) {
emit signalCollision();
if(create->boolSetting("EMSSCONTROLLER_EMERGENCY_STOP_ENABLED") == true) emergencyStop();
}
// Object detected?
if (sharpIRSensor < create->intSetting("EMSSCONTROLLER_SHARP_IR_SENSOR_CUTOFF_VALUE")) {
emit signalObjectDetected(sharpIRSensor, 0); // Angle is 0 because it is strait ahead always!
if(create->boolSetting("EMSSCONTROLLER_EMERGENCY_STOP_ENABLED") == true && sharpIRSensor < create->intSetting("EMSSCONTROLLER_SHARP_IR_SENSOR_EMERGENCYSTOP_BUFFER_MM")) emergencyStop();
}
// Processs movement
distanceMoved += distanceDelta;
angleTurned += angleDelta;
// Determine wheel speeds
if (mode == EmssController::Idle || mode == EmssController::EmergencyStop) {
// Idle mode!
Lwheel = 0;
Rwheel = 0;
} else if (mode == EmssController::Joystick) {
// Joystick mode!
if (this->yokeY == 0) {
// Left or right
Lwheel = (short) (this->speed * this->yokeX);
Rwheel = -(short) (this->speed * this->yokeX);
} else {
// Move forwards backwards
Lwheel = (short) (this->speed * this->yokeY);
Rwheel = (short) (this->speed * this->yokeY);
}
} else if (mode == EmssController::Move) {
// Move mode!
Lwheel = speed;
Rwheel = speed;
} else if (mode == EmssController::Turn) {
// Turn mode!
if (angleToTurn > 0) {
Lwheel = -speed;
Rwheel = +speed;
} else {
Lwheel = +speed;
Rwheel = -speed;
}
//Debug::print("[EmssController] debug");
} else if (mode == EmssController::WheelDrive) {
// No change, just drive at current wheel values...
}
// Send wheel speeds to COIL
if(mode != EmssController::EmergencyStop) coil_directDrive(Lwheel, Rwheel);
//Debug::print("[EmssController] debug");
// Sleep our interval...
this->msleep(interval);
}
}
