Hi all
I finish my test with the Odometry control on Azurit and here how it's work.
Main states are now control in distance move with accel and brake on left and right Wheel.
The Accel is linear and timed control (the sinus compute work also but not better)
The brake is sinus compute and Odometry control to reach a cible.
Into each one shot state there is now:
The accel and brake boolean for the 2 wheels.
The speed and direction of each Wheel.
The Odometry cible for each Wheel.
Here a sample of two states (I replace all the If, ElseIf by a switch case because it's more clear for this parts of code and easier to find the one shot and permanent test corresponding)
Into each permanent state there is now.
A test to know if the OdoCible is reach.
A test to wait until a motor completly stop if rotation is inverted.
And the next state.
Here sample of code
And also new OdoRampCompute function to compute the accel brake etc..
Code
And the new motorControlOdo
For the moment it's work but need more test to be sure there is no problem on slope and motordriver because the mowing is very fast.
The main problem is for small movement (less the 1/4 of Wheel turn) , If PWM too low the motor don't turn and the Odocible never reach.
Maybe i add a Maxtime into each state and jump to the other if trigger ?
Video .
https://youtu.be/5usduXYCLEg
I finish my test with the Odometry control on Azurit and here how it's work.
Main states are now control in distance move with accel and brake on left and right Wheel.
The Accel is linear and timed control (the sinus compute work also but not better)
The brake is sinus compute and Odometry control to reach a cible.
Into each one shot state there is now:
The accel and brake boolean for the 2 wheels.
The speed and direction of each Wheel.
The Odometry cible for each Wheel.
Here a sample of two states (I replace all the If, ElseIf by a switch case because it's more clear for this parts of code and easier to find the one shot and permanent test corresponding)
Code:
case STATE_PERI_OUT_REV: //in normal mowing reverse after the wire trigger
if (rollDir == RIGHT) {
UseAccelLeft = 1;
UseBrakeLeft = 1;
UseAccelRight = 1;
UseBrakeRight = 0;
}
else
{
UseAccelLeft = 1;
UseBrakeLeft = 0;
UseAccelRight = 1;
UseBrakeRight = 1;
}
motorLeftSpeedRpmSet = motorRightSpeedRpmSet = -motorSpeedMaxRpm;
stateEndOdometryRight = odometryRight - (odometryTicksPerCm * DistPeriOutRev);
stateEndOdometryLeft = odometryLeft - (odometryTicksPerCm * DistPeriOutRev);
OdoRampCompute();
break;
case STATE_PERI_OUT_ROLL: //roll left or right in normal mode
AngleRotate = random(motorRollDegMin, motorRollDegMax);
Tempovar = 36000 / AngleRotate; //need a value*100 for integer division later
if (dir == RIGHT) {
UseAccelLeft = 1;
UseBrakeLeft = 0;
UseAccelRight = 0;
UseBrakeRight = 1;
motorLeftSpeedRpmSet = motorSpeedMaxRpm ;
motorRightSpeedRpmSet = -motorSpeedMaxRpm;
stateEndOdometryRight = odometryRight - (int)100 * (odometryTicksPerCm * PI * odometryWheelBaseCm / Tempovar);
stateEndOdometryLeft = odometryLeft + (int)100 * (odometryTicksPerCm * PI * odometryWheelBaseCm / Tempovar);
} else {
UseAccelLeft = 0;
UseBrakeLeft = 1;
UseAccelRight = 1;
UseBrakeRight = 0;
motorLeftSpeedRpmSet = -motorSpeedMaxRpm ;
motorRightSpeedRpmSet = motorSpeedMaxRpm;
stateEndOdometryRight = odometryRight + (int)100 * (odometryTicksPerCm * PI * odometryWheelBaseCm / Tempovar);
stateEndOdometryLeft = odometryLeft - (int)100 * (odometryTicksPerCm * PI * odometryWheelBaseCm / Tempovar);
}
OdoRampCompute();
break;Into each permanent state there is now.
A test to know if the OdoCible is reach.
A test to wait until a motor completly stop if rotation is inverted.
And the next state.
Here sample of code
Code:
case STATE_PERI_OUT_STOP:
if ((odometryRight >= stateEndOdometryRight) && (odometryLeft >= stateEndOdometryLeft) )
if (motorLeftPWMCurr == 0 && motorRightPWMCurr == 0) { //wait until the left motor completly stop because rotation is inverted
setNextState(STATE_PERI_OUT_REV, rollDir);
}
motorControlOdo();
break;
case STATE_PERI_OUT_REV:
if ((odometryRight <= stateEndOdometryRight) && (odometryLeft <= stateEndOdometryLeft) )
if (rollDir == RIGHT) {
if (motorLeftPWMCurr == 0 ) { //wait until the left motor completly stop because rotation is inverted
setNextState(STATE_PERI_OUT_ROLL, RIGHT);
}
}
else
{
if (motorRightPWMCurr == 0 ) { //wait until the left motor completly stop because rotation is inverted
setNextState(STATE_PERI_OUT_ROLL, LEFT);
}
}
motorControlOdo();
break;
case STATE_PERI_OUT_ROLL:
if (mowPatternCurr == MOW_LANES_ODO) {
if (rollDir == RIGHT) {
if ((odometryRight <= stateEndOdometryRight) && (odometryLeft >= stateEndOdometryLeft)) setNextState(STATE_REACH_NEXT_LANE, 0);
}
else if (rollDir == LEFT) {
if ((odometryRight >= stateEndOdometryRight) && (odometryLeft <= stateEndOdometryLeft)) setNextState(STATE_REACH_NEXT_LANE, 0);
}
}
else ///MOW_RANDOM, MOW_LANES, MOW_BIDIR,
{
if (rollDir == RIGHT) {
if ((odometryRight <= stateEndOdometryRight) && (odometryLeft >= stateEndOdometryLeft) ) {
if (motorRightPWMCurr == 0 ) { //wait until the left motor completly stop because rotation is inverted
setNextState(STATE_PERI_OUT_FORW, RIGHT);
}
}
}
else {
if ((odometryRight >= stateEndOdometryRight) && (odometryLeft <= stateEndOdometryLeft) ) {
if (motorLeftPWMCurr == 0 ) { //wait until the left motor completly stop because rotation is inverted
setNextState(STATE_PERI_OUT_FORW, LEFT);
}
}
}
motorControlOdo();
}
checkPerimeterBoundary();
break;And also new OdoRampCompute function to compute the accel brake etc..
Code
Code:
void Robot::OdoRampCompute() {
//Compute the ramp to accel and brake the 2 wheels (very important for small distance)
stateStartOdometryLeft = odometryLeft;
stateStartOdometryRight = odometryRight;
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)
//compute the movingtime in millis()
if (abs(stateStartOdometryLeft - stateEndOdometryLeft) >= odometryTicksPerRevolution) OdoRampLeft = 0.8 * odometryTicksPerRevolution;
else OdoRampLeft = abs(0.33 * odometryTicksPerRevolution);
if (abs(stateStartOdometryRight - stateEndOdometryRight) >= odometryTicksPerRevolution) OdoRampRight = 0.8 * odometryTicksPerRevolution;
else OdoRampRight = abs(0.33 * odometryTicksPerRevolution);
movingTimeLeft = (abs(stateStartOdometryLeft - stateEndOdometryLeft) * odometryTicksPerRevolution * motorSpeedMaxRpm / 60000); //0.58= nb ticks/ms at max speed
movingTimeRight = (abs(stateStartOdometryRight - stateEndOdometryRight) * 0.58); //0.58= nb ticks/ms at max speed
if (movingTimeLeft >= motorOdoAccel) accelDurationLeft = motorOdoAccel;
else accelDurationLeft = movingTimeLeft / 2;
if (movingTimeRight >= motorOdoAccel) accelDurationRight = motorOdoAccel;
else accelDurationRight = movingTimeLeft / 2;
Console.print(" ************************************ compute at ");
Console.print(millis());
Console.print(" movingTimeLeft ");
Console.print(movingTimeLeft);
Console.print(" movingTimeRight ");
Console.print(movingTimeRight);
Console.print(" accelDurationLeft ");
Console.print(accelDurationLeft);
Console.print(" accelDurationRight ");
Console.print(accelDurationRight);
Console.print(" OdoRampRight ");
Console.print(OdoRampRight);
Console.print(" OdoRampLeft ");
Console.print(OdoRampLeft);
Console.println(stateEndOdometryRight);And the new motorControlOdo
Code:
void Robot::motorControlOdo() {
// call to reach a ODO cible accel and slow is used to smooth the movement of the mower
//Stop motor independently when the cible is reach
//
if (UseBrakeLeft && (motorLeftSpeedRpmSet >= 0) && (stateEndOdometryLeft - odometryLeft <= -20)) {//Forward left need -20because when stop the ticks can move
PwmLeftSpeed = 0;
motorLeftSpeedRpmSet = 0;
motorLeftRpmCurr = 0;
}
if (UseBrakeRight && (motorRightSpeedRpmSet >= 0) && (stateEndOdometryRight - odometryRight <= -20)) {//right
PwmRightSpeed = 0;
motorRightSpeedRpmSet = 0;
motorRightRpmCurr = 0;
}
//Reverse
if (UseBrakeRight && (motorRightSpeedRpmSet <= 0) && (stateEndOdometryRight - odometryRight >= 20)) {//right
PwmRightSpeed = 0;
motorRightSpeedRpmSet = 0;
motorRightRpmCurr = 0;
}
if (UseBrakeLeft && (motorLeftSpeedRpmSet <= 0) && (stateEndOdometryLeft - odometryLeft >= 20)) {//left
PwmLeftSpeed = 0;
motorLeftSpeedRpmSet = 0;
motorLeftRpmCurr = 0;
}
//if (millis() < (stateStartTime + 50))return; // Possible to set a pause between movement
if (millis() < nextTimeMotorOdoControl) return;
nextTimeMotorOdoControl = millis() + 15;
//LEFT WHEEL
leftSpeed = PwmLeftSpeed ; //Set first to Normal speed and stay like this if not change by accel or brake so limit the compute time
if (motorLeftSpeedRpmSet > 0) { //forward left wheel
if ((UseAccelLeft) && (millis() - stateStartTime < accelDurationLeft)) { //Accel mode for duration
//Sinus accel
//angleCorresp = map(millis() - stateStartTime, 0, accelDurationLeft, 0, 85);
//leftSpeed = PwmLeftSpeed * sin(radians(angleCorresp)); //convert degree to radians
//Linear accel
angleCorresp = map(millis() - stateStartTime, 0, accelDurationLeft, 0, 100); // map from 0 to 100%
leftSpeed = PwmLeftSpeed * angleCorresp / 100;
}
if (UseBrakeLeft && (odometryLeft > stateEndOdometryLeft - (OdoRampLeft))) { //Braking mode by odometry
angleCorresp = map(abs(stateEndOdometryLeft - odometryLeft), OdoRampLeft, 0, 90, 15);
leftSpeed = PwmLeftSpeed * sin(radians(angleCorresp));
}
}
if (motorLeftSpeedRpmSet < 0) { //reverse left wheel
if ((UseAccelLeft) && (millis() - stateStartTime < accelDurationLeft)) { //Accel mode for duration
//Sinus accel
// angleCorresp = map(millis() - stateStartTime, 0, accelDurationLeft, 0, 85);
// leftSpeed = PwmLeftSpeed * sin(radians(angleCorresp)); //convert degree to radians
//Linear accel
angleCorresp = map(millis() - stateStartTime, 0, accelDurationLeft, 0, 100);
leftSpeed = PwmLeftSpeed * angleCorresp / 100;
}
if (UseBrakeLeft && (odometryLeft < stateEndOdometryLeft + OdoRampLeft)) { //Braking mode by odometry
angleCorresp = map(abs(stateEndOdometryLeft - odometryLeft), OdoRampLeft, 0, 90, 15);
leftSpeed = PwmLeftSpeed * sin(radians(angleCorresp));
}
}
// RIGHT WHEEL
rightSpeed = PwmRightSpeed ; //Normal speed
if (motorRightSpeedRpmSet > 0) { //forward Right wheel
// Console.print(" FR rotate ");
if (UseAccelRight && (millis() - stateStartTime < accelDurationRight)) { //Accel mode for duration
//Sinus accel
// angleCorresp = map(millis() - stateStartTime, 0, accelDurationRight, 0, 85);
//rightSpeed = PwmRightSpeed * sin(radians(angleCorresp));
//Linear accel
angleCorresp = map(millis() - stateStartTime, 0, accelDurationRight, 0, 100);
rightSpeed = PwmRightSpeed * angleCorresp / 100;
}
if (UseBrakeRight && (odometryRight > stateEndOdometryRight - OdoRampRight)) { //Braking mode by odometry
angleCorresp = map(abs(stateEndOdometryRight - odometryRight), OdoRampRight, 0, 90, 15);
rightSpeed = PwmRightSpeed * sin(radians(angleCorresp));
}
}
if (motorRightSpeedRpmSet < 0) { //reverse Right wheel
if (UseAccelRight && (millis() - stateStartTime < accelDurationRight)) { //Accel mode for duration
//Sinus accel
//angleCorresp = map(millis() - stateStartTime, 0, accelDurationRight, 0, 85);
//rightSpeed = PwmRightSpeed * sin(radians(angleCorresp));
//Linear accel
angleCorresp = map(millis() - stateStartTime, 0, accelDurationRight, 0, 100);
rightSpeed = PwmRightSpeed * angleCorresp / 100;
}
if (UseBrakeRight && (odometryRight < stateEndOdometryRight + OdoRampRight)) { //Braking mode by odometry
angleCorresp = map(abs(stateEndOdometryRight - odometryRight), OdoRampRight, 0, 90, 15);
rightSpeed = PwmRightSpeed * sin(radians(angleCorresp));
}
}
Console.print(millis());
Console.print(" *** Lspeed= ");
Console.print(leftSpeed);
Console.print(" ODO ");
Console.print(stateStartOdometryLeft);
Console.print("/");
Console.print(odometryLeft);
Console.print("/");
Console.print(stateEndOdometryLeft);
Console.print(" ************************************ Rspeed= ");
Console.print(rightSpeed);
Console.print(" ODO ");
Console.print(stateStartOdometryRight);
Console.print("/");
Console.print(odometryRight);
Console.print("/");
Console.println(stateEndOdometryRight);
setMotorPWM(leftSpeed, rightSpeed, false );
}For the moment it's work but need more test to be sure there is no problem on slope and motordriver because the mowing is very fast.
The main problem is for small movement (less the 1/4 of Wheel turn) , If PWM too low the motor don't turn and the Odocible never reach.
Maybe i add a Maxtime into each state and jump to the other if trigger ?
Video .
https://youtu.be/5usduXYCLEg
