How motor management work.

B

Bernard

Well-known member
Staff member
Here a fast description of the motor speed and odometry management.
Unfortunately it's very complex and i am not a teacher :ROFLMAO:

The goal is to accelerate for a compute duration (according the distance to drive) ,run at speed max and reduce speed just before the end of the distance to drive using odometry counter.
PS : Distance to drive give us a total movement duration (use later as movement timeout) according the ticks per second motor calibration)

To do that:

In the :
--------------------------------------------------------------------------------
{
Code: 
void Robot::setNextState(byte stateNew, byte dir) {
It is execute only each time state change.

Main code send a distance to drive (stateEndOdometryRight), a speed motorLeftSpeedRpmSet(rpm), and acceleration UseAccelRight (Y/N) or brake UseBrakeRight (Y/N)
For example near line 3878 you can find the code for mower to leave the station.

Code: 
case STATE_STATION_REV: //when start in auto mode the mower first reverse to leave the station
      statusCurr = TRACK_TO_START;
      if (RaspberryPIUse) MyRpi.SendStatusToPi();
      UseAccelLeft = 1;
      UseBrakeLeft = 1;
      UseAccelRight = 1;
      UseBrakeRight = 1;
      motorLeftSpeedRpmSet = motorRightSpeedRpmSet = -motorSpeedMaxRpm / 2 ;
      stateEndOdometryRight = odometryRight - (odometryTicksPerCm * stationRevDist);
      stateEndOdometryLeft = odometryLeft - (odometryTicksPerCm * stationRevDist);
      OdoRampCompute();
      break;

----------------------------------------------------------------------------------------------
now OdoRampCompute(); is launched to compute some var.

Code: 
void Robot::OdoRampCompute() { //execute only one time when a new state execution
  //Compute the accel duration (very important for small distance)
  //Compute when you need to brake the 2 wheels to stop at the ODO
  //Compute the estimate duration of the state so can force next state if the mower is stuck



  stateStartOdometryLeft = odometryLeft;
  stateStartOdometryRight = odometryRight;

  motorRightPID.reset();
  PwmRightSpeed = min(motorSpeedMaxPwm, max(-motorSpeedMaxPwm, map(motorRightSpeedRpmSet, -motorSpeedMaxRpm, motorSpeedMaxRpm, -motorSpeedMaxPwm, motorSpeedMaxPwm)));
  PwmLeftSpeed = min(motorSpeedMaxPwm, max(-motorSpeedMaxPwm, map(motorLeftSpeedRpmSet, -motorSpeedMaxRpm, motorSpeedMaxRpm, -motorSpeedMaxPwm, motorSpeedMaxPwm)));
  //try to find when we need to brake the wheel (depend of the distance)

  int  distToMoveLeft;
  int  distToMoveRight;
  distToMoveLeft = abs(stateStartOdometryLeft - stateEndOdometryLeft);
  distToMoveRight = abs(stateStartOdometryRight - stateEndOdometryRight);
  //left wheel

it 's :


distToMoveRight in odometry ticks
PwmRightSpeed ,the speed in PWM compute using a map function between motorSpeedMaxRpm and motorSpeedMaxPwm


OdoStartBrakeRight the moment when we need to reduce speed.
***************************
Code: 
if (distToMoveRight >= odometryTicksPerRevolution) { //more than 1 rev
    OdoStartBrakeRight =  odometryTicksPerRevolution / 2;
    SpeedOdoMaxRight = PwmRightSpeed;
  }

2 cases, if the distance to drive is > at 1 full rev we start to reduce motor speed at half of the last revolution OdoStartBrakeRight = odometryTicksPerRevolution / 2;
else
we start to reduce speed at the half of the distance to drive OdoStartBrakeRight = distToMoveRight / 2;

movingTimeRight it's in millis the moving max duration of the movement
*************************************************
Code: 
movingTimeRight = 1000 * distToMoveRight / motorTickPerSecond ;
  movingTimeRight = movingTimeRight * motorSpeedMaxPwm / abs(SpeedOdoMaxRight);


accelDurationRight it's in millis the accel duration
********************************************
Code: 
if (movingTimeRight >= motorOdoAccel) accelDurationRight = motorOdoAccel;
  else   accelDurationRight =  movingTimeRight / 2;
we use the value from arduremote setting if the movement duration time is > to it or half of the total movement duration.

Now we know all the main movement var :
distToMoveRight
PwmRightSpeed
OdoStartBrakeRight
accelDurationRight




After these setting the main loop find the same select case STATE_STATION_REV: but this time it's execute non stop .
In the example it's near line 7547

Code: 
case STATE_STATION_REV:

      motorControlOdo();

      if ((moveRightFinish) && (moveLeftFinish) ) {
        if ((motorLeftPWMCurr == 0 ) && (motorRightPWMCurr == 0 )) { //wait until the 2 motor completly stop
          setNextState(STATE_STATION_ROLL, 1);
        }
      }
      if (millis() > (stateStartTime + MaxOdoStateDuration)) {//the motor have not enought power to reach the cible
        if (developerActive) {
          ShowMessage ("Warning station rev not in time Max Compute duration in ms :");
        }
        setNextState(STATE_STATION_ROLL, 1);//if the motor can't reach the odocible in slope
      }
      break;

here we check if the movement is finish or if a timeout is trig (MaxOdoStateDuration)


motorControlOdo(); is execute each 15 ms and is the most important part to set the speed variation of the motor according all var IMU Yaw etc...

Code: 
void Robot::motorControlOdo() {

  // call to reach a ODO cible on left AND right wheel so they don't stop at the same time accel and slow are used to smooth the movement of the mower
  //Stop motor independently when the cible is reach
  //
 
  if (UseBrakeRight && (motorRightSpeedRpmSet >= 0) && (stateEndOdometryRight - odometryRight <= -10)) {//right
    moveRightFinish = true;
    PwmRightSpeed = 0;
    motorRightSpeedRpmSet = 0;
    motorRightRpmCurr = 0;
  }

check if the odometry cible is OK to stop the wheel


here the acceleration part according millis and a duration
Code: 
//  RIGHT WHEEL
  rightSpeed = PwmRightSpeed ; //Normal speed

  if (motorRightSpeedRpmSet > 0) { //forward Right wheel -----------------------------------------------------------------------------
    // ShowMessage(" FR rotate ");
    if (UseAccelRight && (millis() - stateStartTime < accelDurationRight)) { //Accel mode for duration
      //Sinus accel
      angleCorresp = map(millis() - stateStartTime, 0, accelDurationRight, 0, 89);
      rightSpeed = PwmRightSpeed * sin(radians(angleCorresp));
    }
compute the acceleration speed using the accelDurationRight and a sinusoidal ramp (for a smooth movement ,but a linear ramp is also ok)

here the brake part according odometry and actualy position of the encoder
Code: 
if (UseBrakeRight && (odometryRight > stateEndOdometryRight - OdoStartBrakeRight)) { //Braking mode by odometry
      //Sinus brake
      angleCorresp = map(abs(stateEndOdometryRight - odometryRight), OdoStartBrakeRight, 0, 89, 10);
      rightSpeed = PwmRightSpeed * sin(radians(angleCorresp));
    }
code start to reduce speed at OdoStartBrakeRight,


All the code locate after is used to change speed of only one wheel to drive straight of limit some speed (sonar ,near wire ,slope etc.....)


@Fürst Ruprecht
To change the acceleration speed and avoid step loose.
You can try.
1 - Into setting arduremote increase motorOdoAccel , but have only result on large movement (total movement duration > the value you have select) ,also pay attention to do a motor calibration to have the var motorTickPerSecond correctly set.
but i thing your issue is not here but only on small movement

2- replace the sinusoidale accel by a ramp

Code: 
  if (UseAccelRight && (millis() - stateStartTime < accelDurationRight)) { //Accel mode for duration

     ramp = map(millis() - stateStartTime, 0, accelDurationRight, 0, 1);
      rightSpeed = PwmRightSpeed * ramp);

3 - try to increase the small movement duration in OdoRampCompute the distToMoveRight var value, so the code later also increase the acceleration duration.
 
Last edited:
You must log in or register to reply here.
Back
Top