Change perimeter tracking direction

petrzel · 3 März 2019

Hello all,
has anyone idea how to change perimeter tracking direction? I try exchange perimeter wires on sender and reverse coil polarity but direction is still same.
Thanks for advance
Best regards
Pavel

bernard · 4 März 2019

Hi.
If you have 2 coil on your mower you can use Raindancer (the tracking is the opposite as Azurit).

If you use Azurit.
You need to make change in the code (Reverse the Coil or wire can't work).
When find the perimeter:
In robot.cpp

Code:

void Robot::checkPerimeterFind(){
  if (!perimeterUse) return;
  if (stateCurr == STATE_PERI_FIND){
    if (perimeterInside) {
      // inside
      if (motorLeftSpeedRpmSet != motorRightSpeedRpmSet){      
        // we just made an 'outside=>inside' rotation, now track
        setNextState(STATE_PERI_TRACK, 0);    
      }
    } else {
      // we are outside, now roll to get inside
      motorRightSpeedRpmSet = -motorSpeedMaxRpm / 1.5;
      motorLeftSpeedRpmSet  = motorSpeedMaxRpm / 1.5;
    }
  }
}

need to reverse the rotation dir of the wheel.
motorRightSpeedRpmSet = motorSpeedMaxRpm / 1.5;
motorLeftSpeedRpmSet = -motorSpeedMaxRpm / 1.5;

And into motor.h

Code:

//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;
  }
  else {
    perimeterPID.w = 0.5;
  }
  //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;
      }
      else {
        rightSpeedperi = -MaxSpeedperiPwm / 2;
        leftSpeedperi = max(-MaxSpeedperiPwm, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 - perimeterPID.y));
      }
    }
	  //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;
      }
      else {
        rightSpeedperi = 0;
        leftSpeedperi = max(-MaxSpeedperiPwm, min(MaxSpeedperiPwm, MaxSpeedperiPwm / 2 - perimeterPID.y));
      }
    }
	
	  //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));
		}
		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));    
		}

		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
		}  
}

Same thing all the motor direction need to be reverse.

CERTAINLY need to test and see for other issue.

By.

petrzel · 4 März 2019

Thank you; that is what I thought. I try it next weekend on cottage.

petrzel · 10 März 2019

I added configuration parameter to pfod for change perimeter tracking direction and modified checkPerimeterFind() and motorControlPerimeter() and all working as expected

Once again thank you for your advice.

asmanavicius · 26 April 2019

which places need to change in the motorControlPerimeter() ?

petrzel · 26 April 2019

I added configuration parameter, changes is in motor.h in method motorControlPerimeter(). Here is my code (not optimal but working):

Code:

