// PID controller: track perimeter
void Robot::motorControlPerimeter() {
if (millis() < nextTimeMotorPerimeterControl) return;
nextTimeMotorPerimeterControl = millis() + 15; //bb read the perimeter each 15ms not possible with the MEGA maxi 30ms
//never stop the PID compute while turning for the new transition
//use the PerimeterMag as cible to smooth the tracking
//Value reference perimeterMagMaxValue , maybe need to be calculate in mower setting up procedure
perimeterPID.x = double(perimeterMag) / perimeterMagMaxValue; // PID is compute between 1 and -1
//if (perimeterPID.x > 1 ) perimeterPID.x = 1;
//if (perimeterPID.x < -1 ) perimeterPID.x = -1;
perimeterPID.w = 0;
perimeterPID.y_min = -MaxSpeedperiPwm;
perimeterPID.y_max = MaxSpeedperiPwm;
perimeterPID.max_output = MaxSpeedperiPwm;
perimeterPID.compute();
perimeterPID.y = 2.5 * perimeterPID.y;
if ((millis() > stateStartTime + 10000) && (millis() > perimeterLastTransitionTime + trackingPerimeterTransitionTimeOut)) {
// robot is wheel-spinning while tracking => roll to get ground again
if (trackingBlockInnerWheelWhilePerimeterStruggling == 0) {
if (perimeterInside) {
rightSpeedperi = max(-MaxSpeedperiPwm, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 + perimeterPID.y));
leftSpeedperi = -MaxSpeedperiPwm / 2;
}
else {
rightSpeedperi = -MaxSpeedperiPwm / 2;
leftSpeedperi = max(-MaxSpeedperiPwm, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 - perimeterPID.y));
}
}
if (trackingBlockInnerWheelWhilePerimeterStruggling == 1) {
if (perimeterInside) {
rightSpeedperi = max(-MaxSpeedperiPwm, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 + perimeterPID.y));
leftSpeedperi = 0;
}
else {
rightSpeedperi = 0;
leftSpeedperi = max(-MaxSpeedperiPwm, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 - perimeterPID.y));
}
}
if (consoleMode == CONSOLE_TRACKING) {
Console.print("Perte du fil ; ");
Console.print(millis());
Console.print("; MAG: ;");
Console.print (perimeterMag);
Console.print("; ;");
Console.print(perimeterInside);
Console.print("; Speed: L/R ;");
Console.print (leftSpeedperi);
Console.print(";");
Console.print (rightSpeedperi);
Console.print("; PerimeterPID.x: ;");
Console.print (perimeterPID.x);
Console.print("; PerimeterPID.y: ;");
Console.print(perimeterPID.y);
Console.print("; perimeterLastTransitionTime: ;");
Console.println(perimeterLastTransitionTime);
}
setMotorPWM( leftSpeedperi, rightSpeedperi, false);
lastTimeForgetWire = millis();
if (millis() > perimeterLastTransitionTime + trackingErrorTimeOut) {
Console.println("Error: tracking error");
addErrorCounter(ERR_TRACKING);
//setNextState(STATE_ERROR,0);
setNextState(STATE_PERI_FIND, 0);
}
return;
}
if ((millis() - lastTimeForgetWire ) < trackingPerimeterTransitionTimeOut / 2 ) {
rightSpeedperi = max(0, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 + perimeterPID.y / 2));
leftSpeedperi = max(0, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 - perimeterPID.y / 2));
if (consoleMode == CONSOLE_TRACKING) {
Console.print("retour lent sur fil ; ");
}
}
else
{
rightSpeedperi = max(0, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 + perimeterPID.y));
leftSpeedperi = max(0, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 - perimeterPID.y));
if (consoleMode == CONSOLE_TRACKING) {
Console.print("suivi du fil ; ");
}
}
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
//Value of timedOutIfBelowSmag depend on the distance between the coil and the perimeter wire maybe need to be calculate in mower setting up procedure
//timedOutIfBelowSmag hope it is the value read a the point the more far of the perimeter wire
// if ((perimeterMag > -300) && (perimeterMag < 300)) {
if (abs(perimeterMag ) < 300) { //300 can be replace by timedOutIfBelowSmag to be tested
perimeterLastTransitionTime = millis(); //initialise perimeterLastTransitionTime if perfect sthraith line
}
if (consoleMode == CONSOLE_TRACKING) {
Console.print(millis());
Console.print("; MAG: ;");
Console.print (perimeterMag);
Console.print("; ;");
Console.print(perimeterInside);
Console.print("; Speed: L/R ;");
Console.print (leftSpeedperi);
Console.print(";");
Console.print (rightSpeedperi);
Console.print("; PerimeterPID.x: ;");
Console.print (perimeterPID.x);
Console.print("; PerimeterPID.y: ;");
Console.print(perimeterPID.y);
Console.print("; perimeterLastTransitionTime: ;");
Console.println(perimeterLastTransitionTime);
}
}