Skip to content
Permalink
Browse files

Revised the navigation code and minor changes

  • Loading branch information
maurodangelo committed Nov 1, 2019
1 parent 708701b commit 4a998c6bebb77127c3d630710cbc60825cc1fbe9
Showing with 40 additions and 37 deletions.
  1. +40 −37 roobopoli/roobokart/modes/mode_nav/NavMode.cpp
@@ -21,9 +21,10 @@ NavMode::NavMode(Serial* ser,Devices* devices,int yourmode, Planning *planning)
{
mymode = yourmode;
currDevices = devices;
//dirPID = new PID(100, -100, 70, 0, 0); // Setting 1
//dirPID = new PID(100, -100, 60, 1, 50); //prima del cambio motore
dirPID = new PID(100, -100, 64, 1, 50); // Setting 1
//dirPID = new PID(100, -100, 50, 20, 25); // Setting 2
dirPID = new PID(100, -100, 85, 4, 6);
//dirPID = new PID(100, -100, 85, 4, 6);

currPlanning = planning;
}
@@ -35,10 +36,10 @@ int NavMode::runMode(void)
printf("Nav Mode: It works!\r\n");
#endif

int speed = currPlanning->getSpeed() ;
double speed = currPlanning->getSpeed() ;
int roadsigndetected = 0; // It stores the information that
double det = 0; // deltaT for PID evaluation
int8_t delSpeed = 0;
double delSpeed = 0;

currentmode = mymode;
nextmode = currPlanning->SetCurrentMode(currentmode);
@@ -56,30 +57,30 @@ int NavMode::runMode(void)
// Reading IRs value
rfrontIR = currDevices->rfrontIR->read();
lfrontIR = currDevices->lfrontIR->read();
cfrontIR = currDevices->cfrontIR->read();
//cfrontIR = currDevices->cfrontIR->read();

// The Duckiebot shall follow a constant value of the difference of the values of IR Sensors, left and right.
// The constant value is evaluated at Calibration time
currentdirection = lfrontIR - rfrontIR;
// The Roobokart shall follow a constant value evaluated at Calibration time
currentdirection = rfrontIR;

// Managing the speed
// Check if the duckiebot leaved the white lane on the right side
if(currentdirection==0) {
// The Duckiebot leaved the whit lane on the righ side, decelerate in order to help the PID algo
// The bot decelerate till the min cruise vel
// Speed is linearly decreased
if(speed > MIN_NAV_SPEED) {
speed = speed - 2;
}
else {
navstatus = HARD_CURVE_STATUS;
}

}else {
// Check if the Roobokart leaved the white lane on the right side
// if(currentdirection==0) {
// // The Roobokart leaved the whit lane on the righ side, decelerate in order to help the PID algo
// // The bot decelerate till the min cruise vel
// // Speed is linearly decreased
// if(speed > MIN_NAV_SPEED) {
// speed = speed - 2;
// }
// else {
// navstatus = HARD_CURVE_STATUS;
// }
//
// }else {
// Accelerate till max cruise vel
//speed = CRUISE_NAV_SPEED;
speed = currPlanning->accelerate(); //50;
navstatus = NORMAL_NAV_STATUS;
}
//navstatus = NORMAL_NAV_STATUS;
// }

// Evaluating PID for direction correction
det = (double)pidtimer.read();
@@ -96,29 +97,31 @@ int NavMode::runMode(void)
#endif


switch(navstatus){
//switch(navstatus){

// Managing normal navigation
// In this status the roadsign can be detected
case NORMAL_NAV_STATUS:
// Duckiebot navigates
//case NORMAL_NAV_STATUS:
// Roobokart navigates
if(roadsigndetected == 0) {
// Check if the road sign is detected
if(currDevices->roadsignDetected(cfrontIR)){
// Once the road sign has been detected, the duckiebot proceeds at slower speed
currDevices->currMotors.turn(0, 40, MOTOR_LEFT , MOTOR_RIGHT);
speed = (0.8)*speed;
currDevices->currMotors.turn(0, speed, MOTOR_LEFT , MOTOR_RIGHT);
roadsigndetected = 1;
}else {
// The duckiebot navigates until the road sign is detected
//delSpeed = 0;
delSpeed = direction;
if (delSpeed<0) delSpeed *= -1;
delSpeed = delSpeed/100*speed/1.9;
currDevices->currMotors.turn(direction, speed, MOTOR_LEFT , MOTOR_RIGHT);
delSpeed = (delSpeed/100)*speed;///1.1;
currDevices->currMotors.turn(-direction, speed-delSpeed, MOTOR_LEFT , MOTOR_RIGHT);
}
}
// Road sign detected - managing the handshake with next mode
else {
// The Duckiebot is stopped as soon as the cfrontIR sensor is over the blue line
// The Roobokart is stopped as soon as the cfrontIR sensor is over the blue line
if(!currDevices->roadsignDetected(cfrontIR)){
currDevices->currMotors.stop(); //.turn(0, 0, MOTOR_LEFT , MOTOR_RIGHT);

@@ -127,17 +130,17 @@ int NavMode::runMode(void)
break;
}
}
break;
//break;

// Managing Hard curve on the right
// This status manages only the curve. It isn't possible to detect the roadsign
// No controls, only lane search based on PID algo
case HARD_CURVE_STATUS:
currDevices->currMotors.turn(direction, speed, MOTOR_LEFT , MOTOR_RIGHT);
break;


} // end switch
// case HARD_CURVE_STATUS:
// currDevices->currMotors.turn(direction, speed, MOTOR_LEFT , MOTOR_RIGHT);
// break;
//
//
// } // end switch
wait_ms(10);//40
} // end while

0 comments on commit 4a998c6

Please sign in to comment.
You can’t perform that action at this time.