//PID controller: track perimeter
void Robot::motorControlPerimeter() {
//3 states
//wire is lost
//On the wire staright line pid fast action
//Back slowly to the wire pid soft action
//Control the perimeter motor only each 30ms
if (millis() < nextTimeMotorPerimeterControl) return;
nextTimeMotorPerimeterControl = millis() + 30; //possible 15ms with the DUE
//PerimeterMagMaxValue=2000; //need to change in the future
//tell to the pid where is the mower (Pid.x)
perimeterPID.x = 5 * (double(perimeterMag) / perimeterMagMaxValue);
//tell to the Pid where to go (Pid.w)
if (perimeterInside) {
//perimeterPID.w = -0.5;
perimeterPID.w = 0.5; //SEFAN: Tracking counterclockwise
}
else {
//perimeterPID.w = 0.5;
perimeterPID.w = -0.5; //SEFAN: Tracking counterclockwise
}
// parameter the PID
perimeterPID.y_min = -MaxSpeedperiPwm ;
perimeterPID.y_max = MaxSpeedperiPwm ;
perimeterPID.max_output = MaxSpeedperiPwm ;
// and compute
perimeterPID.compute();
// First state wire is lost
// important to understand TrackingPerimeterTransitionTimeOut It's if during this maximum duration the robot does not make a transition in and out
// then it is supposed to have lost the wire, the PID is stopped to go into blocking mode of one of the two wheels.
// If the trackingPerimeterTransitionTimeOut is too large the robot goes too far out of the wire and goes round in a circle outside the wire without finding it
// If the second condition is true the robot has lost the wire since more trackingPerimeterTransitionTimeOut (2500ms)
// for example it is then necessary to stop one of the 2 wheels to make a half turn and find again the wire
if ((millis() > stateStartTime + 10000) && (millis() > perimeterLastTransitionTime + trackingPerimeterTransitionTimeOut)) {
//If this condition is true one of the 2 wheels makes backward the other continues with the result of the PID (not advised)
if (trackingBlockInnerWheelWhilePerimeterStruggling == 0) { //
if (perimeterInside) {
// rightSpeedperi = max(-MaxSpeedperiPwm, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 + perimeterPID.y));
// leftSpeedperi = -MaxSpeedperiPwm / 2;
leftSpeedperi = max(-MaxSpeedperiPwm, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 + perimeterPID.y)); //SEFAN: Tracking counterclockwise
rightSpeedperi = -MaxSpeedperiPwm / 2; //SEFAN: Tracking counterclockwise
}
else {
// rightSpeedperi = -MaxSpeedperiPwm / 2;
// leftSpeedperi = max(-MaxSpeedperiPwm, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 - perimeterPID.y));
leftSpeedperi = -MaxSpeedperiPwm / 2; //SEFAN: Tracking counterclockwise
rightSpeedperi = max(-MaxSpeedperiPwm, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 - perimeterPID.y)); //SEFAN: Tracking counterclockwise
}
}
//If this condition is true one of the 2 wheels stop rotate the other continues with the result of the PID (advised)
if (trackingBlockInnerWheelWhilePerimeterStruggling == 1) {
if (perimeterInside) {
// rightSpeedperi = max(-MaxSpeedperiPwm, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 + perimeterPID.y));
// leftSpeedperi = 0;
leftSpeedperi = max(-MaxSpeedperiPwm, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 + perimeterPID.y)); //SEFAN: Tracking counterclockwise
rightSpeedperi = 0; //SEFAN: Tracking counterclockwise
}
else {
// rightSpeedperi = 0;
// leftSpeedperi = max(-MaxSpeedperiPwm, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 - perimeterPID.y));
leftSpeedperi = 0; //SEFAN: Tracking counterclockwise
rightSpeedperi = max(-MaxSpeedperiPwm, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 - perimeterPID.y)); //SEFAN: Tracking counterclockwise
}
}
//send to motor
setMotorPWM( leftSpeedperi, rightSpeedperi, false);
// we record The time at which the last wire loss occurred
lastTimeForgetWire = millis();
// if we have lost the wire from too long time (the robot is running in a circle outside the wire we stop everything)
if (millis() > perimeterLastTransitionTime + trackingErrorTimeOut) {
Console.println(F("Error: tracking error"));
addErrorCounter(ERR_TRACKING);
//setNextState(STATE_ERROR,0);
setNextState(STATE_PERI_FIND, 0);
}
// out of the fonction until the next loop
return;
}
// here we have just found again the wire we need a slow return to let the pid temp react by decreasing its action (perimeterPID.y / PeriCoeffAccel)
if ((millis() - lastTimeForgetWire ) < trackingPerimeterTransitionTimeOut) {
//PeriCoeffAccel move gently from 3 to 1 and so perimeterPID.y/PeriCoeffAccel increase during 3 secondes
PeriCoeffAccel = (3000.00 - (millis() - lastTimeForgetWire))/1000.00 ;
if (PeriCoeffAccel < 1.00) PeriCoeffAccel = 1.00;
// rightSpeedperi = max(0, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 1.5 + perimeterPID.y / PeriCoeffAccel));
// leftSpeedperi = max(0, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 1.5 - perimeterPID.y / PeriCoeffAccel));
leftSpeedperi = max(0, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 1.5 + perimeterPID.y / PeriCoeffAccel)); //SEFAN: Tracking counterclockwise
rightSpeedperi = max(0, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 1.5 - perimeterPID.y / PeriCoeffAccel)); //SEFAN: Tracking counterclockwise
}
else
//we are in straight line the pid is total and not/2
{
// rightSpeedperi = max(0, min(MaxSpeedperiPwm, MaxSpeedperiPwm/1.5 + perimeterPID.y));
// leftSpeedperi = max(0, min(MaxSpeedperiPwm, MaxSpeedperiPwm/1.5 - perimeterPID.y));
leftSpeedperi = max(0, min(MaxSpeedperiPwm, MaxSpeedperiPwm/1.5 + perimeterPID.y)); //SEFAN: Tracking counterclockwise
rightSpeedperi = max(0, min(MaxSpeedperiPwm, MaxSpeedperiPwm/1.5 - perimeterPID.y)); //SEFAN: Tracking counterclockwise
}
setMotorPWM( leftSpeedperi, rightSpeedperi, false);
//if the mower move in perfect straight line the transition between in and out is longuer so you need to reset the perimeterLastTransitionTime
if (abs(perimeterMag ) < perimeterMagMaxValue/4) {
perimeterLastTransitionTime = millis(); //initialise perimeterLastTransitionTime in perfect sthraith line
}
}