Move to a Specified Distance, Revisited

Posted 24 December, 2022

As part of the suite of tools associated with wall tracking and IR beam homing, I created a set of ‘move to specified distance’ routines using my home-grown PID algorithm as follows:

MoveToDesiredFrontDistCm (MTFD)
MoveToDesiredRearDistCm(MTRD)
MoveToDesiredLeftDistCm(MTLD)
MoveToDesiredRightDistCm(MTRD)

I saw some odd problems in my past wall-tracking exercises, so I thought it would be a good idea to go back and test these in isolation to work out any bugs. As usual, I constructed a limited part-task program for this purpose, and ran a number of tests on my desktop ‘range’. I started with the ‘MoveToDesiredFrontDistCm()’ function, as shown below:

MoveToDesiredFrontDistCm ():

bool MoveToDesiredFrontDistCm(uint16_t offsetCm, float Kp, float Ki, float Kd)
{
  //Purpose: Move forward or backward to a specified front offset distance
  // Inputs:
  //	offsetCm - int denoting desired forward distance offset
  //	Kp,Ki,Kd set at top of program (lines 85-87)
  // Outputs:
  //	Robot moves forward or backward to the desired offset, under PID control
  //Notes:
  //	04/25/21 the Kp/Ki/Kd values are much lower than prescribed by Z-N theory
  //		but I like the slower, more deliberate action.
  //  03/06/22 rev for Teensy 3.5 and PIDCalcs().
  //  05/07/22 bugfix: using the same PID values for 'fwd' and 'bkwrd' positioning means 
  //    using a minus sign here for motor speed.

  float lastError = 0;
  float lastInput = 0;
  float lastIval = 0;
  float lastDerror = 0;

  //DEBUG!!
  gl_pSerPort->printf("MoveToDesiredFrontDistCm(%d): Kp/Ki/Kd= %2.2f/%2.2f/%2.2f\n",
    offsetCm, Kp, Ki, Kd);
  //DEBUG!!

  StopBothMotors();

  //05/30/22 re-init front/back dist arrays to avoid spurious 'stuck' detection
  InitFrontDistArray();
  InitRearDistArray();

  gl_pSerPort->printf("MTFD: at start, tgt = %dcm, curr_dist = %d, front/rear var = %2.1f/%2.1f\n",
    offsetCm, GetFrontDistCm(), glFrontvar, glRearvar);

  gl_pSerPort->printf("Msec\tFdist\tTgtD\terr\tIval\tDerr\tOut\tSpeed\tFVar\tRVar\n");

  UpdateAllEnvironmentParameters();
  //AnomalyCode errcode = NO_ANOMALIES;
  AnomalyCode errcode = CheckForAnomalies();

  MsecSinceLastDistUpdate = 0; //01/08/22 not using ISR anymore
  MsecSinceLastFrontDistUpdate = 0; //added 10/02/22 to slow front LIDAR update rate

  //06/12/22 bugfix: can't have GetFrontDistCm() in while() statement - cuz it gets executed too often
  //OffsetDistVal = (double)GetFrontDistCm();
  //while (abs(GetFrontDistCm() - offsetCm) > 1 && errcode == NO_ANOMALIES)

  while (abs(glFrontCm - offsetCm) > 1 && errcode != STUCK_AHEAD && errcode != STUCK_BEHIND)
  {
    CheckForUserInput();

    if (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE)
    {
      MsecSinceLastDistUpdate -= MSEC_PER_DIST_UPDATE;

      UpdateAllEnvironmentParameters();
      errcode = CheckForAnomalies();

      OffsetDistVal = (double)GetFrontDistCm();

      //gl_pSerPort->printf("MTFD: in loop, tgt = %dcm, curr_dist = %2.1f\n", offsetCm, OffsetDistVal);

      //OffsetDistOutput = PIDCalcs(OffsetDistVal, (float)offsetCm, lastError,
      //  lastInput, lastIval, lastDerror, OffsetDistKp, OffsetDistKi, OffsetDistKd);
      OffsetDistOutput = PIDCalcs((double)glFrontCm, (float)offsetCm, lastError,
        lastInput, lastIval, lastDerror, Kp, Ki, Kd);

      int16_t speed = (int16_t)OffsetDistOutput;
      //gl_pSerPort->printf("MTFD: in loop, speed = %dcm\n", speed);

      //PIDCalcs out can be negative, must handle both signs
      if (speed >= 0)
      {
        speed = (speed > MOTOR_SPEED_QTR) ? MOTOR_SPEED_QTR : speed;
        speed = (speed <= MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : speed;
      }
      else //speed < 0
      {
        speed = (speed < -MOTOR_SPEED_QTR) ? -MOTOR_SPEED_QTR : speed;
      }

      gl_pSerPort->printf("%lu\t%d\t%d\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%d\t%2.1f\t%2.1f\n",
        millis(), glFrontCm, offsetCm, lastError, lastIval, lastDerror, OffsetDistOutput, speed, glFrontvar, glRearvar);

      //  05/07/22 bugfix: using the same PID values for 'fwd' and 'bkwrd' requires the 
      //    use of a minus sign here for motor speed.
      RunBothMotorsBidirectional(-speed, -speed);
    }
  }

  //05/30/22 only worry about 'stuck' conditions
  //if (errcode == STUCK_AHEAD || errcode == STUCK_BEHIND)
  //{
  //  HandleAnomalousConditions(errcode, TRACKING_NONE);
  //  return false;
  //}

  gl_pSerPort->printf("MTFD: Stopped with front dist = %d and errcode = %s\n", GetFrontDistCm(), AnomalyStrArray[errcode]);
  StopBothMotors();
  return true;
}

100

101

bool MoveToDesiredFrontDistCm(uint16_t offsetCm, float Kp, float Ki, float Kd)

{

//Purpose: Move forward or backward to a specified front offset distance

// Inputs:

// offsetCm - int denoting desired forward distance offset

// Kp,Ki,Kd set at top of program (lines 85-87)

// Outputs:

// Robot moves forward or backward to the desired offset, under PID control

//Notes:

// 04/25/21 the Kp/Ki/Kd values are much lower than prescribed by Z-N theory

// but I like the slower, more deliberate action.

// 03/06/22 rev for Teensy 3.5 and PIDCalcs().

// 05/07/22 bugfix: using the same PID values for 'fwd' and 'bkwrd' positioning means

// using a minus sign here for motor speed.

float lastError = 0;

float lastInput = 0;

float lastIval = 0;

float lastDerror = 0;

//DEBUG!!

gl_pSerPort->printf("MoveToDesiredFrontDistCm(%d): Kp/Ki/Kd= %2.2f/%2.2f/%2.2f\n",

offsetCm, Kp, Ki, Kd);

//DEBUG!!

StopBothMotors();

//05/30/22 re-init front/back dist arrays to avoid spurious 'stuck' detection

InitFrontDistArray();

InitRearDistArray();

gl_pSerPort->printf("MTFD: at start, tgt = %dcm, curr_dist = %d, front/rear var = %2.1f/%2.1f\n",

offsetCm, GetFrontDistCm(), glFrontvar, glRearvar);

gl_pSerPort->printf("Msec\tFdist\tTgtD\terr\tIval\tDerr\tOut\tSpeed\tFVar\tRVar\n");

UpdateAllEnvironmentParameters();

//AnomalyCode errcode = NO_ANOMALIES;

AnomalyCode errcode = CheckForAnomalies();

MsecSinceLastDistUpdate = 0; //01/08/22 not using ISR anymore

MsecSinceLastFrontDistUpdate = 0; //added 10/02/22 to slow front LIDAR update rate

//06/12/22 bugfix: can't have GetFrontDistCm() in while() statement - cuz it gets executed too often

//OffsetDistVal = (double)GetFrontDistCm();

//while (abs(GetFrontDistCm() - offsetCm) > 1 && errcode == NO_ANOMALIES)

while (abs(glFrontCm - offsetCm) > 1 && errcode != STUCK_AHEAD && errcode != STUCK_BEHIND)

{

CheckForUserInput();

if (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE)

{

MsecSinceLastDistUpdate -= MSEC_PER_DIST_UPDATE;

UpdateAllEnvironmentParameters();

errcode = CheckForAnomalies();

OffsetDistVal = (double)GetFrontDistCm();

//gl_pSerPort->printf("MTFD: in loop, tgt = %dcm, curr_dist = %2.1f\n", offsetCm, OffsetDistVal);

//OffsetDistOutput = PIDCalcs(OffsetDistVal, (float)offsetCm, lastError,

// lastInput, lastIval, lastDerror, OffsetDistKp, OffsetDistKi, OffsetDistKd);

OffsetDistOutput = PIDCalcs((double)glFrontCm, (float)offsetCm, lastError,

lastInput, lastIval, lastDerror, Kp, Ki, Kd);

int16_t speed = (int16_t)OffsetDistOutput;

//gl_pSerPort->printf("MTFD: in loop, speed = %dcm\n", speed);

//PIDCalcs out can be negative, must handle both signs

if (speed >= 0)

{

speed = (speed > MOTOR_SPEED_QTR) ? MOTOR_SPEED_QTR : speed;

speed = (speed <= MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : speed;

}

else //speed < 0

{

speed = (speed < -MOTOR_SPEED_QTR) ? -MOTOR_SPEED_QTR : speed;

}

gl_pSerPort->printf("%lu\t%d\t%d\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%d\t%2.1f\t%2.1f\n",

millis(), glFrontCm, offsetCm, lastError, lastIval, lastDerror, OffsetDistOutput, speed, glFrontvar, glRearvar);

// 05/07/22 bugfix: using the same PID values for 'fwd' and 'bkwrd' requires the

// use of a minus sign here for motor speed.

RunBothMotorsBidirectional(-speed, -speed);

}

//05/30/22 only worry about 'stuck' conditions

//if (errcode == STUCK_AHEAD || errcode == STUCK_BEHIND)

//{

// HandleAnomalousConditions(errcode, TRACKING_NONE);

// return false;

//}

gl_pSerPort->printf("MTFD: Stopped with front dist = %d and errcode = %s\n", GetFrontDistCm(), AnomalyStrArray[errcode]);

StopBothMotors();

return true;

}

Here’s some output from a typical run:

Starting Front/BackSide Motion Test - enter Direction (1 = F, 2 = B, 3 = L, 4 = R), offset, Kp,Ki,Kd
At top of parameter retrieval section
DirStr = 1
OffStr = 60
Moving Front to 60 cm, with PID = (1.50,0.10,0.20)
Enter any key to start

Before MTDFD, FVar = 13062.84
MoveToDesiredFrontDistCm(60): Kp/Ki/Kd= 1.50/0.10/0.20
MTFD: at start, tgt = 60cm, curr_dist = 21, front/rear var = 13328.0/833.0
Msec	Fdist	TgtD	err	Ival	Derr	Out	Speed	FVar	RVar
163417	20	60	40.0	4.0	40.0	56.0	56	13305.6	21444.6
163480	20	60	40.0	8.0	0.0	68.0	68	13974.0	30074.4
163530	20	60	40.0	12.0	0.0	72.0	72	14987.5	40126.6
163581	21	60	39.0	15.9	-1.0	74.6	74	16226.3	49659.8
163628	24	60	36.0	19.5	-3.0	74.1	74	18030.5	58677.5
163680	26	60	34.0	22.9	-2.0	74.3	74	19318.8	67183.6
163729	28	60	32.0	26.1	-2.0	74.5	74	20393.4	73500.8
163779	29	60	31.0	29.2	-1.0	75.9	75	21153.6	79398.7
163830	30	60	30.0	32.2	-1.0	77.4	75	21426.0	84880.5
163878	32	60	28.0	35.0	-2.0	77.4	75	20739.2	89949.6
163929	33	60	27.0	37.7	-1.0	78.4	75	19204.9	94609.4
163980	34	60	26.0	40.3	-1.0	79.5	75	16580.3	98863.2
164032	37	60	23.0	42.6	-3.0	77.7	75	12651.8	104153.8
164081	39	60	21.0	44.7	-2.0	76.6	75	7213.3	107565.6
164130	44	60	16.0	46.3	-5.0	71.3	71	53.6	110581.6
164181	49	60	11.0	47.4	-5.0	64.9	64	67.6	114521.3
164220	53	60	7.0	48.1	-4.0	59.4	59	86.6	117988.8
164281	53	60	7.0	48.8	0.0	59.3	59	99.7	120987.7
164333	57	60	3.0	49.1	-4.0	54.4	54	113.1	123521.6
164382	59	60	1.0	49.2	-2.0	51.1	51	124.7	124450.2
MTFD: Stopped with front dist = 59 and errcode = NO_ANOMALIES
Front	Rear	Left	Right
59	819.10	32.00	779.80
63	819.10	33.00	779.80
65	819.10	34.00	779.80
66	819.10	33.00	779.80
69	750.40	32.00	779.80
67	750.40	32.00	779.80
68	750.40	33.00	779.70
67	819.10	32.00	779.70
68	819.10	32.00	779.80
67	819.10	33.00	779.80

Starting Front/BackSide Motion Test - enter Direction (1 = F, 2 = B, 3 = L, 4 = R), offset, Kp,Ki,Kd

At top of parameter retrieval section

DirStr = 1

OffStr = 60

Moving Front to 60 cm, with PID = (1.50,0.10,0.20)

Enter any key to start

Before MTDFD, FVar = 13062.84

MoveToDesiredFrontDistCm(60): Kp/Ki/Kd= 1.50/0.10/0.20

MTFD: at start, tgt = 60cm, curr_dist = 21, front/rear var = 13328.0/833.0

Msec Fdist TgtD err Ival Derr Out Speed FVar RVar

163417 20 60 40.0 4.0 40.0 56.0 56 13305.6 21444.6

163480 20 60 40.0 8.0 0.0 68.0 68 13974.0 30074.4

163530 20 60 40.0 12.0 0.0 72.0 72 14987.5 40126.6

163581 21 60 39.0 15.9 -1.0 74.6 74 16226.3 49659.8

163628 24 60 36.0 19.5 -3.0 74.1 74 18030.5 58677.5

163680 26 60 34.0 22.9 -2.0 74.3 74 19318.8 67183.6

163729 28 60 32.0 26.1 -2.0 74.5 74 20393.4 73500.8

163779 29 60 31.0 29.2 -1.0 75.9 75 21153.6 79398.7

163830 30 60 30.0 32.2 -1.0 77.4 75 21426.0 84880.5

163878 32 60 28.0 35.0 -2.0 77.4 75 20739.2 89949.6

163929 33 60 27.0 37.7 -1.0 78.4 75 19204.9 94609.4

163980 34 60 26.0 40.3 -1.0 79.5 75 16580.3 98863.2

164032 37 60 23.0 42.6 -3.0 77.7 75 12651.8 104153.8

164081 39 60 21.0 44.7 -2.0 76.6 75 7213.3 107565.6

164130 44 60 16.0 46.3 -5.0 71.3 71 53.6 110581.6

164181 49 60 11.0 47.4 -5.0 64.9 64 67.6 114521.3

164220 53 60 7.0 48.1 -4.0 59.4 59 86.6 117988.8

164281 53 60 7.0 48.8 0.0 59.3 59 99.7 120987.7

164333 57 60 3.0 49.1 -4.0 54.4 54 113.1 123521.6

164382 59 60 1.0 49.2 -2.0 51.1 51 124.7 124450.2

MTFD: Stopped with front dist = 59 and errcode = NO_ANOMALIES

Front Rear Left Right

59 819.10 32.00 779.80

63 819.10 33.00 779.80

65 819.10 34.00 779.80

66 819.10 33.00 779.80

69 750.40 32.00 779.80

67 750.40 32.00 779.80

68 750.40 33.00 779.70

67 819.10 32.00 779.70

68 819.10 32.00 779.80

67 819.10 33.00 779.80

The movement goes OK, and the robot telemetry says it stopped very close to the desired 60cm distance. However, when I measured the actual distance from the ‘wall’ to the robot, I got more like 67 or 68cm, probably indicating that the robot coasted some after the motors were turned off. When I instrumented the code to show the next 10 distance measurements after the exit from the subroutine, it became easy to see that this was the case – the robot coasted from 59 to 67cm. The robot should correct this by going back the opposite way, but it doesn’t because the last measurement (59cm) fits inside the 59-61cm ‘basket’ for subroutine termination (the ‘while’ loop termination criteria).

So, I thought what I could do is check the reported front distance after function exit, and just call it again if the robot had coasted too far from the target. The second run should get much closer, as the starting error term would be much smaller. So now the test code looks like this:

        gl_pSerPort->printf("Before MTDFD, FVar = %2.2f\n", glFrontvar);

        MoveToDesiredFrontDistCm(Offset,Kp,Ki,Kd);

        //show 10 distances after stop
        gl_pSerPort->printf("Front\tRear\tLeft\tRight\n");
        for (size_t i = 0; i < 10; i++)
        {
          GetRequestedVL53l0xValues(VL53L0X_ALL);
          glFrontCm = GetFrontDistCm();
          gl_pSerPort->printf("%d\t%2.2f\t%2.2f\t%2.2f\n",
            glFrontCm, glRearCm, glLeftCenterCm, glRightCenterCm);
          delay(100);
        }

        if (abs(glFrontCm - Offset) > 1)
        {
          gl_pSerPort->printf("Second time through MTDFD with error = %d\n", abs(glFrontCm - Offset));
          MoveToDesiredFrontDistCm(Offset,Kp/2,Ki,Kd);
        }

gl_pSerPort->printf("Before MTDFD, FVar = %2.2f\n", glFrontvar);

MoveToDesiredFrontDistCm(Offset,Kp,Ki,Kd);

//show 10 distances after stop

gl_pSerPort->printf("Front\tRear\tLeft\tRight\n");

for (size_t i = 0; i < 10; i++)

{

GetRequestedVL53l0xValues(VL53L0X_ALL);

glFrontCm = GetFrontDistCm();

gl_pSerPort->printf("%d\t%2.2f\t%2.2f\t%2.2f\n",

glFrontCm, glRearCm, glLeftCenterCm, glRightCenterCm);

delay(100);

}

if (abs(glFrontCm - Offset) > 1)

{

gl_pSerPort->printf("Second time through MTDFD with error = %d\n", abs(glFrontCm - Offset));

MoveToDesiredFrontDistCm(Offset,Kp/2,Ki,Kd);

}

This should have worked well, except when I tried it, the function exited with a ‘STUCK_AHEAD’ error – oops! Here’s a run going from 60 from 30cm:

Moving Front to 30 cm, with PID = (1.50,0.10,0.20)
Enter any key to start
Got 51
Got 46
Got 46
Got 46
Got 255
Before MTDFD, FVar = 12228.16
MoveToDesiredFrontDistCm(30): Kp/Ki/Kd= 1.50/0.10/0.20
MTFD: at start, tgt = 30cm, curr_dist = 65, front/rear var = 13328.0/833.0
Msec	Fdist	TgtD	err	Ival	Derr	Out	Speed	FVar	RVar
4812697	64	30	-34.0	-3.4	-34.0	-47.6	-47	11618.7	23386.2
4812746	66	30	-36.0	-7.0	-2.0	-60.6	-60	11452.8	23271.3
4812800	65	30	-35.0	-10.5	1.0	-63.2	-63	11682.5	33698.9
4812849	64	30	-34.0	-13.9	1.0	-65.1	-65	12236.1	41759.9
4812899	63	30	-33.0	-17.2	1.0	-66.9	-66	13316.3	49390.2
4812944	60	30	-30.0	-20.2	3.0	-65.8	-65	14265.6	56593.1
4812981	61	30	-31.0	-23.3	-1.0	-69.6	-69	15163.5	63372.0
4813049	60	30	-30.0	-26.3	1.0	-71.5	-71	15891.4	69730.2
4813095	57	30	-27.0	-29.0	3.0	-70.1	-70	16326.1	77314.4
4813148	55	30	-25.0	-31.5	2.0	-69.4	-69	16138.6	84400.2
4813199	55	30	-25.0	-34.0	0.0	-71.5	-71	15176.2	90991.4
4813235	52	30	-22.0	-36.2	3.0	-69.8	-69	13311.0	97091.6
4813297	50	30	-20.0	-38.2	2.0	-68.6	-68	10317.8	101237.0
4813346	49	30	-19.0	-40.1	1.0	-68.8	-68	5972.6	104982.1
4813396	44	30	-14.0	-41.5	5.0	-63.5	-63	40.8	108330.4
MTFD: Stopped with front dist = 44 and errcode = STUCK_AHEAD

Moving Front to 30 cm, with PID = (1.50,0.10,0.20)

Enter any key to start

Got 51

Got 46

Got 255

Before MTDFD, FVar = 12228.16

MoveToDesiredFrontDistCm(30): Kp/Ki/Kd= 1.50/0.10/0.20

MTFD: at start, tgt = 30cm, curr_dist = 65, front/rear var = 13328.0/833.0

Msec Fdist TgtD err Ival Derr Out Speed FVar RVar

4812697 64 30 -34.0 -3.4 -34.0 -47.6 -47 11618.7 23386.2

4812746 66 30 -36.0 -7.0 -2.0 -60.6 -60 11452.8 23271.3

4812800 65 30 -35.0 -10.5 1.0 -63.2 -63 11682.5 33698.9

4812849 64 30 -34.0 -13.9 1.0 -65.1 -65 12236.1 41759.9

4812899 63 30 -33.0 -17.2 1.0 -66.9 -66 13316.3 49390.2

4812944 60 30 -30.0 -20.2 3.0 -65.8 -65 14265.6 56593.1

4812981 61 30 -31.0 -23.3 -1.0 -69.6 -69 15163.5 63372.0

4813049 60 30 -30.0 -26.3 1.0 -71.5 -71 15891.4 69730.2

4813095 57 30 -27.0 -29.0 3.0 -70.1 -70 16326.1 77314.4

4813148 55 30 -25.0 -31.5 2.0 -69.4 -69 16138.6 84400.2

4813199 55 30 -25.0 -34.0 0.0 -71.5 -71 15176.2 90991.4

4813235 52 30 -22.0 -36.2 3.0 -69.8 -69 13311.0 97091.6

4813297 50 30 -20.0 -38.2 2.0 -68.6 -68 10317.8 101237.0

4813346 49 30 -19.0 -40.1 1.0 -68.8 -68 5972.6 104982.1

4813396 44 30 -14.0 -41.5 5.0 -63.5 -63 40.8 108330.4

MTFD: Stopped with front dist = 44 and errcode = STUCK_AHEAD

The ‘stuck’ checks have to be there because the robot can’t ‘see’ obstacles that are too low to interrupt the front LIDAR beam, or aren’t directly in the line of sight, but how to manage? The ‘stuck’ checks depend on a variance calculation on the last 50 measurements (held in a bit-bucket array), so I began to wonder why the front variance was decreasing so rapidly while the rear variance wasn’t (one would think they would behave more or less the same). So I went back and printed out the front & back distance array contents after each run, and saw that the reason the front variance was decreasing so rapidly was because I was using a 50mSec time interval, meaning the 50-element array was getting filled in 2500mSec – or about 2.5sec. So, in order to get more variance in a normal run, either the run has to be done at a faster speed (leading to more overshoot) or the timing interval has to be increased. In earlier work I had discovered that the front LIDAR system produces errors for long distance measurements when using short measurement intervals, so I increased the measurement interval from 50 to 200mSec, and now the variance decreases at a much slower rate – yay!

With the longer 200mSec measurement interval, the movement routine now has time to adjust for overshoots, as shown below:

Moving Front to 60 cm, with PID = (1.50,0.10,0.20)
Enter any key to start
Before MTDFD, FVar = 12843.11
MoveToDesiredFrontDistCm(60): Kp/Ki/Kd= 1.50/0.10/0.20
MTFD: at start, tgt = 60cm, curr_dist = 31, front/rear var = 13328.0/833.0
Msec	Fdist	TgtD	err	Ival	Derr	Out	Speed	FVar	RVar
872999	29	60	31.0	3.1	31.0	43.4	43	12907.2	21444.6
873198	32	60	28.0	5.9	-3.0	48.5	48	13605.9	30074.4
873398	36	60	24.0	8.3	-4.0	45.1	45	14795.4	40126.6
873599	44	60	16.0	9.9	-8.0	35.5	35	16147.8	47845.2
873798	52	60	8.0	10.7	-8.0	24.3	24	17428.3	56907.6
873984	56	60	4.0	11.1	-4.0	17.9	17	18228.5	63729.1
874197	56	60	4.0	11.5	0.0	17.5	17	18902.0	71816.1
874401	57	60	3.0	11.8	-1.0	16.5	16	18813.2	77754.5
874602	59	60	1.0	11.9	-2.0	13.8	13	17558.3	84880.5
MTFD: Stopped with front dist = 59 and errcode = NO_ANOMALIES
Front	Rear	Left	Right
59	750.40	32.00	779.80
59	91.50	33.00	779.70
59	750.40	33.00	779.80
60	819.10	32.00	779.80
59	819.10	32.00	779.80
60	819.10	33.00	779.80
59	750.40	32.00	779.80
58	750.40	31.00	779.80
57	750.40	32.00	779.80
59	750.40	32.00	779.80

Moving Front to 60 cm, with PID = (1.50,0.10,0.20)

Enter any key to start

Before MTDFD, FVar = 12843.11

MoveToDesiredFrontDistCm(60): Kp/Ki/Kd= 1.50/0.10/0.20

MTFD: at start, tgt = 60cm, curr_dist = 31, front/rear var = 13328.0/833.0

Msec Fdist TgtD err Ival Derr Out Speed FVar RVar

872999 29 60 31.0 3.1 31.0 43.4 43 12907.2 21444.6

873198 32 60 28.0 5.9 -3.0 48.5 48 13605.9 30074.4

873398 36 60 24.0 8.3 -4.0 45.1 45 14795.4 40126.6

873599 44 60 16.0 9.9 -8.0 35.5 35 16147.8 47845.2

873798 52 60 8.0 10.7 -8.0 24.3 24 17428.3 56907.6

873984 56 60 4.0 11.1 -4.0 17.9 17 18228.5 63729.1

874197 56 60 4.0 11.5 0.0 17.5 17 18902.0 71816.1

874401 57 60 3.0 11.8 -1.0 16.5 16 18813.2 77754.5

874602 59 60 1.0 11.9 -2.0 13.8 13 17558.3 84880.5

MTFD: Stopped with front dist = 59 and errcode = NO_ANOMALIES

Front Rear Left Right

59 750.40 32.00 779.80

59 91.50 33.00 779.70

59 750.40 33.00 779.80

60 819.10 32.00 779.80

59 819.10 32.00 779.80

60 819.10 33.00 779.80

59 750.40 32.00 779.80

58 750.40 31.00 779.80

57 750.40 32.00 779.80

59 750.40 32.00 779.80

As the above run shows, the robot stopped very close (59cm) to the desired 60cm, without any significant overshoot, and the front variance actually increased significantly during the run – nice!

The following run was in the other direction – from 60 to 30cm:

Moving Front to 30 cm, with PID = (1.50,0.10,0.20)
Enter any key to start
Before MTDFD, FVar = 17702.84
MoveToDesiredFrontDistCm(30): Kp/Ki/Kd= 1.50/0.10/0.20
MTFD: at start, tgt = 30cm, curr_dist = 61, front/rear var = 13328.0/833.0
Msec	Fdist	TgtD	err	Ival	Derr	Out	Speed	FVar	RVar
1164375	61	30	-31.0	-3.1	-31.0	-43.4	-43	11740.1	19499.2
1164572	59	30	-29.0	-6.0	2.0	-49.9	-49	11739.5	30074.4
1164772	54	30	-24.0	-8.4	5.0	-45.4	-45	12473.4	38267.3
1164975	51	30	-21.0	-10.5	3.0	-42.6	-42	13723.7	47845.2
1165173	44	30	-14.0	-11.9	7.0	-34.3	-34	15250.3	55133.9
1165363	37	30	-7.0	-12.6	7.0	-24.5	-24	16422.6	63729.1
1165575	32	30	-2.0	-12.8	5.0	-16.8	-16	17689.6	70127.6
1165774	30	30	0.0	-12.8	2.0	-13.2	-13	18190.7	76106.5
MTFD: Stopped with front dist = 30 and errcode = WALL_OFFSET_DIST_AHEAD
Front	Rear	Left	Right
30	750.40	29.00	779.80
30	750.40	29.00	779.80
30	750.40	29.00	779.80
31	819.10	29.00	779.80
32	819.10	28.00	779.80
32	819.10	29.00	779.80
31	819.10	29.00	779.80
28	819.10	29.00	779.80
32	819.10	28.00	779.80
28	750.40	28.00	779.80
Second time through MTDFD with error = 2
MoveToDesiredFrontDistCm(30): Kp/Ki/Kd= 1.50/0.10/0.20
MTFD: at start, tgt = 30cm, curr_dist = 30, front/rear var = 13328.0/833.0
Msec	Fdist	TgtD	err	Ival	Derr	Out	Speed	FVar	RVar
MTFD: Stopped with front dist = 29 and errcode = OBSTACLE_AHEAD

Moving Front to 30 cm, with PID = (1.50,0.10,0.20)

Enter any key to start

Before MTDFD, FVar = 17702.84

MoveToDesiredFrontDistCm(30): Kp/Ki/Kd= 1.50/0.10/0.20

MTFD: at start, tgt = 30cm, curr_dist = 61, front/rear var = 13328.0/833.0

Msec Fdist TgtD err Ival Derr Out Speed FVar RVar

1164375 61 30 -31.0 -3.1 -31.0 -43.4 -43 11740.1 19499.2

1164572 59 30 -29.0 -6.0 2.0 -49.9 -49 11739.5 30074.4

1164772 54 30 -24.0 -8.4 5.0 -45.4 -45 12473.4 38267.3

1164975 51 30 -21.0 -10.5 3.0 -42.6 -42 13723.7 47845.2

1165173 44 30 -14.0 -11.9 7.0 -34.3 -34 15250.3 55133.9

1165363 37 30 -7.0 -12.6 7.0 -24.5 -24 16422.6 63729.1

1165575 32 30 -2.0 -12.8 5.0 -16.8 -16 17689.6 70127.6

1165774 30 30 0.0 -12.8 2.0 -13.2 -13 18190.7 76106.5

MTFD: Stopped with front dist = 30 and errcode = WALL_OFFSET_DIST_AHEAD

Front Rear Left Right

30 750.40 29.00 779.80

31 819.10 29.00 779.80

32 819.10 28.00 779.80

32 819.10 29.00 779.80

31 819.10 29.00 779.80

28 819.10 29.00 779.80

32 819.10 28.00 779.80

28 750.40 28.00 779.80

Second time through MTDFD with error = 2

MoveToDesiredFrontDistCm(30): Kp/Ki/Kd= 1.50/0.10/0.20

MTFD: at start, tgt = 30cm, curr_dist = 30, front/rear var = 13328.0/833.0

Msec Fdist TgtD err Ival Derr Out Speed FVar RVar

MTFD: Stopped with front dist = 29 and errcode = OBSTACLE_AHEAD

The robot did a very good job of stopping right on the mark, but coasted 2cm further over the next 2sec. This caused the test program to run the movement routine a second time, but it almost immediately exited again. The front/rear variance numbers were quite high, well out of the danger zone (the error codes shown did not effect the routine – it only looks for ‘STUCK_AHEAD’ and ‘STUCK_BEHIND’ which trigger on front and rear variance thresholds respectively.

So, for at least the ‘MoveToDesiredFrontDistCm()’ function, it appears that PID = (1.5, 0.1, 0.2) works very well. On to the next one!

MoveToDesiredRearDistCm():

Using the same PID set as for MoveToDesiredFrontDistCm() produces excellent results for the ‘rear motion’ routine as well. Here’s a run going from 60 to 30cm based on the rear distance sensor:

Starting Front/BackSide Motion Test - enter Direction (1 = F, 2 = B, 3 = L, 4 = R), offset, Kp,Ki,Kd
At top of parameter retrieval section
DirStr = 2
OffStr = 30
Moving Back to 30 cm, with PID = (1.50,0.10,0.20)
Enter any key to start
In MoveToDesiredRearDistCm(30) with Kp/Ki/Kd = 1.5/0.1/0.2
	MTRD: at start, tgt = 30cm, curr_dist = 57, front/rear var = 13328.0/833.0
Msec	Fdist	TgtD	err	Ival	Derr	Out	Speed	FVar	RVar
27488	64.10	30	-34.1	-3.4	-34.1	-47.7	-47	10490.5	743.4
27685	57.20	30	-27.2	-6.1	6.9	-48.3	-48	9380.3	697.8
27889	58.80	30	-28.8	-9.0	-1.6	-51.9	-51	8803.9	653.9
28087	47.90	30	-17.9	-10.8	10.9	-39.8	-39	8602.4	612.5
28288	47.00	30	-17.0	-12.5	0.9	-38.2	-38	8804.5	573.6
28489	37.50	30	-7.5	-13.2	9.5	-26.4	-26	8982.6	542.8
28689	37.80	30	-7.8	-14.0	-0.3	-25.7	-25	9235.9	514.8
28889	30.60	30	-0.6	-14.1	7.2	-16.4	-16	9657.8	496.2
	MTRD: Stopped with rear dist = 34 and errcode = NO_ANOMALIES
Front	Rear	Left	Right
120	29.10	824.00	27.30
119	28.30	824.00	27.10
118	28.80	824.00	27.40
117	29.00	824.00	26.90
121	32.60	824.00	27.30
118	32.50	824.00	27.30
120	32.10	824.00	27.90
119	32.00	824.00	27.30
121	28.80	824.00	27.30
119	28.80	824.00	27.20

Starting Front/BackSide Motion Test - enter Direction (1 = F, 2 = B, 3 = L, 4 = R), offset, Kp,Ki,Kd

At top of parameter retrieval section

DirStr = 2

OffStr = 30

Moving Back to 30 cm, with PID = (1.50,0.10,0.20)

Enter any key to start

In MoveToDesiredRearDistCm(30) with Kp/Ki/Kd = 1.5/0.1/0.2

MTRD: at start, tgt = 30cm, curr_dist = 57, front/rear var = 13328.0/833.0

Msec Fdist TgtD err Ival Derr Out Speed FVar RVar

27488 64.10 30 -34.1 -3.4 -34.1 -47.7 -47 10490.5 743.4

27685 57.20 30 -27.2 -6.1 6.9 -48.3 -48 9380.3 697.8

27889 58.80 30 -28.8 -9.0 -1.6 -51.9 -51 8803.9 653.9

28087 47.90 30 -17.9 -10.8 10.9 -39.8 -39 8602.4 612.5

28288 47.00 30 -17.0 -12.5 0.9 -38.2 -38 8804.5 573.6

28489 37.50 30 -7.5 -13.2 9.5 -26.4 -26 8982.6 542.8

28689 37.80 30 -7.8 -14.0 -0.3 -25.7 -25 9235.9 514.8

28889 30.60 30 -0.6 -14.1 7.2 -16.4 -16 9657.8 496.2

MTRD: Stopped with rear dist = 34 and errcode = NO_ANOMALIES

Front Rear Left Right

120 29.10 824.00 27.30

119 28.30 824.00 27.10

118 28.80 824.00 27.40

117 29.00 824.00 26.90

121 32.60 824.00 27.30

118 32.50 824.00 27.30

120 32.10 824.00 27.90

119 32.00 824.00 27.30

121 28.80 824.00 27.30

119 28.80 824.00 27.20

MoveToDesiredLeftDistCm():

Next, I tackled the ‘MoveToDesiredLeftDistCm()’ function, which uses the left center distance measurement (corrected for orientation angle). This went fairly quickly, as the PID values for the front/back case seemed to work well for the ‘side’ case too. Here’s the output and a short video from a typical run:

In MoveToDesiredLeftDistCm(50) with Kp/Ki/Kd = 1.5/0.1/0.2
	MTLD: at start, tgt = 50cm, curr_dist = 11.6, tgt_window = 2.0, front/rear var = 12424.4/795.3
Msec	Left	Steer	LCorr	TgtD	err	Ival	Dval	Out	Speed	FVar	RVar
449355	15.0	0.4	11.0	50	35.0	3.5	35.0	49.0	49	11670.6	765.1
449555	17.0	0.4	13.6	50	33.0	6.8	-2.0	56.7	56	10361.3	733.2
449756	20.0	0.3	16.5	50	30.0	9.8	-3.0	55.4	55	8874.8	703.7
449967	24.0	0.4	17.6	50	26.0	12.4	-4.0	52.2	52	7277.8	671.1
450155	28.0	0.5	20.3	50	22.0	14.6	-4.0	48.4	48	5770.4	639.1
450358	32.0	0.3	26.4	50	18.0	16.4	-4.0	44.2	44	4821.3	606.5
450568	36.0	0.4	29.1	50	14.0	17.8	-4.0	39.6	39	3530.9	575.9
450757	40.0	0.4	29.8	50	10.0	18.8	-4.0	34.6	34	2451.5	548.3
450957	44.0	0.5	31.5	50	6.0	19.4	-4.0	29.2	29	1616.3	521.7
451158	49.0	0.3	41.3	50	1.0	19.5	-5.0	22.0	22	1034.0	499.6
451346	48.0	0.1	46.5	50	2.0	19.7	1.0	22.5	22	787.6	477.2
451558	51.0	0.4	40.8	50	-1.0	19.6	-3.0	18.7	18	584.6	460.0
451769	52.0	0.5	34.2	50	-2.0	19.4	-1.0	16.6	16	546.3	443.4
451958	52.0	0.4	42.0	50	-2.0	19.2	0.0	16.2	16	603.0	425.0
452160	54.0	0.5	38.7	50	-4.0	18.8	-2.0	13.2	13	679.0	410.9
452358	53.0	0.3	45.6	50	-3.0	18.5	1.0	13.8	13	688.5	397.6
452557	55.0	0.2	52.8	50	-5.0	18.0	-2.0	10.9	10	569.1	383.8
452759	55.0	0.4	43.5	50	-5.0	17.5	0.0	10.0	10	523.6	370.3
452960	54.0	0.3	46.0	50	-4.0	17.1	1.0	10.9	10	451.6	359.2
453161	54.0	0.2	51.8	50	-4.0	16.7	0.0	10.7	10	358.7	347.7
	MTLD: Stopped with left dist = 51.8 and errcode = NO_ANOMALIES
tLeft	Steer	LCorr
54.00	0.24	51.81
53.00	0.31	51.81
53.00	0.16	51.81
54.00	0.27	51.81
53.00	0.31	51.81
56.00	0.30	51.81
56.00	0.54	51.81
55.00	0.33	51.81
55.00	0.37	51.81
54.00	0.36	51.81

In MoveToDesiredLeftDistCm(50) with Kp/Ki/Kd = 1.5/0.1/0.2

MTLD: at start, tgt = 50cm, curr_dist = 11.6, tgt_window = 2.0, front/rear var = 12424.4/795.3

Msec Left Steer LCorr TgtD err Ival Dval Out Speed FVar RVar

449355 15.0 0.4 11.0 50 35.0 3.5 35.0 49.0 49 11670.6 765.1

449555 17.0 0.4 13.6 50 33.0 6.8 -2.0 56.7 56 10361.3 733.2

449756 20.0 0.3 16.5 50 30.0 9.8 -3.0 55.4 55 8874.8 703.7

449967 24.0 0.4 17.6 50 26.0 12.4 -4.0 52.2 52 7277.8 671.1

450155 28.0 0.5 20.3 50 22.0 14.6 -4.0 48.4 48 5770.4 639.1

450358 32.0 0.3 26.4 50 18.0 16.4 -4.0 44.2 44 4821.3 606.5

450568 36.0 0.4 29.1 50 14.0 17.8 -4.0 39.6 39 3530.9 575.9

450757 40.0 0.4 29.8 50 10.0 18.8 -4.0 34.6 34 2451.5 548.3

450957 44.0 0.5 31.5 50 6.0 19.4 -4.0 29.2 29 1616.3 521.7

451158 49.0 0.3 41.3 50 1.0 19.5 -5.0 22.0 22 1034.0 499.6

451346 48.0 0.1 46.5 50 2.0 19.7 1.0 22.5 22 787.6 477.2

451558 51.0 0.4 40.8 50 -1.0 19.6 -3.0 18.7 18 584.6 460.0

451769 52.0 0.5 34.2 50 -2.0 19.4 -1.0 16.6 16 546.3 443.4

451958 52.0 0.4 42.0 50 -2.0 19.2 0.0 16.2 16 603.0 425.0

452160 54.0 0.5 38.7 50 -4.0 18.8 -2.0 13.2 13 679.0 410.9

452358 53.0 0.3 45.6 50 -3.0 18.5 1.0 13.8 13 688.5 397.6

452557 55.0 0.2 52.8 50 -5.0 18.0 -2.0 10.9 10 569.1 383.8

452759 55.0 0.4 43.5 50 -5.0 17.5 0.0 10.0 10 523.6 370.3

452960 54.0 0.3 46.0 50 -4.0 17.1 1.0 10.9 10 451.6 359.2

453161 54.0 0.2 51.8 50 -4.0 16.7 0.0 10.7 10 358.7 347.7

MTLD: Stopped with left dist = 51.8 and errcode = NO_ANOMALIES

tLeft Steer LCorr

54.00 0.24 51.81

53.00 0.31 51.81

53.00 0.16 51.81

54.00 0.27 51.81

53.00 0.31 51.81

56.00 0.30 51.81

56.00 0.54 51.81

55.00 0.33 51.81

55.00 0.37 51.81

54.00 0.36 51.81

MoveToDesiredRightDistCm():

For this side, I simply copied ‘MoveToDesiredLeftDistCm()’ and changed all occurrences of ‘Left’ to ‘Right’. Here’s the telemetry and a short video from a typical run:

In MoveToDesiredRightDistCm(50) with Kp/Ki/Kd = 1.5/0.1/0.2
	MTLD: at start, tgt = 50cm, curr_dist = 7.6, tgt_window = 2.0, front/rear var = 11089.8/793.5
Msec	Right	Steer	LCorr	TgtD	err	Ival	Dval	Out	Speed	FVar	RVar
550636	12.5	0.6	8.0	50	37.5	3.8	37.5	52.5	52	9725.3	757.3
550836	12.6	0.5	8.2	50	37.4	7.5	-0.1	63.6	63	7898.5	721.0
551037	14.9	0.6	9.5	50	35.1	11.0	-2.3	64.1	64	6359.4	685.1
551236	18.5	0.6	11.5	50	31.5	14.1	-3.6	62.1	62	5139.8	650.9
551436	21.7	0.5	15.5	50	28.3	17.0	-3.2	60.1	60	4263.9	615.4
551638	25.7	0.6	16.2	50	24.3	19.4	-4.0	56.7	56	3856.9	582.3
551837	31.3	0.4	23.3	50	18.7	21.3	-5.6	50.5	50	3521.1	551.2
552037	36.1	0.5	25.2	50	13.9	22.7	-4.8	44.5	44	3467.5	521.7
552238	38.6	0.3	31.9	50	11.4	23.8	-2.5	41.4	41	3645.5	498.1
552438	43.1	0.5	30.4	50	6.9	24.5	-4.5	35.8	35	3949.4	472.4
552629	45.6	0.5	31.3	50	4.4	24.9	-2.5	32.0	32	4180.0	458.8
552838	48.1	0.7	24.3	50	1.9	25.1	-2.5	28.5	28	4478.5	442.0
553039	49.7	0.4	36.5	50	0.3	25.2	-1.6	25.9	25	4570.0	438.2
553239	50.6	0.5	34.3	50	-0.6	25.1	-0.9	24.4	24	4308.9	9713.3
553439	52.1	0.5	33.8	50	-2.1	24.9	-1.5	22.0	22	3548.7	9677.5
553628	54.0	0.3	47.3	50	-4.0	24.5	-1.9	18.9	18	2677.1	18508.9
553839	54.6	0.5	35.4	50	-4.6	24.0	-0.6	17.3	17	676.3	28807.2
554041	55.3	0.4	41.7	50	-5.3	23.5	-0.7	15.7	15	648.9	38619.4
554240	54.9	0.2	51.2	50	-4.9	23.0	0.4	15.6	15	588.1	46162.1
	MTLD: Stopped with Right dist = 51.2 and errcode = NO_ANOMALIES
tRight	Steer	LCorr
56.30	0.43	51.15
55.30	0.37	51.15
54.90	0.36	51.15
56.80	0.23	51.15
55.60	0.52	51.15
55.60	0.34	51.15
56.20	0.41	51.15
55.70	0.37	51.15
55.90	0.53	51.15
55.70	0.21	51.15

In MoveToDesiredRightDistCm(50) with Kp/Ki/Kd = 1.5/0.1/0.2

MTLD: at start, tgt = 50cm, curr_dist = 7.6, tgt_window = 2.0, front/rear var = 11089.8/793.5

Msec Right Steer LCorr TgtD err Ival Dval Out Speed FVar RVar

550636 12.5 0.6 8.0 50 37.5 3.8 37.5 52.5 52 9725.3 757.3

550836 12.6 0.5 8.2 50 37.4 7.5 -0.1 63.6 63 7898.5 721.0

551037 14.9 0.6 9.5 50 35.1 11.0 -2.3 64.1 64 6359.4 685.1

551236 18.5 0.6 11.5 50 31.5 14.1 -3.6 62.1 62 5139.8 650.9

551436 21.7 0.5 15.5 50 28.3 17.0 -3.2 60.1 60 4263.9 615.4

551638 25.7 0.6 16.2 50 24.3 19.4 -4.0 56.7 56 3856.9 582.3

551837 31.3 0.4 23.3 50 18.7 21.3 -5.6 50.5 50 3521.1 551.2

552037 36.1 0.5 25.2 50 13.9 22.7 -4.8 44.5 44 3467.5 521.7

552238 38.6 0.3 31.9 50 11.4 23.8 -2.5 41.4 41 3645.5 498.1

552438 43.1 0.5 30.4 50 6.9 24.5 -4.5 35.8 35 3949.4 472.4

552629 45.6 0.5 31.3 50 4.4 24.9 -2.5 32.0 32 4180.0 458.8

552838 48.1 0.7 24.3 50 1.9 25.1 -2.5 28.5 28 4478.5 442.0

553039 49.7 0.4 36.5 50 0.3 25.2 -1.6 25.9 25 4570.0 438.2

553239 50.6 0.5 34.3 50 -0.6 25.1 -0.9 24.4 24 4308.9 9713.3

553439 52.1 0.5 33.8 50 -2.1 24.9 -1.5 22.0 22 3548.7 9677.5

553628 54.0 0.3 47.3 50 -4.0 24.5 -1.9 18.9 18 2677.1 18508.9

553839 54.6 0.5 35.4 50 -4.6 24.0 -0.6 17.3 17 676.3 28807.2

554041 55.3 0.4 41.7 50 -5.3 23.5 -0.7 15.7 15 648.9 38619.4

554240 54.9 0.2 51.2 50 -4.9 23.0 0.4 15.6 15 588.1 46162.1

MTLD: Stopped with Right dist = 51.2 and errcode = NO_ANOMALIES

tRight Steer LCorr

56.30 0.43 51.15

55.30 0.37 51.15

54.90 0.36 51.15

56.80 0.23 51.15

55.60 0.52 51.15

55.60 0.34 51.15

56.20 0.41 51.15

55.70 0.37 51.15

55.90 0.53 51.15

55.70 0.21 51.15

Summary:

It looks like all four (front/back/left/right) motion features are now working fine, with the same PID = (1.5, 0.1, 0.2) configuration – yay!

Now the challenge is to integrate this all back into my mainline program and start testing wall tracking again.

Stay Tuned,

Frank

Post Views: 391

Paynter's Palace

Move to a Specified Distance, Revisited

MoveToDesiredFrontDistCm ():

MoveToDesiredRearDistCm():

MoveToDesiredLeftDistCm():

MoveToDesiredRightDistCm():

Summary:

Leave a Reply Cancel reply