// PID controller: track perimeter 
void Robot::motorControlPerimeter (int speedPwm) {
  //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

  //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)
  // PP: modified for inverse perimeter direction
  if (perimeterInside) {
    perimeterPID.w = invertPerimeterDirection ? 0.5 : -0.5;
  } else {
    perimeterPID.w = invertPerimeterDirection ? -0.5 : 0.5;
  }

  //parameter the PID 
  perimeterPID.y_min = -speedPwm;
  perimeterPID.y_max = speedPwm;
  perimeterPID.max_output = speedPwm;

  //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 (invertPerimeterDirection) {
        // PP: reverse code
        if (perimeterInside) {
          rightSpeedperi = -speedPwm / 2;
          leftSpeedperi = max(-speedPwm, min(speedPwm, speedPwm / 2  + perimeterPID.y));
        } else {
          rightSpeedperi = max(-speedPwm, min(speedPwm, speedPwm / 2 - perimeterPID.y));
          leftSpeedperi = -speedPwm / 2;
        }
      } else {
        // PP: original code
        if (perimeterInside) {
          leftSpeedperi = -speedPwm / 2;
          rightSpeedperi = max(-speedPwm, min(speedPwm, speedPwm / 2  + perimeterPID.y));
        } else {
          leftSpeedperi = max(-speedPwm, min(speedPwm, speedPwm / 2 - perimeterPID.y));
          rightSpeedperi = -speedPwm / 2;
        }
      }
    }
	  //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 (invertPerimeterDirection) {
        // PP: reverse code
        if (perimeterInside) {
          rightSpeedperi = 0;
          leftSpeedperi = max(-speedPwm, min(speedPwm, speedPwm / 2  + perimeterPID.y));
        } else {
          rightSpeedperi = max(-speedPwm, min(speedPwm, speedPwm / 2 - perimeterPID.y));
          leftSpeedperi = 0;
        }
      } else {
        // PP: original code
        if (perimeterInside) {
          leftSpeedperi = 0;
          rightSpeedperi = max(-speedPwm, min(speedPwm, speedPwm / 2  + perimeterPID.y));
        } else {
          leftSpeedperi = max(-speedPwm, min(speedPwm, speedPwm / 2 - perimeterPID.y));
          rightSpeedperi = 0;
        }
      }
    }
	
	  //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;

      if (invertPerimeterDirection) {
        // PP: reverse code
			  leftSpeedperi = max(0, min(speedPwm, speedPwm / 1.5 +  perimeterPID.y / PeriCoeffAccel));
			  rightSpeedperi = max(0, min(speedPwm, speedPwm / 1.5 -  perimeterPID.y / PeriCoeffAccel));
      } else {
        // PP: original code
			  rightSpeedperi = max(0, min(speedPwm, speedPwm / 1.5 +  perimeterPID.y / PeriCoeffAccel));
			  leftSpeedperi = max(0, min(speedPwm, speedPwm / 1.5 -  perimeterPID.y / PeriCoeffAccel));
      }
		} else {
		  //we are in straight line the pid is total and not/2
      if (invertPerimeterDirection) {
        // PP: reverse code
			  leftSpeedperi = max(0, min(speedPwm, speedPwm/1.5   + perimeterPID.y));
			  rightSpeedperi = max(0, min(speedPwm, speedPwm/1.5  - perimeterPID.y));    
      } else {
        // PP: original code
			  rightSpeedperi = max(0, min(speedPwm, speedPwm/1.5   + perimeterPID.y));
			  leftSpeedperi = max(0, min(speedPwm, speedPwm/1.5  - perimeterPID.y));    
      }
		}

		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
		}  
}

also is changed robot rotation in robot.cpp method checkPerimeterFind():

Code:

void Robot::checkPerimeterFind() {
  if ((stateCurr == STATE_PERI_FIND) || (stateCurr == STATE_PERI_FIND_FOR_MOW)) {
    if (perimeterInside) {
      // inside
      if (motorLeftSpeedRpmSet != motorRightSpeedRpmSet){      
        // we just made an 'outside=>inside' rotation, now track
        setNextState (STATE_PERI_TRACK, 0);
      }
    } else {
      // we are outside, now roll to get inside
      if (invertPerimeterDirection) {
        // PP: reverse code
        motorRightSpeedRpmSet = motorSpeedMaxRpm / 1.5;
        motorLeftSpeedRpmSet  = -motorSpeedMaxRpm / 1.5;
      } else {
        // PP: original code
        motorRightSpeedRpmSet = -motorSpeedMaxRpm / 1.5;
        motorLeftSpeedRpmSet  = motorSpeedMaxRpm / 1.5;
      }
    }
  }
}

asmanavicius · 29 April 2019

Changed perimeter tracking direction is working well, but when the robot drives out of the charging station, it turn to wrong direction and stops due to perimeter error. I'm thinking need to do additional changes for this reason.

petrzel · 29 April 2019

You can modify roll when exit charging station in robot.cpp, method Robot::setNextState

Code:

...
  } else if (stateNew == STATE_STATION_ROLL){
    motorLeftSpeedRpmSet = motorSpeedMaxRpm;
    motorRightSpeedRpmSet = -motorSpeedMaxRpm;                              
    stateEndTime = millis() + stationRollTime + motorZeroSettleTime; 
...

bernard · 30 April 2019

Hi the roll dir is the second arg of the setNextState
So from timer it's state forward, after it's state station check and in station check you can find the rev and station_roll
Try to change the roll dir in Line 1656

Code:

} if (millis() >= stateEndTime) setNextState(STATE_STATION_ROLL, 0);

by

Code:

} if (millis() >= stateEndTime) setNextState(STATE_STATION_ROLL, 1);

And test

Suche

Suche

Change perimeter tracking direction

petrzel

New member

bernard

Well-known member

petrzel

New member

petrzel

New member

asmanavicius

New member

petrzel

New member

asmanavicius

New member

petrzel

New member

bernard

Well-known member