Tag Archives: Wall-E2

Wall-E2 Gets a VL53L0X Array Upgrade

Posted 13 March 2021

Back in May of last year I started the process of replacing the ultrasonic distance sensors on Wall-E2, my autonomous wall-following robot, with two side-looking arrays of STMicroelectronics’ VL53L0X infrared laser ‘Time-of-Flight’ distance sensor. Later that same year I upgraded the installation by adding a rear distance sensor as well, as shown in the following photo:

left-hand VL53L0X array and rear sensor shown. Identical 3-elelment array on the other side

All went swimmingly until I started having problems when Wall-E2 connected up to its charger. Wall-E2 connecting to its charger is a pretty dynamic event, as it has to build up enough speed to make sure the charging probe seats into the charging jack properly. I discovered that about every other time Wall-E2 connected, the distance sensors would start reporting ‘-1’ instead of the actual distances. This meant that when Wall-E2 disconnected from charging, it had no idea what to do or which way to turn. This didn’t happen all the time, but often enough to be very worrisome.

After some head-scratching and program instrumentation I became convinced this was a real issue, and I posted on the ST Micro’s forum about the issue. ST Micro’s VL53L0X expert John E. Kvam answered with this post:

The VL53L0X sensor will not return a -1. The user manual does define a -1 error, but it states that this cannot happen. What does return a -1 is an I2C timeout.
And I think that is what is killing you. The I2C is notorious for being basically unreliable. And the major symptom of a dropped bit (the most likely failure), is that the bus is stuck low. (When nothing is being transmitted both the clock and data lines should be high.) Philips designed the I2C bus and NXP bought Philips. So the NXP site has some documentation on how to tweak the bus. I’d read and understand that.
One other possibility is that one or more of the sensors rebooted – perhaps due to power glitch. If this happens the sensor will revert to it’s base I2C address.
If you’ve changed all your addresses there should be nothing at address 0x29 (0x52/0x53). You could occasionally ping the base address – and if you ever got an answer, you would would know you had a reboot of some kind.

After thinking about this some more, I realized that there is a good chance that the I2C ‘daisy-chain’ wiring and/or the power/ground connections are getting interrupted due to the impulse generated when Wall-E2 hits the charging station stop, and this is causing one or more of the sensors to drop out. I thought I had done a very careful job with the I2C/power ‘daisy-chain’, but I had thought that about a previous effort where I found a faulty ground connection, so I knew it was a possibility.

After thinking about this some more, it occurred to me that I could eliminate most of the point-point wiring issues by creating a PCB to house the VL53L0X modules, with a single 4-pin connector for I2C and power. I had made a PCB some years before using DipTrace that had turned out petty well, so making one for the very simple VL53L0X wiring scheme should be a piece of cake.

Well, as it turned out, designing the PCB was the easy part. However, when it came to the part about having the boards actually manufactured, I was in for a bit of a shock. For my previous board I used DipTrace’s ‘baked-in’ manufacturer Bay Area Circuits to purchase 5 boards for around $30, but when I tried this same trick with my new board, the minimum charge for boards from BAC was $150 – ouch!

After a lot of web searching and research, I eventually found the Chinese company JLCPCB and discovered they have a very nice document that shows how to export the required files from DipTrace and upload them to their site. Took me about 30 minutes to go through the process the first time, and now I have my 5ea boards ordered for a grand total of about $15USD. Only a 10:1 ratio from BAC to JLCPCB. I can’t imagine how BAC can stay in business, and I can’t imagine why DipTrace has that company ‘baked in’ and not JLCPCB. DipTrace must be getting a heck of a kickback from BAC!

Less than 10 days later, I had the finished PCBs in my hand – wow! Here’s a photo showing two PCB’s installed on Wall-E2. I installed 4-pin headers on both ends of the near PCB in order to daisy-chain the I2C and power connections to the rear distance sensor (hidden behind the red ‘rear bumper’ block in the right background).

New VL53L0X array PCB to replace all the hand-wired I2C/Power connections.

Comparing the ‘before’ and ‘after’ photos, it is easy to see that the PCB installation eliminates two 4-pin connectors and a three-loop daisy chain on one side of the robot, and two 4-pin connectors and a four-loop daisy chain on the other side (the one that also connects to the rear distance sensor. Moreover, now none of the 3 connectors used for all seven sensors has more than one wire per pin.

Hopefully this upgrade will eliminate (or at least significantly suppress) wiring and/or connector issues associated with the charger-connect ‘impulse’ – we’ll see

25 March 2021 Update:

After getting the new PCBs installed and some other connection issues solved, Wall-E2’s sensors seem to be a lot more reliable. However, I decided to take this opportunity to study John Kvam’s idea of re-initializing all seven sensors if one of them happens to show up at the default VL53L0X I2C address.

So, I modified the Teensy 3.5 VL53L0X array management code to abstract the sensor init code from setup() to its own function so it can be called if the program detects a sensor at the default I2C address, and then added code in loop() to do just that. Here’s the detection code:

  //03/25/21 see if any VL53L0X responds to a query at the default address
  Wire1.beginTransmission(DEFAULT_VL53L0X_ADDR);
  if (!Wire1.endTransmission() != 0)
  {
    Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire1!\n", DEFAULT_VL53L0X_ADDR);
    InitAllSensors();
  }
  Wire2.beginTransmission(DEFAULT_VL53L0X_ADDR);
  if (!Wire2.endTransmission() != 0)
  {
    Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire2!\n", DEFAULT_VL53L0X_ADDR);
    InitAllSensors();
  }

//03/25/21 see if any VL53L0X responds to a query at the default address

Wire1.beginTransmission(DEFAULT_VL53L0X_ADDR);

if (!Wire1.endTransmission() != 0)

{

Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire1!\n", DEFAULT_VL53L0X_ADDR);

InitAllSensors();

}

Wire2.beginTransmission(DEFAULT_VL53L0X_ADDR);

if (!Wire2.endTransmission() != 0)

{

Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire2!\n", DEFAULT_VL53L0X_ADDR);

InitAllSensors();

}

and here’s the new ‘InitAllSensors()’ routine:

bool InitAllSensors()
{
  //Purpose: Initialize all seven VL53L0X sensors
  //Provenance: 03/22/21

  Serial.printf("Initializing VL53L0X Sensor Array Elements\n");
 
  //Put all sensors in reset mode by pulling XSHUT low
  digitalWrite(RF_XSHUT_PIN, LOW);
  digitalWrite(RC_XSHUT_PIN, LOW);
  digitalWrite(RR_XSHUT_PIN, LOW);

  digitalWrite(LF_XSHUT_PIN, LOW);
  digitalWrite(LC_XSHUT_PIN, LOW);
  digitalWrite(LR_XSHUT_PIN, LOW);

  //added 10/23/20 for rear sensor
  pinMode(Rear_XSHUT_PIN, OUTPUT);
  digitalWrite(Rear_XSHUT_PIN, LOW);


  //now bring lidar_RF only out of reset and set it's address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RF_XSHUT_PIN, INPUT);
  delay(10);

  bool retval = true;

  if (!lidar_RF.init())
  {
    Serial.println("Failed to detect and initialize lidar_RF!");
    //while (1) {}
    retval = false;
  }

  //from Pololu forum post
  lidar_RF.setMeasurementTimingBudget(20000);
  lidar_RF.startContinuous();


  lidar_RF.setAddress(DEFAULT_VL53L0X_ADDR + 1);
  Serial.printf("lidar_RF address is 0x%x\n", lidar_RF.getAddress());

  delay(100);

  //now bring lidar_RC only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RC_XSHUT_PIN, INPUT);

  delay(10);
  if (!lidar_RC.init())
  {
    Serial.println("Failed to detect and initialize lidar_RC!");
    //while (1) {}
    retval = false;
  }

  //from Pololu forum post
  lidar_RC.setMeasurementTimingBudget(20000);
  lidar_RC.startContinuous();

  lidar_RC.setAddress(DEFAULT_VL53L0X_ADDR + 2);
  Serial.printf("lidar_RC address is 0x%x\n", lidar_RC.getAddress());

  delay(100);

  //now bring lidar_RR only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RR_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_RR.init())
  {
    Serial.println("Failed to detect and initialize lidar_RR!");
    //while (1) {}
    retval = false;
  }

  //from Pololu forum post
  lidar_RR.setMeasurementTimingBudget(20000);
  lidar_RR.startContinuous();

  lidar_RR.setAddress(DEFAULT_VL53L0X_ADDR + 3);
  Serial.printf("lidar_RR address is 0x%x\n", lidar_RR.getAddress());

  delay(100);

  //now bring lidar_LF only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(LF_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_LF.init())
  {
    Serial.println("Failed to detect and initialize lidar_LF!");
    //while (1) {}
    retval = false;
  }

  //from Pololu forum post
  lidar_LF.setMeasurementTimingBudget(20000);
  lidar_LF.startContinuous();

  lidar_LF.setAddress(DEFAULT_VL53L0X_ADDR + 4);
  Serial.printf("lidar_LF address is 0x%x\n", lidar_LF.getAddress());

  delay(100);

  //now bring lidar_LC only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(LC_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_LC.init())
  {
    Serial.println("Failed to detect and initialize lidar_LC!");
    //while (1) {}
    retval = false;
  }

  //from Pololu forum post
  lidar_LC.setMeasurementTimingBudget(20000);
  lidar_LC.startContinuous();

  lidar_LC.setAddress(DEFAULT_VL53L0X_ADDR + 5);
  Serial.printf("lidar_LC address is 0x%x\n", lidar_LC.getAddress());

  delay(100);

  //now bring lidar_LR only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(LR_XSHUT_PIN, INPUT);

  if (!lidar_LR.init())
  {
    Serial.println("Failed to detect and initialize lidar_LR!");
    //while (1) {}
    retval = false;
  }

  //from Pololu forum post
  lidar_LR.setMeasurementTimingBudget(20000);
  lidar_LR.startContinuous();

  lidar_LR.setAddress(DEFAULT_VL53L0X_ADDR + 6);
  Serial.printf("lidar_LR address is 0x%x\n", lidar_LR.getAddress());

  delay(100);

  //added 10/23/20 for rear sensor
  //now bring lidar_Rear only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(Rear_XSHUT_PIN, INPUT);

  if (!lidar_Rear.init())
  {
    Serial.println("Failed to detect and initialize lidar_Rear!");
    //while (1) {}
    retval = false;
  }

  //from Pololu forum post
  lidar_Rear.setMeasurementTimingBudget(20000);
  lidar_Rear.startContinuous();

  lidar_Rear.setAddress(DEFAULT_VL53L0X_ADDR + 7);
  Serial.printf("lidar_Rear address is 0x%x\n", lidar_Rear.getAddress());

  return retval;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

bool InitAllSensors()

{

//Purpose: Initialize all seven VL53L0X sensors

//Provenance: 03/22/21

Serial.printf("Initializing VL53L0X Sensor Array Elements\n");

//Put all sensors in reset mode by pulling XSHUT low

digitalWrite(RF_XSHUT_PIN, LOW);

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

digitalWrite(LF_XSHUT_PIN, LOW);

digitalWrite(LC_XSHUT_PIN, LOW);

digitalWrite(LR_XSHUT_PIN, LOW);

//added 10/23/20 for rear sensor

pinMode(Rear_XSHUT_PIN, OUTPUT);

digitalWrite(Rear_XSHUT_PIN, LOW);

//now bring lidar_RF only out of reset and set it's address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(RF_XSHUT_PIN, INPUT);

delay(10);

bool retval = true;

if (!lidar_RF.init())

{

Serial.println("Failed to detect and initialize lidar_RF!");

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_RF.setMeasurementTimingBudget(20000);

lidar_RF.startContinuous();

lidar_RF.setAddress(DEFAULT_VL53L0X_ADDR + 1);

Serial.printf("lidar_RF address is 0x%x\n", lidar_RF.getAddress());

delay(100);

//now bring lidar_RC only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(RC_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RC.init())

{

Serial.println("Failed to detect and initialize lidar_RC!");

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_RC.setMeasurementTimingBudget(20000);

lidar_RC.startContinuous();

lidar_RC.setAddress(DEFAULT_VL53L0X_ADDR + 2);

Serial.printf("lidar_RC address is 0x%x\n", lidar_RC.getAddress());

delay(100);

//now bring lidar_RR only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(RR_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RR.init())

{

Serial.println("Failed to detect and initialize lidar_RR!");

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_RR.setMeasurementTimingBudget(20000);

lidar_RR.startContinuous();

lidar_RR.setAddress(DEFAULT_VL53L0X_ADDR + 3);

Serial.printf("lidar_RR address is 0x%x\n", lidar_RR.getAddress());

delay(100);

//now bring lidar_LF only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(LF_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_LF.init())

{

Serial.println("Failed to detect and initialize lidar_LF!");

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_LF.setMeasurementTimingBudget(20000);

lidar_LF.startContinuous();

lidar_LF.setAddress(DEFAULT_VL53L0X_ADDR + 4);

Serial.printf("lidar_LF address is 0x%x\n", lidar_LF.getAddress());

delay(100);

//now bring lidar_LC only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(LC_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_LC.init())

{

Serial.println("Failed to detect and initialize lidar_LC!");

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_LC.setMeasurementTimingBudget(20000);

lidar_LC.startContinuous();

lidar_LC.setAddress(DEFAULT_VL53L0X_ADDR + 5);

Serial.printf("lidar_LC address is 0x%x\n", lidar_LC.getAddress());

delay(100);

//now bring lidar_LR only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(LR_XSHUT_PIN, INPUT);

if (!lidar_LR.init())

{

Serial.println("Failed to detect and initialize lidar_LR!");

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_LR.setMeasurementTimingBudget(20000);

lidar_LR.startContinuous();

lidar_LR.setAddress(DEFAULT_VL53L0X_ADDR + 6);

Serial.printf("lidar_LR address is 0x%x\n", lidar_LR.getAddress());

delay(100);

//added 10/23/20 for rear sensor

//now bring lidar_Rear only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(Rear_XSHUT_PIN, INPUT);

if (!lidar_Rear.init())

{

Serial.println("Failed to detect and initialize lidar_Rear!");

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_Rear.setMeasurementTimingBudget(20000);

lidar_Rear.startContinuous();

lidar_Rear.setAddress(DEFAULT_VL53L0X_ADDR + 7);

Serial.printf("lidar_Rear address is 0x%x\n", lidar_Rear.getAddress());

return retval;

}

And here is the complete Teensy program for managing the VL53L0X arrays:

/*
    Name:       Teensy_7VL53L0X_I2C_Slave_V3.ino
    Created:	1/24/2021 4:03:35 PM
    Author:     FRANKNEWXPS15\Frank

    This is the 'final' version of the Teensy 3.5 project to manage all 
    seven VL53L0X lidar distance sensors, three on left, three on right, 
    and now including a rear distance sensor.  As of this date it was an
    identical copy of the TeensyHexVL53L0XHighSpeedDemo project, just
    renamed for clarity.  This program was uploaded to the Teensy on 
    Wall-E2's second deck on 1/24/21

    Notes:
      02/01/21 Added code in requestEvent() to blink LED_BUILTIN when distance data is requested
      03/22/21 Abstracted sensor init code from setup() to InitAllSensors() so can use elsewhere

*/

#include <Wire.h> //using Wire.h here instead of i2c_t3.h for multiple I2C bus access
#include <VL53L0X.h>
#include "I2C_Anything.h" //local version modified to use Wire.h vs SBWire

volatile uint8_t request_type;

const int SETUP_DURATION_OUTPUT_PIN = 33;
const int REQUEST_EVENT_DURATION_OUTPUT_PIN = 34;
bool bTeensyReady = false;


enum VL53L0X_REQUEST
{
  VL53L0X_READYCHECK, //added 11/10/20 to prevent bad reads during Teensy setup()
  VL53L0X_CENTERS_ONLY,
  VL53L0X_RIGHT,
  VL53L0X_LEFT,
  VL53L0X_ALL, //added 08/05/20, chg 'BOTH' to 'ALL' 10/31/20
  VL53L0X_REAR_ONLY //added 10/24/20
};


//right side array
VL53L0X lidar_RF(&Wire1);
VL53L0X lidar_RC(&Wire1);
VL53L0X lidar_RR(&Wire1);

//left side array
VL53L0X lidar_LF(&Wire2);
VL53L0X lidar_LC(&Wire2);
VL53L0X lidar_LR(&Wire2);

//XSHUT required for I2C address init 
//right side array
const int RF_XSHUT_PIN = 23;
const int RC_XSHUT_PIN = 22;
const int RR_XSHUT_PIN = 21;

//left side array
const int LF_XSHUT_PIN = 5;
const int LC_XSHUT_PIN = 6;
const int LR_XSHUT_PIN = 7;

const int MAX_LIDAR_RANGE_MM = 1000; //make it obvious when nearest object is out of range

//right side array
uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;

//left side array
uint16_t LF_Dist_mm;
uint16_t LC_Dist_mm;
uint16_t LR_Dist_mm;


float RightSteeringVal;
float LeftSteeringVal;

const int SLAVE_ADDRESS = 0x20; //just a guess for now
const int DEFAULT_VL53L0X_ADDR = 0x29;

//10/23/20 added rear distance sensor
VL53L0X lidar_Rear(&Wire2); //added to Wire2 daisy-chain
const int Rear_XSHUT_PIN = 8;
uint16_t Rear_Dist_mm;

void setup()
{
  // wait until serial port opens ... For 5 seconds max
  Serial.begin(115200);
  delay(5000);

  pinMode(LED_BUILTIN, OUTPUT);
  bTeensyReady = false; //11/10/20 added to prevent bad data reads by main controller

  pinMode(SETUP_DURATION_OUTPUT_PIN, OUTPUT);
  pinMode(REQUEST_EVENT_DURATION_OUTPUT_PIN, OUTPUT);
  digitalWrite(SETUP_DURATION_OUTPUT_PIN, HIGH);

  Wire.begin(SLAVE_ADDRESS); //set Teensy Wire0 port up as slave
  Wire.onRequest(requestEvent);  //ISR for I2C requests from master (Mega 2560)
  Wire.onReceive(receiveEvent); //ISR for I2C data from master (Mega 2560)

  Wire1.begin();
  Wire2.begin();


  pinMode(RF_XSHUT_PIN, OUTPUT);
  pinMode(RC_XSHUT_PIN, OUTPUT);
  pinMode(RR_XSHUT_PIN, OUTPUT);

  pinMode(LF_XSHUT_PIN, OUTPUT);
  pinMode(LC_XSHUT_PIN, OUTPUT);
  pinMode(LR_XSHUT_PIN, OUTPUT);

  //03/22/21 sensor init abstracted to function so can call from loop()
  bool bSensorsOK = InitAllSensors();
  Serial.printf("Just after InitAllSensors()\n");

  if (!bSensorsOK)
  {
    Serial.printf("Sensor initialization failed - quitting!\n");
    while (true)
    {

    }
  }

  Serial.printf("Msec\tLFront\tLCtr\tLRear\tRFront\tRCtr\tRRear\tLSteer\tRSteer\tRear\n");

  //02/01/21 now using LED_BUILTIN to provide visible indication of I2C data requests
  pinMode(LED_BUILTIN, OUTPUT);

  digitalWrite(SETUP_DURATION_OUTPUT_PIN, LOW);
  bTeensyReady = true; //11/10/20 added to prevent bad data reads by main controller
}

void loop()
{
  //from Pololu forum post
  RF_Dist_mm = lidar_RF.readRangeContinuousMillimeters();
  RC_Dist_mm = lidar_RC.readRangeContinuousMillimeters();
  RR_Dist_mm = lidar_RR.readRangeContinuousMillimeters();

  RightSteeringVal = (RF_Dist_mm - RR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

  //from Pololu forum post
  LF_Dist_mm = lidar_LF.readRangeContinuousMillimeters();
  LC_Dist_mm = lidar_LC.readRangeContinuousMillimeters();
  LR_Dist_mm = lidar_LR.readRangeContinuousMillimeters();

  LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

  //added 10/23/20 for rear sensor
  Rear_Dist_mm = lidar_Rear.readRangeContinuousMillimeters();


  Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n",
    millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, Rear_Dist_mm);

  delay(100);

  //03/25/21 see if any VL53L0X responds to a query at the default address
  Wire1.beginTransmission(DEFAULT_VL53L0X_ADDR);
  if (!Wire1.endTransmission() != 0)
  {
    Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire1!\n", DEFAULT_VL53L0X_ADDR);
    InitAllSensors();
  }
  Wire2.beginTransmission(DEFAULT_VL53L0X_ADDR);
  if (!Wire2.endTransmission() != 0)
  {
    Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire2!\n", DEFAULT_VL53L0X_ADDR);
    InitAllSensors();
  }

}

// function that executes whenever data is requested by master
// this function is registered as an event, see setup()
void requestEvent()
{
  //Purpose: Send requested sensor data to the Mega controller via main I2C bus
  //Inputs: 
  //  request_type = uint8_t value denoting type of data requested (from receiveEvent())
  //      0 = left & right center distances only
  //      1 = right side front, center and rear distances, plus steering value
  //      2 = left side front, center and rear distances, plus steering value
  //      3 = both side front, center and rear distances, plus both steering values
//Outputs:
  //  Requested data sent to master
  //Notes:
  //  08/05/20 added VL53L0X_ALL request type to get both sides at once
  //  10/24/20 added VL53L0X_REAR_ONLY request type
  //  11/09/20 added write to pin for O'scope monitoring
  //  02/01/21 aded LED_PIN blink


//DEBUG!!
    //Serial.printf("RequestEvent() with request_type = %d: VL53L0X Front/Center/Rear distances = %d, %d, %d\n", 
    //    request_type, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);
//DEBUG!!

  digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, HIGH);
  digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));

  switch (request_type)
  {
  case 	VL53L0X_READYCHECK: //added 11/10/20 to prevent bad reads during Teensy setup()
    Serial.printf("in VL53L0X_READYCHECK case at %lu with bTeensyReady = %d\n", millis(), bTeensyReady);
    I2C_writeAnything(bTeensyReady);
    break;

  case VL53L0X_CENTERS_ONLY:

    //DEBUG!!
            //data_size = 2*sizeof(uint16_t);
            //Serial.printf("Sending %d bytes LC_Dist_mm = %d, RC_Dist_mm = %d to master\n", data_size, LC_Dist_mm, RC_Dist_mm);
    //DEBUG!!

    I2C_writeAnything(RC_Dist_mm);
    I2C_writeAnything(LC_Dist_mm);
    break;

  case VL53L0X_RIGHT:

    //DEBUG!!
            //data_size = 3 * sizeof(uint16_t) + sizeof(float);
            //Serial.printf("Sending %d bytes RF/RC/RR/RS vals = %d, %d, %d, %3.2f to master\n",
            //    data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal);
    //DEBUG!!

    I2C_writeAnything(RF_Dist_mm);
    I2C_writeAnything(RC_Dist_mm);
    I2C_writeAnything(RR_Dist_mm);
    I2C_writeAnything(RightSteeringVal);
    break;

  case VL53L0X_LEFT:

    //DEBUG!!
            //data_size = 3 * sizeof(uint16_t) + sizeof(float);
            //Serial.printf("Sending %d bytes LF/LC/LR/LS vals = %d, %d, %d, %3.2f to master\n",
            //    data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);
    //DEBUG!!

    I2C_writeAnything(LF_Dist_mm);
    I2C_writeAnything(LC_Dist_mm);
    I2C_writeAnything(LR_Dist_mm);
    I2C_writeAnything(LeftSteeringVal);
    break;

  case VL53L0X_ALL:
    //added 08/05/20 to get data from both sides at once
    //10/31/20 chg to VL53L0X_ALL & report all 7 sensor values

//DEBUG!!
    //data_size = 3 * sizeof(uint16_t) + sizeof(float);
    //Serial.printf("Sending %d bytes LF/LC/LR/LS vals = %d, %d, %d, %3.2f to master\n",
    //    data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);
    //Serial.printf("left %d\t%d\t%d\t%d\t%2.3f",
    //  LF_Dist_mm, LC_Dist_mm, LR_Dist_mm);
//DEBUG!!

    //Serial.printf("In VL53L0X_ALL case\n");

    //left side  
    I2C_writeAnything(LF_Dist_mm);
    I2C_writeAnything(LC_Dist_mm);
    I2C_writeAnything(LR_Dist_mm);
    I2C_writeAnything(LeftSteeringVal);


    //DEBUG!!
        //data_size = 3 * sizeof(uint16_t) + sizeof(float);
        //Serial.printf("Sending %d bytes RF/RC/RR/RS vals = %d, %d, %d, %3.2f to master\n",
        //    data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, LeftSteeringVal);

        //Serial.printf(" right %d\t%d\t%d\t%d\t%2.3f\n",
        //RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);
    //DEBUG!!

       //right side
    I2C_writeAnything(RF_Dist_mm);
    I2C_writeAnything(RC_Dist_mm);
    I2C_writeAnything(RR_Dist_mm);
    I2C_writeAnything(RightSteeringVal);

    //10/31/20 now also reporting rear sensor value
    I2C_writeAnything(Rear_Dist_mm);

    break;

  case VL53L0X_REAR_ONLY:

    //DEBUG!!
      //data_size = sizeof(uint16_t);
      //Serial.printf("Sending %d bytes Rear_Dist_mm = %d to master\n", data_size, Rear_Dist_mm);
    //DEBUG!!

    I2C_writeAnything(Rear_Dist_mm);
    break;

  default:
    break;
  }

  digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, LOW);
}

//
// handle Rx Event (incoming I2C data)
//
void receiveEvent(int numBytes)
{
  //Purpose: capture data request type from Mega I2C master
  //Inputs:
  //  numBytes = int value denoting number of bytes received from master
  //Outputs:
  //  uint8_t request_type filled with request type value from master


//DEBUG!!
    //Serial.printf("receiveEvent(%d)\n", numBytes);
//DEBUG!!

  I2C_readAnything(request_type);

  //DEBUG!!
      //Serial.printf("receiveEvent: Got %d from master\n", request_type);
  //DEBUG!!
}
bool InitAllSensors()
{
  //Purpose: Initialize all seven VL53L0X sensors
  //Provenance: 03/22/21

  Serial.printf("Initializing VL53L0X Sensor Array Elements\n");
 
  //Put all sensors in reset mode by pulling XSHUT low
  digitalWrite(RF_XSHUT_PIN, LOW);
  digitalWrite(RC_XSHUT_PIN, LOW);
  digitalWrite(RR_XSHUT_PIN, LOW);

  digitalWrite(LF_XSHUT_PIN, LOW);
  digitalWrite(LC_XSHUT_PIN, LOW);
  digitalWrite(LR_XSHUT_PIN, LOW);

  //added 10/23/20 for rear sensor
  pinMode(Rear_XSHUT_PIN, OUTPUT);
  digitalWrite(Rear_XSHUT_PIN, LOW);


  //now bring lidar_RF only out of reset and set it's address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RF_XSHUT_PIN, INPUT);
  delay(10);

  bool retval = true;

  if (!lidar_RF.init())
  {
    Serial.println("Failed to detect and initialize lidar_RF!");
    //while (1) {}
    retval = false;
  }

  //from Pololu forum post
  lidar_RF.setMeasurementTimingBudget(20000);
  lidar_RF.startContinuous();


  lidar_RF.setAddress(DEFAULT_VL53L0X_ADDR + 1);
  Serial.printf("lidar_RF address is 0x%x\n", lidar_RF.getAddress());

  delay(100);

  //now bring lidar_RC only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RC_XSHUT_PIN, INPUT);

  delay(10);
  if (!lidar_RC.init())
  {
    Serial.println("Failed to detect and initialize lidar_RC!");
    //while (1) {}
    retval = false;
  }

  //from Pololu forum post
  lidar_RC.setMeasurementTimingBudget(20000);
  lidar_RC.startContinuous();

  lidar_RC.setAddress(DEFAULT_VL53L0X_ADDR + 2);
  Serial.printf("lidar_RC address is 0x%x\n", lidar_RC.getAddress());

  delay(100);

  //now bring lidar_RR only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RR_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_RR.init())
  {
    Serial.println("Failed to detect and initialize lidar_RR!");
    //while (1) {}
    retval = false;
  }

  //from Pololu forum post
  lidar_RR.setMeasurementTimingBudget(20000);
  lidar_RR.startContinuous();

  lidar_RR.setAddress(DEFAULT_VL53L0X_ADDR + 3);
  Serial.printf("lidar_RR address is 0x%x\n", lidar_RR.getAddress());

  delay(100);

  //now bring lidar_LF only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(LF_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_LF.init())
  {
    Serial.println("Failed to detect and initialize lidar_LF!");
    //while (1) {}
    retval = false;
  }

  //from Pololu forum post
  lidar_LF.setMeasurementTimingBudget(20000);
  lidar_LF.startContinuous();

  lidar_LF.setAddress(DEFAULT_VL53L0X_ADDR + 4);
  Serial.printf("lidar_LF address is 0x%x\n", lidar_LF.getAddress());

  delay(100);

  //now bring lidar_LC only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(LC_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_LC.init())
  {
    Serial.println("Failed to detect and initialize lidar_LC!");
    //while (1) {}
    retval = false;
  }

  //from Pololu forum post
  lidar_LC.setMeasurementTimingBudget(20000);
  lidar_LC.startContinuous();

  lidar_LC.setAddress(DEFAULT_VL53L0X_ADDR + 5);
  Serial.printf("lidar_LC address is 0x%x\n", lidar_LC.getAddress());

  delay(100);

  //now bring lidar_LR only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(LR_XSHUT_PIN, INPUT);

  if (!lidar_LR.init())
  {
    Serial.println("Failed to detect and initialize lidar_LR!");
    //while (1) {}
    retval = false;
  }

  //from Pololu forum post
  lidar_LR.setMeasurementTimingBudget(20000);
  lidar_LR.startContinuous();

  lidar_LR.setAddress(DEFAULT_VL53L0X_ADDR + 6);
  Serial.printf("lidar_LR address is 0x%x\n", lidar_LR.getAddress());

  delay(100);

  //added 10/23/20 for rear sensor
  //now bring lidar_Rear only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(Rear_XSHUT_PIN, INPUT);

  if (!lidar_Rear.init())
  {
    Serial.println("Failed to detect and initialize lidar_Rear!");
    //while (1) {}
    retval = false;
  }

  //from Pololu forum post
  lidar_Rear.setMeasurementTimingBudget(20000);
  lidar_Rear.startContinuous();

  lidar_Rear.setAddress(DEFAULT_VL53L0X_ADDR + 7);
  Serial.printf("lidar_Rear address is 0x%x\n", lidar_Rear.getAddress());

  return retval;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

Name: Teensy_7VL53L0X_I2C_Slave_V3.ino

Created: 1/24/2021 4:03:35 PM

Author: FRANKNEWXPS15\Frank

This is the 'final' version of the Teensy 3.5 project to manage all

seven VL53L0X lidar distance sensors, three on left, three on right,

and now including a rear distance sensor. As of this date it was an

identical copy of the TeensyHexVL53L0XHighSpeedDemo project, just

renamed for clarity. This program was uploaded to the Teensy on

Wall-E2's second deck on 1/24/21

Notes:

02/01/21 Added code in requestEvent() to blink LED_BUILTIN when distance data is requested

03/22/21 Abstracted sensor init code from setup() to InitAllSensors() so can use elsewhere

#include <Wire.h> //using Wire.h here instead of i2c_t3.h for multiple I2C bus access

#include <VL53L0X.h>

#include "I2C_Anything.h" //local version modified to use Wire.h vs SBWire

volatile uint8_t request_type;

const int SETUP_DURATION_OUTPUT_PIN = 33;

const int REQUEST_EVENT_DURATION_OUTPUT_PIN = 34;

bool bTeensyReady = false;

enum VL53L0X_REQUEST

{

VL53L0X_READYCHECK, //added 11/10/20 to prevent bad reads during Teensy setup()

VL53L0X_CENTERS_ONLY,

VL53L0X_RIGHT,

VL53L0X_LEFT,

VL53L0X_ALL, //added 08/05/20, chg 'BOTH' to 'ALL' 10/31/20

VL53L0X_REAR_ONLY //added 10/24/20

};

//right side array

VL53L0X lidar_RF(&Wire1);

VL53L0X lidar_RC(&Wire1);

VL53L0X lidar_RR(&Wire1);

//left side array

VL53L0X lidar_LF(&Wire2);

VL53L0X lidar_LC(&Wire2);

VL53L0X lidar_LR(&Wire2);

//XSHUT required for I2C address init

//right side array

const int RF_XSHUT_PIN = 23;

const int RC_XSHUT_PIN = 22;

const int RR_XSHUT_PIN = 21;

//left side array

const int LF_XSHUT_PIN = 5;

const int LC_XSHUT_PIN = 6;

const int LR_XSHUT_PIN = 7;

const int MAX_LIDAR_RANGE_MM = 1000; //make it obvious when nearest object is out of range

//right side array

uint16_t RF_Dist_mm;

uint16_t RC_Dist_mm;

uint16_t RR_Dist_mm;

//left side array

uint16_t LF_Dist_mm;

uint16_t LC_Dist_mm;

uint16_t LR_Dist_mm;

float RightSteeringVal;

float LeftSteeringVal;

const int SLAVE_ADDRESS = 0x20; //just a guess for now

const int DEFAULT_VL53L0X_ADDR = 0x29;

//10/23/20 added rear distance sensor

VL53L0X lidar_Rear(&Wire2); //added to Wire2 daisy-chain

const int Rear_XSHUT_PIN = 8;

uint16_t Rear_Dist_mm;

void setup()

{

// wait until serial port opens ... For 5 seconds max

Serial.begin(115200);

delay(5000);

pinMode(LED_BUILTIN, OUTPUT);

bTeensyReady = false; //11/10/20 added to prevent bad data reads by main controller

pinMode(SETUP_DURATION_OUTPUT_PIN, OUTPUT);

pinMode(REQUEST_EVENT_DURATION_OUTPUT_PIN, OUTPUT);

digitalWrite(SETUP_DURATION_OUTPUT_PIN, HIGH);

Wire.begin(SLAVE_ADDRESS); //set Teensy Wire0 port up as slave

Wire.onRequest(requestEvent); //ISR for I2C requests from master (Mega 2560)

Wire.onReceive(receiveEvent); //ISR for I2C data from master (Mega 2560)

Wire1.begin();

Wire2.begin();

pinMode(RF_XSHUT_PIN, OUTPUT);

pinMode(RC_XSHUT_PIN, OUTPUT);

pinMode(RR_XSHUT_PIN, OUTPUT);

pinMode(LF_XSHUT_PIN, OUTPUT);

pinMode(LC_XSHUT_PIN, OUTPUT);

pinMode(LR_XSHUT_PIN, OUTPUT);

//03/22/21 sensor init abstracted to function so can call from loop()

bool bSensorsOK = InitAllSensors();

Serial.printf("Just after InitAllSensors()\n");

if (!bSensorsOK)

{

Serial.printf("Sensor initialization failed - quitting!\n");

while (true)

{

}

Serial.printf("Msec\tLFront\tLCtr\tLRear\tRFront\tRCtr\tRRear\tLSteer\tRSteer\tRear\n");

//02/01/21 now using LED_BUILTIN to provide visible indication of I2C data requests

pinMode(LED_BUILTIN, OUTPUT);

digitalWrite(SETUP_DURATION_OUTPUT_PIN, LOW);

bTeensyReady = true; //11/10/20 added to prevent bad data reads by main controller

}

void loop()

{

//from Pololu forum post

RF_Dist_mm = lidar_RF.readRangeContinuousMillimeters();

RC_Dist_mm = lidar_RC.readRangeContinuousMillimeters();

RR_Dist_mm = lidar_RR.readRangeContinuousMillimeters();

RightSteeringVal = (RF_Dist_mm - RR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

//from Pololu forum post

LF_Dist_mm = lidar_LF.readRangeContinuousMillimeters();

LC_Dist_mm = lidar_LC.readRangeContinuousMillimeters();

LR_Dist_mm = lidar_LR.readRangeContinuousMillimeters();

LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

//added 10/23/20 for rear sensor

Rear_Dist_mm = lidar_Rear.readRangeContinuousMillimeters();

Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n",

millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, Rear_Dist_mm);

delay(100);

//03/25/21 see if any VL53L0X responds to a query at the default address

Wire1.beginTransmission(DEFAULT_VL53L0X_ADDR);

if (!Wire1.endTransmission() != 0)

{

Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire1!\n", DEFAULT_VL53L0X_ADDR);

InitAllSensors();

}

Wire2.beginTransmission(DEFAULT_VL53L0X_ADDR);

if (!Wire2.endTransmission() != 0)

{

Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire2!\n", DEFAULT_VL53L0X_ADDR);

InitAllSensors();

}

// function that executes whenever data is requested by master

// this function is registered as an event, see setup()

void requestEvent()

{

//Purpose: Send requested sensor data to the Mega controller via main I2C bus

//Inputs:

// request_type = uint8_t value denoting type of data requested (from receiveEvent())

// 0 = left & right center distances only

// 1 = right side front, center and rear distances, plus steering value

// 2 = left side front, center and rear distances, plus steering value

// 3 = both side front, center and rear distances, plus both steering values

//Outputs:

// Requested data sent to master

//Notes:

// 08/05/20 added VL53L0X_ALL request type to get both sides at once

// 10/24/20 added VL53L0X_REAR_ONLY request type

// 11/09/20 added write to pin for O'scope monitoring

// 02/01/21 aded LED_PIN blink

//DEBUG!!

//Serial.printf("RequestEvent() with request_type = %d: VL53L0X Front/Center/Rear distances = %d, %d, %d\n",

// request_type, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);

//DEBUG!!

digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, HIGH);

digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));

switch (request_type)

{

case VL53L0X_READYCHECK: //added 11/10/20 to prevent bad reads during Teensy setup()

Serial.printf("in VL53L0X_READYCHECK case at %lu with bTeensyReady = %d\n", millis(), bTeensyReady);

I2C_writeAnything(bTeensyReady);

break;

case VL53L0X_CENTERS_ONLY:

//DEBUG!!

//data_size = 2*sizeof(uint16_t);

//Serial.printf("Sending %d bytes LC_Dist_mm = %d, RC_Dist_mm = %d to master\n", data_size, LC_Dist_mm, RC_Dist_mm);

//DEBUG!!

I2C_writeAnything(RC_Dist_mm);

I2C_writeAnything(LC_Dist_mm);

break;

case VL53L0X_RIGHT:

//DEBUG!!

//data_size = 3 * sizeof(uint16_t) + sizeof(float);

//Serial.printf("Sending %d bytes RF/RC/RR/RS vals = %d, %d, %d, %3.2f to master\n",

// data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal);

//DEBUG!!

I2C_writeAnything(RF_Dist_mm);

I2C_writeAnything(RC_Dist_mm);

I2C_writeAnything(RR_Dist_mm);

I2C_writeAnything(RightSteeringVal);

break;

case VL53L0X_LEFT:

//DEBUG!!

//data_size = 3 * sizeof(uint16_t) + sizeof(float);

//Serial.printf("Sending %d bytes LF/LC/LR/LS vals = %d, %d, %d, %3.2f to master\n",

// data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);

//DEBUG!!

I2C_writeAnything(LF_Dist_mm);

I2C_writeAnything(LC_Dist_mm);

I2C_writeAnything(LR_Dist_mm);

I2C_writeAnything(LeftSteeringVal);

break;

case VL53L0X_ALL:

//added 08/05/20 to get data from both sides at once

//10/31/20 chg to VL53L0X_ALL & report all 7 sensor values

//DEBUG!!

//data_size = 3 * sizeof(uint16_t) + sizeof(float);

//Serial.printf("Sending %d bytes LF/LC/LR/LS vals = %d, %d, %d, %3.2f to master\n",

// data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);

//Serial.printf("left %d\t%d\t%d\t%d\t%2.3f",

// LF_Dist_mm, LC_Dist_mm, LR_Dist_mm);

//DEBUG!!

//Serial.printf("In VL53L0X_ALL case\n");

//left side

I2C_writeAnything(LF_Dist_mm);

I2C_writeAnything(LC_Dist_mm);

I2C_writeAnything(LR_Dist_mm);

I2C_writeAnything(LeftSteeringVal);

//DEBUG!!

//data_size = 3 * sizeof(uint16_t) + sizeof(float);

//Serial.printf("Sending %d bytes RF/RC/RR/RS vals = %d, %d, %d, %3.2f to master\n",

// data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, LeftSteeringVal);

//Serial.printf(" right %d\t%d\t%d\t%d\t%2.3f\n",

//RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);

//DEBUG!!

//right side

I2C_writeAnything(RF_Dist_mm);

I2C_writeAnything(RC_Dist_mm);

I2C_writeAnything(RR_Dist_mm);

I2C_writeAnything(RightSteeringVal);

//10/31/20 now also reporting rear sensor value

I2C_writeAnything(Rear_Dist_mm);

break;

case VL53L0X_REAR_ONLY:

//DEBUG!!

//data_size = sizeof(uint16_t);

//Serial.printf("Sending %d bytes Rear_Dist_mm = %d to master\n", data_size, Rear_Dist_mm);

//DEBUG!!

I2C_writeAnything(Rear_Dist_mm);

break;

default:

break;

}

digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, LOW);

}

// handle Rx Event (incoming I2C data)

void receiveEvent(int numBytes)

{

//Purpose: capture data request type from Mega I2C master

//Inputs:

// numBytes = int value denoting number of bytes received from master

//Outputs:

// uint8_t request_type filled with request type value from master

//DEBUG!!

//Serial.printf("receiveEvent(%d)\n", numBytes);

//DEBUG!!

I2C_readAnything(request_type);

//DEBUG!!

//Serial.printf("receiveEvent: Got %d from master\n", request_type);

//DEBUG!!

}

bool InitAllSensors()

{

//Purpose: Initialize all seven VL53L0X sensors

//Provenance: 03/22/21

Serial.printf("Initializing VL53L0X Sensor Array Elements\n");

//Put all sensors in reset mode by pulling XSHUT low

digitalWrite(RF_XSHUT_PIN, LOW);

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

digitalWrite(LF_XSHUT_PIN, LOW);

digitalWrite(LC_XSHUT_PIN, LOW);

digitalWrite(LR_XSHUT_PIN, LOW);

//added 10/23/20 for rear sensor

pinMode(Rear_XSHUT_PIN, OUTPUT);

digitalWrite(Rear_XSHUT_PIN, LOW);

//now bring lidar_RF only out of reset and set it's address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(RF_XSHUT_PIN, INPUT);

delay(10);

bool retval = true;

if (!lidar_RF.init())

{

Serial.println("Failed to detect and initialize lidar_RF!");

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_RF.setMeasurementTimingBudget(20000);

lidar_RF.startContinuous();

lidar_RF.setAddress(DEFAULT_VL53L0X_ADDR + 1);

Serial.printf("lidar_RF address is 0x%x\n", lidar_RF.getAddress());

delay(100);

//now bring lidar_RC only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(RC_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RC.init())

{

Serial.println("Failed to detect and initialize lidar_RC!");

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_RC.setMeasurementTimingBudget(20000);

lidar_RC.startContinuous();

lidar_RC.setAddress(DEFAULT_VL53L0X_ADDR + 2);

Serial.printf("lidar_RC address is 0x%x\n", lidar_RC.getAddress());

delay(100);

//now bring lidar_RR only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(RR_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RR.init())

{

Serial.println("Failed to detect and initialize lidar_RR!");

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_RR.setMeasurementTimingBudget(20000);

lidar_RR.startContinuous();

lidar_RR.setAddress(DEFAULT_VL53L0X_ADDR + 3);

Serial.printf("lidar_RR address is 0x%x\n", lidar_RR.getAddress());

delay(100);

//now bring lidar_LF only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(LF_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_LF.init())

{

Serial.println("Failed to detect and initialize lidar_LF!");

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_LF.setMeasurementTimingBudget(20000);

lidar_LF.startContinuous();

lidar_LF.setAddress(DEFAULT_VL53L0X_ADDR + 4);

Serial.printf("lidar_LF address is 0x%x\n", lidar_LF.getAddress());

delay(100);

//now bring lidar_LC only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(LC_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_LC.init())

{

Serial.println("Failed to detect and initialize lidar_LC!");

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_LC.setMeasurementTimingBudget(20000);

lidar_LC.startContinuous();

lidar_LC.setAddress(DEFAULT_VL53L0X_ADDR + 5);

Serial.printf("lidar_LC address is 0x%x\n", lidar_LC.getAddress());

delay(100);

//now bring lidar_LR only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(LR_XSHUT_PIN, INPUT);

if (!lidar_LR.init())

{

Serial.println("Failed to detect and initialize lidar_LR!");

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_LR.setMeasurementTimingBudget(20000);

lidar_LR.startContinuous();

lidar_LR.setAddress(DEFAULT_VL53L0X_ADDR + 6);

Serial.printf("lidar_LR address is 0x%x\n", lidar_LR.getAddress());

delay(100);

//added 10/23/20 for rear sensor

//now bring lidar_Rear only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(Rear_XSHUT_PIN, INPUT);

if (!lidar_Rear.init())

{

Serial.println("Failed to detect and initialize lidar_Rear!");

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_Rear.setMeasurementTimingBudget(20000);

lidar_Rear.startContinuous();

lidar_Rear.setAddress(DEFAULT_VL53L0X_ADDR + 7);

Serial.printf("lidar_Rear address is 0x%x\n", lidar_Rear.getAddress());

return retval;

}

After getting everything set up, I started experimenting. The first thing I tried was momentarily disconnecting one or the other of the two I2C bus connections at the Teensy end. This was interesting in that the affected sensor array results became invalid as soon as the I2C connection was pulled, but resumed valid output as soon as the connection was restored – nice!

Next, I tried momentarily disconnecting the +V line to the arrays. All sensor outputs immediately became invalid, and stayed invalid after +V was restored. In addition, the code in loop() designed to detect one or more VL53L0X sensors at the default address immediately triggered – nice – but the re-initialization code failed to restore valid output.

Here’s the output from a run were the +V line was momentarily disconnected and then reconnected:

Opening port
Port open
Initializing VL53L0X Sensor Array Elements
lidar_RF address is 0x2a
lidar_RC address is 0x2b
lidar_RR address is 0x2c
lidar_LF address is 0x2d
lidar_LC address is 0x2e
lidar_LR address is 0x2f
lidar_Rear address is 0x30
Just after InitAllSensors()
Msec	LFront	LCtr	LRear	RFront	RCtr	RRear	LSteer	RSteer	Rear
6448	373	350	363	544	8191	8190	996
6556	369	344	353	486	8191	8191	1006
6665	371	353	368	475	8191	8190	988
6773	379	352	365	510	512	8190	991
6881	377	346	363	489	8191	8191	1009
6990	368	353	365	541	8191	8191	997
7098	375	350	372	487	8191	8191	989
7206	356	350	366	497	535	8190	983
7315	360	358	364	483	543	8190	997
7423	370	353	363	502	633	8190	1001
7531	373	335	350	465	8191	8190	982
7639	374	349	352	483	497	8191	1017
7748	356	350	381	503	8191	8190	1000
7856	362	350	364	489	585	8191	1013
7964	359	346	380	477	557	8191	979
8073	357	350	361	482	8191	8190	1019
8181	357	347	367	463	8191	8191	1011
8289	374	349	359	459	8191	8190	996
8397	363	348	371	494	8191	8191	1030
8506	365	351	370	557	509	8191	1000
8614	367	345	368	480	445	8191	974
8722	364	354	365	500	8191	8190	987
8831	384	349	356	524	470	8190	1019
8939	376	348	363	539	534	8191	1007
9047	354	349	371	522	475	8191	1020
9156	369	353	362	758	8191	8190	1003
9264	361	347	380	489	8191	8190	981
9372	364	350	367	463	8191	8190	998
9480	366	350	375	505	498	8190	985
9589	382	346	364	445	8191	8190	1004
9697	365	346	373	484	492	8190	996
9805	375	349	353	493	616	8190	1004
9914	371	349	356	538	492	8191	1001
10022	379	346	366	496	503	8191	1016
10130	365	349	354	446	428	8190	1025
10239	381	347	366	474	8191	8191	1008
10347	372	351	362	493	647	8191	1006
10455	366	344	358	487	8191	8191	987
10563	379	346	367	487	566	8191	979
10672	370	348	366	496	8191	8191	975
10780	375	356	360	510	601	8191	1000
10888	370	343	370	497	550	8191	1006
10997	371	352	360	488	8191	8190	982
11105	369	351	353	501	8191	8191	1019
11213	370	345	363	483	8191	8190	1020
11321	372	349	361	503	561	8190	1023
11430	369	351	379	505	453	8191	976
11538	373	347	366	495	8191	8191	988
11646	364	351	372	488	8191	8191	989
11755	365	339	361	470	544	8191	1011
11863	374	358	369	483	8191	8191	1008
11971	367	343	370	490	499	8190	1013
12080	364	348	373	475	491	8191	1006
12188	367	344	363	495	8191	8190	1025
12296	376	349	362	514	580	8191	1035
12404	366	351	365	474	8191	8190	1021
12513	364	350	372	489	8191	8191	1004
12621	373	348	357	494	8191	8190	998
12729	371	352	364	523	8191	8191	1015
12838	365	355	372	500	8191	8190	987
12946	364	357	374	497	8191	8190	998
13054	365	347	364	520	436	8190	990
13163	364	351	381	535	8191	8191	1028
13271	366	345	375	499	8191	8191	1011
13379	358	350	356	482	705	8190	1008
13487	366	350	365	532	590	8191	1008
13596	370	349	355	491	563	8190	1021
13704	364	348	368	517	480	8190	1018
13812	358	348	375	481	8191	8191	1014
13921	363	346	359	503	8191	8191	1004
14029	371	353	363	458	8191	8191	983
14137	376	357	357	565	641	8191	999
14246	360	348	368	548	535	8191	1021
14354	364	347	367	494	601	8191	1002
14462	364	352	372	470	550	8191	991
14570	366	349	366	532	536	8191	1003
14679	374	348	364	493	520	8191	1000
14787	364	345	363	471	8191	8191	1000
14895	364	341	371	503	8191	8190	994
15004	360	349	366	474	605	8191	1030
15112	375	342	356	464	548	8190	986
15220	367	351	364	471	524	8190	1022
15328	365	349	372	491	656	8191	1001
15437	364	347	365	472	8191	8191	993
15545	366	348	365	520	8191	8191	980
15653	374	348	361	527	591	8191	1010
15762	363	352	361	500	609	8190	1002
15870	370	352	370	531	8191	8191	1004
15978	367	353	365	555	8191	8191	987
16087	366	348	365	501	8191	8191	992
16195	366	347	362	481	558	8190	1013
16303	368	352	364	485	8191	8190	1018
16411	372	340	360	459	8003	8191	993
16520	366	357	369	503	561	8191	1009
16628	365	345	364	416	497	8191	986
16736	363	352	362	542	8191	8191	996
16845	373	356	365	455	8191	8190	975
16953	354	342	364	488	551	8191	971
17061	368	349	364	495	8191	8191	983
17170	371	350	367	451	421	8191	1019
17278	367	345	362	465	8191	8190	977
17386	369	351	367	517	604	8190	990
17494	371	342	363	513	8191	8190	999
17603	368	350	365	498	8191	8191	986
17711	362	350	371	509	600	8190	988
17819	366	351	360	515	8191	8190	1002
17928	378	348	364	478	8191	8190	999
18036	367	346	365	436	8191	8191	1035
18144	378	349	356	515	596	8190	1017
18253	365	347	364	491	8191	8191	1018
18361	370	349	367	478	649	8191	1024
18469	370	348	366	443	8191	8190	1002
18577	374	345	366	471	8191	8190	1009
18686	370	355	372	499	8191	8191	1022
18794	362	353	364	480	8191	8191	1003
18902	368	347	365	507	510	8191	1007
19011	373	357	367	486	8191	8190	1029
19119	369	351	362	498	8191	8190	999
19227	360	347	365	445	8191	8190	995
19336	363	351	371	471	557	8190	985
19444	364	354	368	439	707	8190	985
19552	360	354	371	529	8191	8190	994
19660	358	351	373	496	438	8191	992
19769	363	348	366	457	8191	8190	1000
19877	361	358	368	435	689	8190	1005
19985	364	351	371	500	8191	8191	996
20094	368	351	363	478	8191	8190	1029
20202	365	351	370	521	8191	8191	996
20310	373	345	365	514	8191	8191	1009
20418	368	348	367	526	596	8191	959
20527	369	351	351	534	8191	8191	1011
20635	373	346	361	512	8191	8190	989
20743	367	349	367	470	8191	8190	1023
20852	364	344	354	443	586	8191	1027
20960	369	353	375	481	8191	8191	1010
21068	372	354	369	471	461	8190	999
21177	369	353	364	509	8191	8191	977
21285	372	351	366	516	517	8191	1013
21393	370	355	351	515	673	8190	1018
21501	366	351	366	481	8191	8190	1037
21610	374	353	371	547	478	8191	1006
21718	367	354	369	492	8191	8190	991
21826	368	347	359	454	664	8191	982
21935	362	347	370	530	635	8190	997
22043	368	350	368	523	488	8190	984
22151	364	352	363	539	8191	8191	1032
22260	362	348	355	552	8191	8190	995
22368	375	348	357	528	616	8191	996
22476	371	349	357	491	679	8191	1020
22584	370	352	363	539	517	8190	1007
22693	367	346	360	491	8191	8191	1015
22801	366	348	373	524	8191	8190	1003
22909	381	346	371	498	682	8191	1027
23018	373	345	367	528	587	8190	1024
23126	369	341	364	482	8191	8191	1021
23234	372	344	365	532	8191	8190	983
23343	369	352	363	457	8191	8191	1012
23451	370	345	360	448	590	8191	997
23559	364	348	380	523	570	8190	996
23667	378	346	368	509	492	8191	1002
23776	370	348	375	473	8191	8190	1004
23884	371	344	365	457	8191	8190	1001
23992	368	350	375	469	8191	8190	1027
24101	376	353	361	484	8191	8190	1009
24209	373	348	374	529	8191	8190	1015
24317	375	352	368	559	8191	8190	973
24426	369	354	370	541	8191	8191	1003
24534	361	345	366	510	8191	1143	987
24642	364	350	363	420	857	8191	982
24750	367	345	364	496	8191	8190	1021
24859	362	344	370	482	8191	8191	1010
24967	366	346	367	420	503	8191	1005
25075	361	349	372	383	447	8191	981
25184	369	343	371	393	386	8190	1003
25292	367	355	374	360	395	8191	1016
25400	369	342	367	368	392	408	1008
25508	360	352	375	374	376	431	1020
25617	364	355	371	368	352	369	1022
25725	364	346	368	364	322	328	1016
25833	362	348	376	381	320	284	1000
25942	368	343	362	402	331	279	1030
26050	367	352	361	411	333	264	1011
26158	366	349	371	399	364	279	1008
26267	361	356	378	390	367	291	997
26375	365	350	370	373	369	306	1002
26483	372	349	357	367	353	306	980
26591	369	350	371	355	340	300	1002
26700	362	348	372	339	340	297	998
26808	370	353	372	338	325	295	1015
26916	371	351	368	341	331	287	1011
27025	362	344	370	338	325	287	994
27133	369	350	357	334	319	292	1000
27241	371	347	365	328	303	293	972
27349	373	352	365	318	310	293	1013
27458	359	352	374	321	302	283	999
27566	366	347	360	311	308	277	1004
27674	370	348	368	321	303	285	1024
27783	369	349	365	320	315	282	1036
27891	371	352	364	316	307	279	1006
27999	369	346	361	326	295	282	989
28108	367	350	359	312	301	279	1001
28216	373	345	373	315	299	279	1006
28324	368	362	364	306	302	274	989
28432	367	346	365	320	303	284	1025
28541	370	355	363	313	306	277	1003
28649	378	347	369	317	295	280	997
28757	368	348	364	324	307	274	1011
28866	377	349	362	330	300	285	1007
28974	371	352	366	326	307	291	998
29104	65535	65535	65535	65535	65535	65535	65535
29224	65535	65535	65535	65535	65535	65535	65535
29418	65535	65535	65535	65535	65535	65535	65535
Ooops! found a VL53L0X sensor at 29 on Wire1!
Initializing VL53L0X Sensor Array Elements
Failed to detect and initialize lidar_RF!
lidar_RF address is 0x2a
Failed to detect and initialize lidar_RC!
lidar_RC address is 0x2b
Failed to detect and initialize lidar_RR!
lidar_RR address is 0x2c
Failed to detect and initialize lidar_LF!
lidar_LF address is 0x2d
Failed to detect and initialize lidar_LC!
lidar_LC address is 0x2e
Failed to detect and initialize lidar_LR!
lidar_LR address is 0x2f
Failed to detect and initialize lidar_Rear!
lidar_Rear address is 0x30
Ooops! found a VL53L0X sensor at 29 on Wire2!
30190	65535	65535	65535	65535	65535	65535	65535
Ooops! found a VL53L0X sensor at 29 on Wire1!
Initializing VL53L0X Sensor Array Elements
Failed to detect and initialize lidar_RF!
lidar_RF address is 0x2a
Failed to detect and initialize lidar_RC!
lidar_RC address is 0x2b
Failed to detect and initialize lidar_RR!
lidar_RR address is 0x2c
Failed to detect and initialize lidar_LF!
lidar_LF address is 0x2d
Failed to detect and initialize lidar_LC!
lidar_LC address is 0x2e
Failed to detect and initialize lidar_LR!
lidar_LR address is 0x2f
Failed to detect and initialize lidar_Rear!
lidar_Rear address is 0x30
Ooops! found a VL53L0X sensor at 29 on Wire2!
30962	65535	65535	65535	65535	65535	65535	65535
Ooops! found a VL53L0X sensor at 29 on Wire1!
Initializing VL53L0X Sensor Array Elements
Failed to detect and initialize lidar_RF!
lidar_RF address is 0x2a
Failed to detect and initialize lidar_RC!
lidar_RC address is 0x2b
Failed to detect and initialize lidar_RR!
lidar_RR address is 0x2c
Failed to detect and initialize lidar_LF!
lidar_LF address is 0x2d
Failed to detect and initialize lidar_LC!
lidar_LC address is 0x2e
Failed to detect and initialize lidar_LR!
lidar_LR address is 0x2f
Failed to detect and initialize lidar_Rear!
lidar_Rear address is 0x30

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

Opening port

Port open

Initializing VL53L0X Sensor Array Elements

lidar_RF address is 0x2a

lidar_RC address is 0x2b

lidar_RR address is 0x2c

lidar_LF address is 0x2d

lidar_LC address is 0x2e

lidar_LR address is 0x2f

lidar_Rear address is 0x30

Just after InitAllSensors()

Msec LFront LCtr LRear RFront RCtr RRear LSteer RSteer Rear

6448 373 350 363 544 8191 8190 996

6556 369 344 353 486 8191 8191 1006

6665 371 353 368 475 8191 8190 988

6773 379 352 365 510 512 8190 991

6881 377 346 363 489 8191 8191 1009

6990 368 353 365 541 8191 8191 997

7098 375 350 372 487 8191 8191 989

7206 356 350 366 497 535 8190 983

7315 360 358 364 483 543 8190 997

7423 370 353 363 502 633 8190 1001

7531 373 335 350 465 8191 8190 982

7639 374 349 352 483 497 8191 1017

7748 356 350 381 503 8191 8190 1000

7856 362 350 364 489 585 8191 1013

7964 359 346 380 477 557 8191 979

8073 357 350 361 482 8191 8190 1019

8181 357 347 367 463 8191 8191 1011

8289 374 349 359 459 8191 8190 996

8397 363 348 371 494 8191 8191 1030

8506 365 351 370 557 509 8191 1000

8614 367 345 368 480 445 8191 974

8722 364 354 365 500 8191 8190 987

8831 384 349 356 524 470 8190 1019

8939 376 348 363 539 534 8191 1007

9047 354 349 371 522 475 8191 1020

9156 369 353 362 758 8191 8190 1003

9264 361 347 380 489 8191 8190 981

9372 364 350 367 463 8191 8190 998

9480 366 350 375 505 498 8190 985

9589 382 346 364 445 8191 8190 1004

9697 365 346 373 484 492 8190 996

9805 375 349 353 493 616 8190 1004

9914 371 349 356 538 492 8191 1001

10022 379 346 366 496 503 8191 1016

10130 365 349 354 446 428 8190 1025

10239 381 347 366 474 8191 8191 1008

10347 372 351 362 493 647 8191 1006

10455 366 344 358 487 8191 8191 987

10563 379 346 367 487 566 8191 979

10672 370 348 366 496 8191 8191 975

10780 375 356 360 510 601 8191 1000

10888 370 343 370 497 550 8191 1006

10997 371 352 360 488 8191 8190 982

11105 369 351 353 501 8191 8191 1019

11213 370 345 363 483 8191 8190 1020

11321 372 349 361 503 561 8190 1023

11430 369 351 379 505 453 8191 976

11538 373 347 366 495 8191 8191 988

11646 364 351 372 488 8191 8191 989

11755 365 339 361 470 544 8191 1011

11863 374 358 369 483 8191 8191 1008

11971 367 343 370 490 499 8190 1013

12080 364 348 373 475 491 8191 1006

12188 367 344 363 495 8191 8190 1025

12296 376 349 362 514 580 8191 1035

12404 366 351 365 474 8191 8190 1021

12513 364 350 372 489 8191 8191 1004

12621 373 348 357 494 8191 8190 998

12729 371 352 364 523 8191 8191 1015

12838 365 355 372 500 8191 8190 987

12946 364 357 374 497 8191 8190 998

13054 365 347 364 520 436 8190 990

13163 364 351 381 535 8191 8191 1028

13271 366 345 375 499 8191 8191 1011

13379 358 350 356 482 705 8190 1008

13487 366 350 365 532 590 8191 1008

13596 370 349 355 491 563 8190 1021

13704 364 348 368 517 480 8190 1018

13812 358 348 375 481 8191 8191 1014

13921 363 346 359 503 8191 8191 1004

14029 371 353 363 458 8191 8191 983

14137 376 357 357 565 641 8191 999

14246 360 348 368 548 535 8191 1021

14354 364 347 367 494 601 8191 1002

14462 364 352 372 470 550 8191 991

14570 366 349 366 532 536 8191 1003

14679 374 348 364 493 520 8191 1000

14787 364 345 363 471 8191 8191 1000

14895 364 341 371 503 8191 8190 994

15004 360 349 366 474 605 8191 1030

15112 375 342 356 464 548 8190 986

15220 367 351 364 471 524 8190 1022

15328 365 349 372 491 656 8191 1001

15437 364 347 365 472 8191 8191 993

15545 366 348 365 520 8191 8191 980

15653 374 348 361 527 591 8191 1010

15762 363 352 361 500 609 8190 1002

15870 370 352 370 531 8191 8191 1004

15978 367 353 365 555 8191 8191 987

16087 366 348 365 501 8191 8191 992

16195 366 347 362 481 558 8190 1013

16303 368 352 364 485 8191 8190 1018

16411 372 340 360 459 8003 8191 993

16520 366 357 369 503 561 8191 1009

16628 365 345 364 416 497 8191 986

16736 363 352 362 542 8191 8191 996

16845 373 356 365 455 8191 8190 975

16953 354 342 364 488 551 8191 971

17061 368 349 364 495 8191 8191 983

17170 371 350 367 451 421 8191 1019

17278 367 345 362 465 8191 8190 977

17386 369 351 367 517 604 8190 990

17494 371 342 363 513 8191 8190 999

17603 368 350 365 498 8191 8191 986

17711 362 350 371 509 600 8190 988

17819 366 351 360 515 8191 8190 1002

17928 378 348 364 478 8191 8190 999

18036 367 346 365 436 8191 8191 1035

18144 378 349 356 515 596 8190 1017

18253 365 347 364 491 8191 8191 1018

18361 370 349 367 478 649 8191 1024

18469 370 348 366 443 8191 8190 1002

18577 374 345 366 471 8191 8190 1009

18686 370 355 372 499 8191 8191 1022

18794 362 353 364 480 8191 8191 1003

18902 368 347 365 507 510 8191 1007

19011 373 357 367 486 8191 8190 1029

19119 369 351 362 498 8191 8190 999

19227 360 347 365 445 8191 8190 995

19336 363 351 371 471 557 8190 985

19444 364 354 368 439 707 8190 985

19552 360 354 371 529 8191 8190 994

19660 358 351 373 496 438 8191 992

19769 363 348 366 457 8191 8190 1000

19877 361 358 368 435 689 8190 1005

19985 364 351 371 500 8191 8191 996

20094 368 351 363 478 8191 8190 1029

20202 365 351 370 521 8191 8191 996

20310 373 345 365 514 8191 8191 1009

20418 368 348 367 526 596 8191 959

20527 369 351 351 534 8191 8191 1011

20635 373 346 361 512 8191 8190 989

20743 367 349 367 470 8191 8190 1023

20852 364 344 354 443 586 8191 1027

20960 369 353 375 481 8191 8191 1010

21068 372 354 369 471 461 8190 999

21177 369 353 364 509 8191 8191 977

21285 372 351 366 516 517 8191 1013

21393 370 355 351 515 673 8190 1018

21501 366 351 366 481 8191 8190 1037

21610 374 353 371 547 478 8191 1006

21718 367 354 369 492 8191 8190 991

21826 368 347 359 454 664 8191 982

21935 362 347 370 530 635 8190 997

22043 368 350 368 523 488 8190 984

22151 364 352 363 539 8191 8191 1032

22260 362 348 355 552 8191 8190 995

22368 375 348 357 528 616 8191 996

22476 371 349 357 491 679 8191 1020

22584 370 352 363 539 517 8190 1007

22693 367 346 360 491 8191 8191 1015

22801 366 348 373 524 8191 8190 1003

22909 381 346 371 498 682 8191 1027

23018 373 345 367 528 587 8190 1024

23126 369 341 364 482 8191 8191 1021

23234 372 344 365 532 8191 8190 983

23343 369 352 363 457 8191 8191 1012

23451 370 345 360 448 590 8191 997

23559 364 348 380 523 570 8190 996

23667 378 346 368 509 492 8191 1002

23776 370 348 375 473 8191 8190 1004

23884 371 344 365 457 8191 8190 1001

23992 368 350 375 469 8191 8190 1027

24101 376 353 361 484 8191 8190 1009

24209 373 348 374 529 8191 8190 1015

24317 375 352 368 559 8191 8190 973

24426 369 354 370 541 8191 8191 1003

24534 361 345 366 510 8191 1143 987

24642 364 350 363 420 857 8191 982

24750 367 345 364 496 8191 8190 1021

24859 362 344 370 482 8191 8191 1010

24967 366 346 367 420 503 8191 1005

25075 361 349 372 383 447 8191 981

25184 369 343 371 393 386 8190 1003

25292 367 355 374 360 395 8191 1016

25400 369 342 367 368 392 408 1008

25508 360 352 375 374 376 431 1020

25617 364 355 371 368 352 369 1022

25725 364 346 368 364 322 328 1016

25833 362 348 376 381 320 284 1000

25942 368 343 362 402 331 279 1030

26050 367 352 361 411 333 264 1011

26158 366 349 371 399 364 279 1008

26267 361 356 378 390 367 291 997

26375 365 350 370 373 369 306 1002

26483 372 349 357 367 353 306 980

26591 369 350 371 355 340 300 1002

26700 362 348 372 339 340 297 998

26808 370 353 372 338 325 295 1015

26916 371 351 368 341 331 287 1011

27025 362 344 370 338 325 287 994

27133 369 350 357 334 319 292 1000

27241 371 347 365 328 303 293 972

27349 373 352 365 318 310 293 1013

27458 359 352 374 321 302 283 999

27566 366 347 360 311 308 277 1004

27674 370 348 368 321 303 285 1024

27783 369 349 365 320 315 282 1036

27891 371 352 364 316 307 279 1006

27999 369 346 361 326 295 282 989

28108 367 350 359 312 301 279 1001

28216 373 345 373 315 299 279 1006

28324 368 362 364 306 302 274 989

28432 367 346 365 320 303 284 1025

28541 370 355 363 313 306 277 1003

28649 378 347 369 317 295 280 997

28757 368 348 364 324 307 274 1011

28866 377 349 362 330 300 285 1007

28974 371 352 366 326 307 291 998

29104 65535 65535 65535 65535 65535 65535 65535

29224 65535 65535 65535 65535 65535 65535 65535

29418 65535 65535 65535 65535 65535 65535 65535

Ooops! found a VL53L0X sensor at 29 on Wire1!

Initializing VL53L0X Sensor Array Elements

Failed to detect and initialize lidar_RF!

lidar_RF address is 0x2a

Failed to detect and initialize lidar_RC!

lidar_RC address is 0x2b

Failed to detect and initialize lidar_RR!

lidar_RR address is 0x2c

Failed to detect and initialize lidar_LF!

lidar_LF address is 0x2d

Failed to detect and initialize lidar_LC!

lidar_LC address is 0x2e

Failed to detect and initialize lidar_LR!

lidar_LR address is 0x2f

Failed to detect and initialize lidar_Rear!

lidar_Rear address is 0x30

Ooops! found a VL53L0X sensor at 29 on Wire2!

30190 65535 65535 65535 65535 65535 65535 65535

Ooops! found a VL53L0X sensor at 29 on Wire1!

Initializing VL53L0X Sensor Array Elements

Failed to detect and initialize lidar_RF!

lidar_RF address is 0x2a

Failed to detect and initialize lidar_RC!

lidar_RC address is 0x2b

Failed to detect and initialize lidar_RR!

lidar_RR address is 0x2c

Failed to detect and initialize lidar_LF!

lidar_LF address is 0x2d

Failed to detect and initialize lidar_LC!

lidar_LC address is 0x2e

Failed to detect and initialize lidar_LR!

lidar_LR address is 0x2f

Failed to detect and initialize lidar_Rear!

lidar_Rear address is 0x30

Ooops! found a VL53L0X sensor at 29 on Wire2!

30962 65535 65535 65535 65535 65535 65535 65535

Ooops! found a VL53L0X sensor at 29 on Wire1!

Initializing VL53L0X Sensor Array Elements

Failed to detect and initialize lidar_RF!

lidar_RF address is 0x2a

Failed to detect and initialize lidar_RC!

lidar_RC address is 0x2b

Failed to detect and initialize lidar_RR!

lidar_RR address is 0x2c

Failed to detect and initialize lidar_LF!

lidar_LF address is 0x2d

Failed to detect and initialize lidar_LC!

lidar_LC address is 0x2e

Failed to detect and initialize lidar_LR!

lidar_LR address is 0x2f

Failed to detect and initialize lidar_Rear!

lidar_Rear address is 0x30

28 March 2021 Update:

I have been continuing to work on the ‘re-initialize after power interruption’ idea for my VL53L0X sensor array, and while I haven’t come up with a very good solution, I have learned some things:

The VL53L0X reverts to it’s default (0x29) I2C address immediately when power is removed, but will actually respond to a 0x29 query even after power has been removed. Apparently, the module can draw enough current from the I2C connections to continue to operate (albeit at the default address).
A ‘powered down’ (power disconnected, but I2C leads still connected) will also respond to distance measurement requests, but will return a nonsense number (65535).
In order to get a VL53L0X sensor to stop responding, the power lead must be physically grounded – just disconnecting isn’t enough.
If I use a digital output to power the array, I can turn the power on and off programmatically. Using this tool:
- Turning power OFF causes distance measurements to return ‘65535’ (because this is the same as connecting the power lead to GND). However, this isn’t the normal failure mode; the normal failure mode for power disconnections is an open-circuit – not GND
- Turning power back ON again causes the system to detect that one or more VL53L0X units has appeared at the default I2C address. This probably means I could successfully re-initialize all modules.

So, it looks like I can’t really protect against an arbitrary length power disconnect scenario, as I can’t tell when the power has been restored. All I can do is have the program detect the ‘65535’ distance value, wait for some time (a few seconds?) and then try to re-initialize the sensors. This is, unfortunately, a one-shot deal, as if power hasn’t been restored when the re-init procedure is executed, the initialization code hangs up permanently.

Stay tuned!

Frank

Wall Tracking Trials Using Office ‘Sandbox’ Part II

Wall-E2 Charging Station Update

20 January 2021 Update:

After getting everything running, I set up an experiment with a spare IR sensor mounted about 20cm away from the IR flashlight emitter. As shown below, the scope trace in the background is from the IR transistor on the left, in response to the 520Hz square wave transmitted over the IR path from the charging station. Once I got it set up, I let it run most of the afternoon and overnight. The next morning after almost 24 hours, the setup was chugging along beautifully – YAY!

Scope trace shows IR signal received by IR transistor on left. IR Flashlight and charging station electronics shown on right

28 December 2021 Update:

In the process of bringing up my new Wall-E3 robot, I was testing the IR homing module, and discovered that it wasn’t acting correctly – the values being read from the IR homing module were basically zero, even with the charging station IR transmitter less than one meter away. Troubleshooting, I found that the IR transmitter wasn’t transmitting correctly, which I finally tracked down to a (probably) misplaced connection to the Teensy 3.2 waveform generator. However, in the process of doing that, I also discovered that the driving waveform to the IR emitter had a big spike on one transition, which if not really a system problem, sure looked unprofessional.

So, I started looking at the IR transmit module schematic, and decided it could use some TLC. One problem that leapt out at me was the 01/17/21 mod to force the IR transmitter OFF if the connection to the Teensy was lost. This mod involves a 100K resistor pullup to +12V, which, if the 2N3904 transistor base failed ‘OPEN’ would mean putting +12V into the Teensy – probably not a good thing! So, I changed the circuit to route the 100K pullup to +5V instead of +12V. This still works to force the 2N3904 output LOW, which in turn would force the IRF520 OFF. Teensy digital I/O pins are all 5V tolerant, so this is a much better deal.

Chasing around a bit regarding the big spike on the leading edge of the IR transmit pulse, I found that a 0.01 uF cap on the gate lead of the IRF520 did a nice job of killing the big spike, as shown in the photos below:

With these two modifications, the IR transmit block looks pretty solid. Here’s the updated schematic:

The way that I implemented the IR transmit hardware on two parallel terminal strips looks kind of ugly, so I decided to replace that with a perfboard setup and not think of barfing every time I look at it. Here’s the ‘before’

and here is the ‘after’

While I was in the neighborhood, I also decided to modify the IR Beacon transmit code to force all unused Teensy GPIO pins to a known HIGH or LOW state in setup(). This is something I learned about just recently in the Teensy forum, and it sounds like a good idea. I think I’ll write a ‘InitAllGPIOPins()’ function that sets ALL pins to a known state, and follow it with just the necessary changes for functionality. From now on, I plan to do this with all my Teensy projects.

Stay tuned,

Frank

Solving the Teensy VL53L0X Array Controller Reset Problem

Wall Tracking Trials Using Office ‘Sandbox’ Part I

1 Reply

Posted 12 November 2020

Back in October I added a TIMER5 timer interrupt to my autonomous wall-following robot (WAll-E2) code to manage sensor updates. Since then I have made the timer interrupt the sole timing source for all sensor and tracking updates, and upped the update rate from 5Hz to 10Hz. In addition, I’ve been making some improvements to Wall-E2’s obstacle detection/response abilities, and this post describes the results of these enhancements.

Wall-E2’s job is to autonomously track walls forever. This implies the ability not only track walls, but to deal with obstacles as they occur, and recharge its batteries at one or more provided charging stations as needed. Wall-tracking per se has been the subject of several previous posts, and is now reasonably well managed using the ‘find parallel’ technique described here. This post deals with the effort to detect and respond to obstacles as they occur. Here’s a recent run in my office ‘sandbox’

Initializing Left & Right Distance Arrays...
Left	Right
340	8190
329	8190
330	8190
In RotateToParallelOrientation(Left)
Parallel Orientation Achieved at 3453 with SteeringVal = 0.01

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	LFront	LCtr	LRear	Front	FrontVar	Rear	Steer	Output	SetPt	LSpd	RSpd
3464	326	334	316	175	 4474.00	508	0.01	34.93	-0.30	40	109
3473	331	341	323	175	 4474.00	508	-0.01	34.93	-0.30	40	109
3554	329	342	326	172	 4286.29	8190	0.03	34.93	-0.30	40	109
3651	326	354	325	181	 4164.85	1192	-0.01	24.80	-0.30	50	99
3750	329	354	349	127	 4520.47	1114	-0.20	24.80	-0.30	50	99
3850	307	387	336	102	 5186.04	1197	-0.29	-75.00	-0.30	150	0
3946	306	342	361	96	 5933.30	1156	-0.55	-75.00	-0.30	150	0
4043	293	325	335	105	 6564.30	1272	-0.29	-2.29	-0.25	77	72
4141	282	310	305	139	 6862.29	8190	-0.23	-2.29	-0.10	77	72
4235	272	296	287	174	 6912.94	914	-0.19	8.02	0.04	66	83
4330	267	284	285	172	 6889.58	920	-0.18	8.02	0.16	66	83
4427	260	283	267	165	 6772.68	967	-0.06	16.33	0.17	58	91
4521	249	286	270	147	 6565.69	1069	-0.21	16.33	0.14	58	91
4620	248	277	268	134	 6196.07	1056	-0.20	-75.00	0.23	150	0
4717	238	261	248	133	 5544.52	1166	-0.10	-75.00	0.30	150	0
4812	230	243	249	126	 4582.67	1046	-0.19	-48.43	0.30	123	26
4910	220	243	236	122	 3231.83	974	-0.16	-48.43	0.30	123	26
5006	218	232	213	121	 1415.66	1029	0.05	43.82	0.30	31	118
5105	218	246	205	119	 1363.33	1169	0.13	43.82	0.30	31	118
5200	215	237	221	115	 1303.73	1136	-0.06	-73.26	0.30	148	1
5300	217	227	218	113	 1215.34	1190	-0.01	-21.19	0.30	96	53
5396	219	235	205	110	 1098.33	1228	0.14	-21.19	0.30	96	53
5491	228	244	215	106	  974.86	8190	0.19	43.48	0.30	31	118
5587	239	260	218	103	  868.73	8190	0.21	43.48	0.30	31	118
5688	244	254	235	97	  807.43	8190	0.09	-57.15	0.30	132	17
5783	253	268	246	94	  766.78	8190	0.07	-57.15	0.30	132	17
5882	260	260	246	92	  731.99	8190	0.14	-7.63	0.30	82	67
5982	282	278	257	88	  697.85	8190	0.25	-7.63	0.22	82	67
6081	287	279	256	85	  608.15	8190	0.30	50.64	0.21	24	125
6178	294	297	276	80	  664.40	8190	0.18	50.64	0.03	24	125
6277	316	305	287	77	  719.44	8190	0.11	-46.88	-0.05	121	28
6373	311	315	289	74	  774.14	8190	0.22	-46.88	-0.15	121	28
6471	311	334	292	73	  840.70	8190	0.25	75.00	-0.30	0	150
6569	327	344	311	68	  894.32	8190	0.16	75.00	-0.30	0	150
6670	340	348	308	62	  816.19	8190	0.32	75.00	-0.30	0	150
6772	342	360	333	54	  737.65	8190	0.09	-31.20	-0.30	106	43
6868	354	350	333	53	  657.72	8190	0.21	-31.20	-0.30	106	43
6967	338	358	326	51	  634.07	8190	0.12	51.06	-0.30	23	126
7063	346	368	333	50	  636.22	8190	0.13	51.06	-0.30	23	126
7161	334	346	345	44	  638.78	8190	-0.11	-49.37	-0.30	124	25
7260	334	371	333	39	  658.55	8190	0.01	-49.37	-0.30	124	25
7360	336	350	330	32	  693.11	8190	0.06	75.00	-0.30	0	150
7457	332	357	319	25	  732.13	8190	0.13	75.00	-0.30	0	150
7556	344	355	332	23	  757.01	8191	0.12	62.97	-0.30	12	137
bObstacleAhead TRUE in TRACKLEFT at 7656 with frontdist 18, variance = 787.12
In SpinTurn(CW, 90.00)
Msec	Hdg	pHdg	Rate	TRate	Spd
7751	6.06	1.38	52.1	20.0	87
7848	1.23	6.06	51.0	20.0	50
7947	-5.41	1.23	68.5	20.0	50
8045	-12.14	-5.41	70.7	20.0	50
8144	-17.56	-12.14	55.5	20.0	50
8244	-21.25	-17.56	38.0	20.0	50
8341	-23.40	-21.25	22.6	20.0	50
8438	-24.34	-23.40	9.9	20.0	90
8537	-25.11	-24.34	8.0	20.0	130
8636	-28.17	-25.11	31.4	20.0	90
8735	-35.01	-28.17	71.1	20.0	50
8835	-41.34	-35.01	65.0	20.0	50
8935	-44.53	-41.34	32.6	20.0	50
9036	-46.09	-44.53	15.8	20.0	90
9135	-47.11	-46.09	10.6	20.0	130
9233	-50.33	-47.11	33.3	20.0	90
9333	-56.48	-50.33	63.0	20.0	50
Slowing down to 10.00 deg/sec at -56.48 deg
9430	-62.64	-56.48	65.1	10.0	50
9529	-65.95	-62.64	34.4	10.0	50
9626	-66.79	-65.95	8.7	10.0	90
9723	-67.14	-66.79	3.7	10.0	130
9823	-68.64	-67.14	15.5	10.0	90
9919	-72.84	-68.64	44.8	10.0	50
10018	-76.92	-72.84	42.1	10.0	50
10116	-79.19	-76.92	23.8	10.0	50
10213	-79.30	-79.19	1.2	10.0	90
10313	-79.37	-79.30	0.7	10.0	130
10414	-80.87	-79.37	15.3	10.0	90
10417: Exiting SpinTurn() at -80.87 deg
In RotateToParallelOrientation(Left)
Parallel Orientation Achieved at 11293 with SteeringVal = 0.00

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	LFront	LCtr	LRear	Front	FrontVar	Rear	Steer	Output	SetPt	LSpd	RSpd
11304	49	69	49	101	  808.15	574	0.00	-63.93	0.30	138	11
11313	45	66	46	101	  808.15	574	-0.01	-63.93	0.30	138	11
11395	44	64	47	109	  891.88	627	-0.03	-63.93	0.30	138	11
11492	43	65	43	162	 1291.60	673	0.00	-28.83	0.30	103	46
11591	47	100	44	216	 2143.29	728	0.03	-28.83	0.30	103	46
11689	47	70	81	94	 2055.42	758	-0.34	-75.00	0.30	150	0
11786	52	71	45	69	 1960.80	749	0.07	-75.00	0.30	150	0
11882	56	75	45	67	 1851.13	701	0.11	75.00	0.30	0	150
11979	63	76	43	63	 1762.12	489	0.20	75.00	0.30	0	150
12079	62	84	56	58	 1678.46	534	0.06	-41.04	0.30	116	33
12176	73	89	71	54	 1594.01	745	0.02	-41.04	0.30	116	33
12275	79	90	73	50	 1519.21	790	0.06	-26.76	0.30	101	48
12372	85	97	74	47	 1480.52	774	0.11	-26.76	0.30	101	48
12473	95	109	78	42	 1477.60	619	0.17	16.85	0.30	58	91
12569	108	113	87	35	 1515.29	570	0.21	16.85	0.30	58	91
12666	113	120	98	33	 1561.57	639	0.15	-24.60	0.30	99	50
Hit Wall Offset Limit (30 cm) at 1769472 with frontdist = 29, Setpoint = 0.00
In StepTurn(Tracking Left)
In SpinTurn(CW, 90.00)
Msec	Hdg	pHdg	Rate	TRate	Spd
12862	-109.12	-107.73	15.9	20.0	167
12961	-116.70	-109.12	78.1	20.0	127
13061	-127.75	-116.70	113.9	20.0	87
13158	-142.57	-127.75	155.6	20.0	50
13257	-155.69	-142.57	136.5	20.0	50
13355	-165.76	-155.69	106.5	20.0	50
Slowing down to 10.00 deg/sec at -165.76 deg
13453	-172.39	-165.76	69.2	10.0	50
13551	-176.13	-172.39	38.6	10.0	50
13650	-176.98	-176.13	8.9	10.0	90
13747	-177.02	-176.98	0.4	10.0	130
13845	-178.80	-177.02	18.5	10.0	90
13946	175.39	-178.80	3604.6	10.0	50
14041	169.71	175.39	61.9	10.0	50
14043: Exiting SpinTurn() at 169.71 deg
In RotateToParallelOrientation(Left)
Parallel Orientation Achieved at 14687 with SteeringVal = 0.01

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	LFront	LCtr	LRear	Front	FrontVar	Rear	Steer	Output	SetPt	LSpd	RSpd
14698	191	208	190	251	10269.92	289	0.01	-75.00	0.30	150	0
14706	196	205	195	251	10269.92	289	-0.06	-75.00	0.30	150	0
14788	198	213	196	246	10313.22	355	0.02	-75.00	0.30	150	0
14884	207	219	198	240	10143.41	402	0.09	-2.10	0.30	77	72
14983	210	226	197	232	 9731.76	451	0.13	-2.10	0.30	77	72
15078	216	228	209	231	 9169.12	488	0.07	-34.79	0.30	109	40
15174	223	240	214	226	 8389.68	547	0.09	-34.79	0.30	109	40
15270	225	243	215	226	 7483.51	576	0.10	-17.39	0.30	92	57
15366	223	244	229	224	 6445.87	624	-0.06	-17.39	0.30	92	57
15462	228	245	232	221	 5279.39	657	-0.01	-71.32	0.30	146	3
15558	234	254	229	217	 4146.39	705	0.05	-71.32	0.30	146	3
15655	248	260	220	218	 3116.80	748	0.28	75.00	0.30	0	150
15749	272	266	238	218	 2207.54	794	0.34	75.00	0.30	0	150
15843	282	288	248	211	 1580.34	839	0.30	17.16	0.12	57	92
15938	293	287	264	200	 1231.91	877	0.29	17.16	0.13	57	92
16038	301	303	266	194	  947.68	895	0.27	15.06	-0.03	59	90
16137	304	307	281	192	  667.96	916	0.23	15.06	-0.07	59	90
16232	305	320	297	187	  675.37	984	0.08	-34.10	-0.20	109	40
16329	321	327	294	183	  719.94	1012	0.27	-34.10	-0.27	109	40
16429	326	344	301	181	  748.10	1030	0.25	75.00	-0.30	0	150
16525	328	345	318	175	  773.36	1061	0.10	75.00	-0.30	0	150
16623	340	350	323	171	  785.28	1139	0.17	20.96	-0.30	54	95
16719	344	351	327	165	  797.68	1142	0.17	20.96	-0.30	54	95
16820	355	364	357	161	  809.43	1160	-0.01	-26.17	-0.30	101	48
16914	356	372	351	157	  816.35	1215	0.05	-26.17	-0.30	101	48
17007	371	376	338	153	  826.46	1232	0.33	75.00	-0.30	0	150
17104	365	378	350	151	  828.07	1275	0.15	75.00	-0.30	0	150
17200	362	363	359	146	  837.66	8190	0.03	-55.29	-0.30	130	19
17295	360	371	353	143	  851.33	8190	0.07	-55.29	-0.30	130	19
17392	361	387	354	136	  892.80	8190	0.07	51.82	-0.30	23	126
17488	364	368	354	134	  916.39	8190	0.10	51.82	-0.30	23	126
17584	352	380	353	129	  948.67	8190	-0.01	8.69	-0.30	66	83
17680	349	368	360	128	  954.63	8190	-0.11	8.69	-0.30	66	83
17776	370	381	356	126	  945.54	8190	0.06	61.77	-0.30	13	136
17872	362	375	362	123	  929.23	8190	0.00	61.77	-0.30	13	136
17972	347	377	363	122	  903.09	8190	-0.04	1.55	-0.30	73	76
18070	348	395	372	122	  843.49	8190	-0.24	1.55	-0.30	73	76
18167	352	370	362	120	  760.07	8190	-0.17	-20.06	-0.30	95	54
18264	332	368	353	115	  694.64	8190	-0.21	-20.06	-0.30	95	54
18362	312	336	339	111	  661.03	8190	-0.20	7.24	-0.30	67	82
18460	297	323	327	105	  644.80	8190	-0.30	7.24	-0.23	67	82
18560	283	312	313	100	  627.13	8190	-0.28	-34.24	-0.12	109	40
18655	285	302	299	96	  615.78	8190	-0.14	-34.24	-0.02	109	40
18754	264	298	287	90	  613.99	8190	-0.19	11.13	0.02	63	86
18850	259	276	255	86	  606.39	8190	0.04	11.13	0.24	63	86
18950	252	284	259	84	  601.15	8190	-0.09	9.38	0.16	65	84
19046	240	280	260	81	  595.10	8190	-0.20	9.38	0.20	65	84
19144	238	252	248	78	  595.94	8190	-0.10	-39.82	0.30	114	35
19240	217	253	242	76	  593.76	8190	-0.13	-39.82	0.30	114	35
19338	218	246	230	71	  598.99	8190	-0.09	-33.99	0.30	108	41
19438	216	236	221	67	  608.08	8190	-0.10	-42.19	0.30	117	32
19535	205	225	214	61	  621.04	8190	-0.09	-42.19	0.30	117	32
19634	208	222	203	55	  647.83	8190	0.06	24.09	0.30	50	99
19733	208	221	203	50	  677.59	8190	0.05	24.09	0.30	50	99
19832	210	230	201	48	  711.95	8190	0.18	23.73	0.30	51	98
19932	224	232	214	45	  739.29	8190	0.10	-44.87	0.30	119	30
20032	224	231	213	40	  776.88	8190	0.10	-21.47	0.30	96	53
20130	228	235	210	37	  803.67	8190	0.18	-21.47	0.30	96	53
20231	240	244	216	32	  829.84	8190	0.15	-1.92	0.30	76	73
Hit Wall Offset Limit 20330 at 1835008 with frontdist = -26214, Setpoint = 0.00
In StepTurn(Tracking Left)
In SpinTurn(CW, 90.00)
Msec	Hdg	pHdg	Rate	TRate	Spd
20427	170.01	171.69	18.9	20.0	167
20528	162.73	170.01	74.4	20.0	127
20628	149.44	162.73	136.8	20.0	87
20724	133.31	149.44	171.6	20.0	50
20824	118.14	133.31	156.0	20.0	50
20921	107.84	118.14	108.3	20.0	50
Slowing down to 10.00 deg/sec at 107.84 deg
21018	100.75	107.84	75.0	10.0	50
21114	96.88	100.75	41.2	10.0	50
21211	95.61	96.88	13.7	10.0	50
21304	95.40	95.61	2.2	10.0	90
21401	95.27	95.40	1.4	10.0	130
21500	92.77	95.27	26.0	10.0	90
21595	86.00	92.77	73.4	10.0	50
21597: Exiting SpinTurn() at 86.00 deg
In RotateToParallelOrientation(Left)
Parallel Orientation Achieved at 21983 with SteeringVal = -0.01

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	LFront	LCtr	LRear	Front	FrontVar	Rear	Steer	Output	SetPt	LSpd	RSpd
21994	161	185	162	108	 5323.48	265	-0.01	-75.00	0.30	150	0
22002	164	182	160	108	 5323.48	265	0.05	-75.00	0.30	150	0
22086	166	184	159	78	 5270.27	295	0.07	-75.00	0.30	150	0
22184	175	185	165	58	 5243.36	352	0.10	9.55	0.30	65	84
22280	183	190	172	175	 5373.00	388	0.11	9.55	0.30	65	84
22375	197	204	176	91	 5247.15	436	0.21	20.28	0.30	54	95
22470	197	215	182	108	 5081.33	467	0.15	20.28	0.30	54	95
22564	207	226	189	151	 4952.56	518	0.18	-25.08	0.30	100	49
22663	218	233	204	52	 4842.48	555	0.14	-25.08	0.30	100	49
22761	236	245	213	141	 4580.68	601	0.18	-16.44	0.30	91	58
bObstacleAhead TRUE in TRACKLEFT at 22858 with frontdist 13, variance = 4694.59
In SpinTurn(CW, 90.00)
Msec	Hdg	pHdg	Rate	TRate	Spd
22952	71.13	73.50	26.9	20.0	87
23052	63.70	71.13	76.5	20.0	50
23147	54.57	63.70	98.3	20.0	50
23246	48.04	54.57	67.2	20.0	50
23343	43.69	48.04	46.2	20.0	50
23441	40.42	43.69	34.1	20.0	50
23540	38.54	40.42	19.5	20.0	90
23638	37.63	38.54	9.7	20.0	130
23734	34.63	37.63	31.7	20.0	90
23833	32.15	34.63	25.5	20.0	50
23931	30.65	32.15	15.7	20.0	90
24031	30.52	30.65	1.4	20.0	130
24127	29.88	30.52	6.8	20.0	170
24225	28.04	29.88	19.3	20.0	210
24324	24.16	28.04	40.3	20.0	170
24419	20.53	24.16	39.3	20.0	130
24515	11.17	20.53	98.9	20.0	90
Slowing down to 10.00 deg/sec at 11.17 deg
24611	0.33	11.17	115.7	10.0	50
24711	-8.59	0.33	92.5	10.0	50
24713: Exiting SpinTurn() at -8.59 deg
In RotateToParallelOrientation(Left)
Parallel Orientation Achieved at 27962 with SteeringVal = -79.86

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	LFront	LCtr	LRear	Front	FrontVar	Rear	Steer	Output	SetPt	LSpd	RSpd
27973	313	8191	8190	230	   14.32	369	-78.77	-75.00	-0.30	150	0
bStuck TRUE in TRACKLEFT at 27981 with frontdist 230, variance =  14.32
---- ExecuteStuckRecoveryManeuver
Using front distance for backup with front/reardist  = 230/36
28059	231	369
28153	248	350
28249	250	327
28346	254	282
28443	255	232
finished backup with frontdist/reardist = 28444	 255/23
In SpinTurn(CW, 90.00)
Msec	Hdg	pHdg	Rate	TRate	Spd
30450	20.77	20.77	0.0	20.0	167
30543	19.58	20.77	13.1	20.0	207
30640	9.56	19.58	106.9	20.0	167
30736	-2.24	9.56	125.2	20.0	127
30830	-14.60	-2.24	134.4	20.0	87
30922	-22.53	-14.60	88.6	20.0	50
31019	-33.56	-22.53	118.1	20.0	50
Slowing down to 10.00 deg/sec at -33.56 deg
31117	-43.11	-33.56	99.6	10.0	50
31214	-49.46	-43.11	66.5	10.0	50
31310	-52.74	-49.46	35.0	10.0	50
31409	-53.79	-52.74	10.8	10.0	50
31510	-53.85	-53.79	0.6	10.0	90
31607	-54.34	-53.85	5.2	10.0	130
31704	-56.91	-54.34	27.5	10.0	90
31802	-62.72	-56.91	60.3	10.0	50
31805: Exiting SpinTurn() at -62.72 deg
Using front distance for fwd travel with front/reardist  = 62/34
33807	62	340
33862	61	329
33959	59	346
34058	57	354
34157	52	377
34254	49	434
34354	41	453
finished forward travel with frontdist/reardist = 41/45
In SpinTurn(CCW, 90.00)
Msec	Hdg	pHdg	Rate	TRate	Spd
36356	-64.92	-64.92	0.0	20.0	167
36419	-64.63	-64.92	4.8	20.0	207
36517	-56.67	-64.63	83.0	20.0	167
36616	-41.04	-56.67	163.3	20.0	127
36712	-25.33	-41.04	166.6	20.0	87
36808	-8.73	-25.33	177.6	20.0	50
Slowing down to 10.00 deg/sec at -8.73 deg
36902	7.93	-8.73	182.0	10.0	50
36998	19.87	7.93	128.6	10.0	50
37000: Exiting SpinTurn() at 19.87 deg
I received: 63
ENTERING COMMAND MODE:
0 = 180 deg CCW Turn
1 = 180 deg CW Turn
A = Back to Auto Mode
S = Stop
F = Forward
R = Reverse

       Faster
	8
Left 4	5  6 Right
	2
       Slower

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

Initializing Left & Right Distance Arrays...

Left Right

340 8190

329 8190

330 8190

In RotateToParallelOrientation(Left)

Parallel Orientation Achieved at 3453 with SteeringVal = 0.01

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec LFront LCtr LRear Front FrontVar Rear Steer Output SetPt LSpd RSpd

3464 326 334 316 175 4474.00 508 0.01 34.93 -0.30 40 109

3473 331 341 323 175 4474.00 508 -0.01 34.93 -0.30 40 109

3554 329 342 326 172 4286.29 8190 0.03 34.93 -0.30 40 109

3651 326 354 325 181 4164.85 1192 -0.01 24.80 -0.30 50 99

3750 329 354 349 127 4520.47 1114 -0.20 24.80 -0.30 50 99

3850 307 387 336 102 5186.04 1197 -0.29 -75.00 -0.30 150 0

3946 306 342 361 96 5933.30 1156 -0.55 -75.00 -0.30 150 0

4043 293 325 335 105 6564.30 1272 -0.29 -2.29 -0.25 77 72

4141 282 310 305 139 6862.29 8190 -0.23 -2.29 -0.10 77 72

4235 272 296 287 174 6912.94 914 -0.19 8.02 0.04 66 83

4330 267 284 285 172 6889.58 920 -0.18 8.02 0.16 66 83

4427 260 283 267 165 6772.68 967 -0.06 16.33 0.17 58 91

4521 249 286 270 147 6565.69 1069 -0.21 16.33 0.14 58 91

4620 248 277 268 134 6196.07 1056 -0.20 -75.00 0.23 150 0

4717 238 261 248 133 5544.52 1166 -0.10 -75.00 0.30 150 0

4812 230 243 249 126 4582.67 1046 -0.19 -48.43 0.30 123 26

4910 220 243 236 122 3231.83 974 -0.16 -48.43 0.30 123 26

5006 218 232 213 121 1415.66 1029 0.05 43.82 0.30 31 118

5105 218 246 205 119 1363.33 1169 0.13 43.82 0.30 31 118

5200 215 237 221 115 1303.73 1136 -0.06 -73.26 0.30 148 1

5300 217 227 218 113 1215.34 1190 -0.01 -21.19 0.30 96 53

5396 219 235 205 110 1098.33 1228 0.14 -21.19 0.30 96 53

5491 228 244 215 106 974.86 8190 0.19 43.48 0.30 31 118

5587 239 260 218 103 868.73 8190 0.21 43.48 0.30 31 118

5688 244 254 235 97 807.43 8190 0.09 -57.15 0.30 132 17

5783 253 268 246 94 766.78 8190 0.07 -57.15 0.30 132 17

5882 260 260 246 92 731.99 8190 0.14 -7.63 0.30 82 67

5982 282 278 257 88 697.85 8190 0.25 -7.63 0.22 82 67

6081 287 279 256 85 608.15 8190 0.30 50.64 0.21 24 125

6178 294 297 276 80 664.40 8190 0.18 50.64 0.03 24 125

6277 316 305 287 77 719.44 8190 0.11 -46.88 -0.05 121 28

6373 311 315 289 74 774.14 8190 0.22 -46.88 -0.15 121 28

6471 311 334 292 73 840.70 8190 0.25 75.00 -0.30 0 150

6569 327 344 311 68 894.32 8190 0.16 75.00 -0.30 0 150

6670 340 348 308 62 816.19 8190 0.32 75.00 -0.30 0 150

6772 342 360 333 54 737.65 8190 0.09 -31.20 -0.30 106 43

6868 354 350 333 53 657.72 8190 0.21 -31.20 -0.30 106 43

6967 338 358 326 51 634.07 8190 0.12 51.06 -0.30 23 126

7063 346 368 333 50 636.22 8190 0.13 51.06 -0.30 23 126

7161 334 346 345 44 638.78 8190 -0.11 -49.37 -0.30 124 25

7260 334 371 333 39 658.55 8190 0.01 -49.37 -0.30 124 25

7360 336 350 330 32 693.11 8190 0.06 75.00 -0.30 0 150

7457 332 357 319 25 732.13 8190 0.13 75.00 -0.30 0 150

7556 344 355 332 23 757.01 8191 0.12 62.97 -0.30 12 137

bObstacleAhead TRUE in TRACKLEFT at 7656 with frontdist 18, variance = 787.12

In SpinTurn(CW, 90.00)

Msec Hdg pHdg Rate TRate Spd

7751 6.06 1.38 52.1 20.0 87

7848 1.23 6.06 51.0 20.0 50

7947 -5.41 1.23 68.5 20.0 50

8045 -12.14 -5.41 70.7 20.0 50

8144 -17.56 -12.14 55.5 20.0 50

8244 -21.25 -17.56 38.0 20.0 50

8341 -23.40 -21.25 22.6 20.0 50

8438 -24.34 -23.40 9.9 20.0 90

8537 -25.11 -24.34 8.0 20.0 130

8636 -28.17 -25.11 31.4 20.0 90

8735 -35.01 -28.17 71.1 20.0 50

8835 -41.34 -35.01 65.0 20.0 50

8935 -44.53 -41.34 32.6 20.0 50

9036 -46.09 -44.53 15.8 20.0 90

9135 -47.11 -46.09 10.6 20.0 130

9233 -50.33 -47.11 33.3 20.0 90

9333 -56.48 -50.33 63.0 20.0 50

Slowing down to 10.00 deg/sec at -56.48 deg

9430 -62.64 -56.48 65.1 10.0 50

9529 -65.95 -62.64 34.4 10.0 50

9626 -66.79 -65.95 8.7 10.0 90

9723 -67.14 -66.79 3.7 10.0 130

9823 -68.64 -67.14 15.5 10.0 90

9919 -72.84 -68.64 44.8 10.0 50

10018 -76.92 -72.84 42.1 10.0 50

10116 -79.19 -76.92 23.8 10.0 50

10213 -79.30 -79.19 1.2 10.0 90

10313 -79.37 -79.30 0.7 10.0 130

10414 -80.87 -79.37 15.3 10.0 90

10417: Exiting SpinTurn() at -80.87 deg

In RotateToParallelOrientation(Left)

Parallel Orientation Achieved at 11293 with SteeringVal = 0.00

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec LFront LCtr LRear Front FrontVar Rear Steer Output SetPt LSpd RSpd

11304 49 69 49 101 808.15 574 0.00 -63.93 0.30 138 11

11313 45 66 46 101 808.15 574 -0.01 -63.93 0.30 138 11

11395 44 64 47 109 891.88 627 -0.03 -63.93 0.30 138 11

11492 43 65 43 162 1291.60 673 0.00 -28.83 0.30 103 46

11591 47 100 44 216 2143.29 728 0.03 -28.83 0.30 103 46

11689 47 70 81 94 2055.42 758 -0.34 -75.00 0.30 150 0

11786 52 71 45 69 1960.80 749 0.07 -75.00 0.30 150 0

11882 56 75 45 67 1851.13 701 0.11 75.00 0.30 0 150

11979 63 76 43 63 1762.12 489 0.20 75.00 0.30 0 150

12079 62 84 56 58 1678.46 534 0.06 -41.04 0.30 116 33

12176 73 89 71 54 1594.01 745 0.02 -41.04 0.30 116 33

12275 79 90 73 50 1519.21 790 0.06 -26.76 0.30 101 48

12372 85 97 74 47 1480.52 774 0.11 -26.76 0.30 101 48

12473 95 109 78 42 1477.60 619 0.17 16.85 0.30 58 91

12569 108 113 87 35 1515.29 570 0.21 16.85 0.30 58 91

12666 113 120 98 33 1561.57 639 0.15 -24.60 0.30 99 50

Hit Wall Offset Limit (30 cm) at 1769472 with frontdist = 29, Setpoint = 0.00

In StepTurn(Tracking Left)

In SpinTurn(CW, 90.00)

Msec Hdg pHdg Rate TRate Spd

12862 -109.12 -107.73 15.9 20.0 167

12961 -116.70 -109.12 78.1 20.0 127

13061 -127.75 -116.70 113.9 20.0 87

13158 -142.57 -127.75 155.6 20.0 50

13257 -155.69 -142.57 136.5 20.0 50

13355 -165.76 -155.69 106.5 20.0 50

Slowing down to 10.00 deg/sec at -165.76 deg

13453 -172.39 -165.76 69.2 10.0 50

13551 -176.13 -172.39 38.6 10.0 50

13650 -176.98 -176.13 8.9 10.0 90

13747 -177.02 -176.98 0.4 10.0 130

13845 -178.80 -177.02 18.5 10.0 90

13946 175.39 -178.80 3604.6 10.0 50

14041 169.71 175.39 61.9 10.0 50

14043: Exiting SpinTurn() at 169.71 deg

In RotateToParallelOrientation(Left)

Parallel Orientation Achieved at 14687 with SteeringVal = 0.01

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec LFront LCtr LRear Front FrontVar Rear Steer Output SetPt LSpd RSpd

14698 191 208 190 251 10269.92 289 0.01 -75.00 0.30 150 0

14706 196 205 195 251 10269.92 289 -0.06 -75.00 0.30 150 0

14788 198 213 196 246 10313.22 355 0.02 -75.00 0.30 150 0

14884 207 219 198 240 10143.41 402 0.09 -2.10 0.30 77 72

14983 210 226 197 232 9731.76 451 0.13 -2.10 0.30 77 72

15078 216 228 209 231 9169.12 488 0.07 -34.79 0.30 109 40

15174 223 240 214 226 8389.68 547 0.09 -34.79 0.30 109 40

15270 225 243 215 226 7483.51 576 0.10 -17.39 0.30 92 57

15366 223 244 229 224 6445.87 624 -0.06 -17.39 0.30 92 57

15462 228 245 232 221 5279.39 657 -0.01 -71.32 0.30 146 3

15558 234 254 229 217 4146.39 705 0.05 -71.32 0.30 146 3

15655 248 260 220 218 3116.80 748 0.28 75.00 0.30 0 150

15749 272 266 238 218 2207.54 794 0.34 75.00 0.30 0 150

15843 282 288 248 211 1580.34 839 0.30 17.16 0.12 57 92

15938 293 287 264 200 1231.91 877 0.29 17.16 0.13 57 92

16038 301 303 266 194 947.68 895 0.27 15.06 -0.03 59 90

16137 304 307 281 192 667.96 916 0.23 15.06 -0.07 59 90

16232 305 320 297 187 675.37 984 0.08 -34.10 -0.20 109 40

16329 321 327 294 183 719.94 1012 0.27 -34.10 -0.27 109 40

16429 326 344 301 181 748.10 1030 0.25 75.00 -0.30 0 150

16525 328 345 318 175 773.36 1061 0.10 75.00 -0.30 0 150

16623 340 350 323 171 785.28 1139 0.17 20.96 -0.30 54 95

16719 344 351 327 165 797.68 1142 0.17 20.96 -0.30 54 95

16820 355 364 357 161 809.43 1160 -0.01 -26.17 -0.30 101 48

16914 356 372 351 157 816.35 1215 0.05 -26.17 -0.30 101 48

17007 371 376 338 153 826.46 1232 0.33 75.00 -0.30 0 150

17104 365 378 350 151 828.07 1275 0.15 75.00 -0.30 0 150

17200 362 363 359 146 837.66 8190 0.03 -55.29 -0.30 130 19

17295 360 371 353 143 851.33 8190 0.07 -55.29 -0.30 130 19

17392 361 387 354 136 892.80 8190 0.07 51.82 -0.30 23 126

17488 364 368 354 134 916.39 8190 0.10 51.82 -0.30 23 126

17584 352 380 353 129 948.67 8190 -0.01 8.69 -0.30 66 83

17680 349 368 360 128 954.63 8190 -0.11 8.69 -0.30 66 83

17776 370 381 356 126 945.54 8190 0.06 61.77 -0.30 13 136

17872 362 375 362 123 929.23 8190 0.00 61.77 -0.30 13 136

17972 347 377 363 122 903.09 8190 -0.04 1.55 -0.30 73 76

18070 348 395 372 122 843.49 8190 -0.24 1.55 -0.30 73 76

18167 352 370 362 120 760.07 8190 -0.17 -20.06 -0.30 95 54

18264 332 368 353 115 694.64 8190 -0.21 -20.06 -0.30 95 54

18362 312 336 339 111 661.03 8190 -0.20 7.24 -0.30 67 82

18460 297 323 327 105 644.80 8190 -0.30 7.24 -0.23 67 82

18560 283 312 313 100 627.13 8190 -0.28 -34.24 -0.12 109 40

18655 285 302 299 96 615.78 8190 -0.14 -34.24 -0.02 109 40

18754 264 298 287 90 613.99 8190 -0.19 11.13 0.02 63 86

18850 259 276 255 86 606.39 8190 0.04 11.13 0.24 63 86

18950 252 284 259 84 601.15 8190 -0.09 9.38 0.16 65 84

19046 240 280 260 81 595.10 8190 -0.20 9.38 0.20 65 84

19144 238 252 248 78 595.94 8190 -0.10 -39.82 0.30 114 35

19240 217 253 242 76 593.76 8190 -0.13 -39.82 0.30 114 35

19338 218 246 230 71 598.99 8190 -0.09 -33.99 0.30 108 41

19438 216 236 221 67 608.08 8190 -0.10 -42.19 0.30 117 32

19535 205 225 214 61 621.04 8190 -0.09 -42.19 0.30 117 32

19634 208 222 203 55 647.83 8190 0.06 24.09 0.30 50 99

19733 208 221 203 50 677.59 8190 0.05 24.09 0.30 50 99

19832 210 230 201 48 711.95 8190 0.18 23.73 0.30 51 98

19932 224 232 214 45 739.29 8190 0.10 -44.87 0.30 119 30

20032 224 231 213 40 776.88 8190 0.10 -21.47 0.30 96 53

20130 228 235 210 37 803.67 8190 0.18 -21.47 0.30 96 53

20231 240 244 216 32 829.84 8190 0.15 -1.92 0.30 76 73

Hit Wall Offset Limit 20330 at 1835008 with frontdist = -26214, Setpoint = 0.00

In StepTurn(Tracking Left)

In SpinTurn(CW, 90.00)

Msec Hdg pHdg Rate TRate Spd

20427 170.01 171.69 18.9 20.0 167

20528 162.73 170.01 74.4 20.0 127

20628 149.44 162.73 136.8 20.0 87

20724 133.31 149.44 171.6 20.0 50

20824 118.14 133.31 156.0 20.0 50

20921 107.84 118.14 108.3 20.0 50

Slowing down to 10.00 deg/sec at 107.84 deg

21018 100.75 107.84 75.0 10.0 50

21114 96.88 100.75 41.2 10.0 50

21211 95.61 96.88 13.7 10.0 50

21304 95.40 95.61 2.2 10.0 90

21401 95.27 95.40 1.4 10.0 130

21500 92.77 95.27 26.0 10.0 90

21595 86.00 92.77 73.4 10.0 50

21597: Exiting SpinTurn() at 86.00 deg

In RotateToParallelOrientation(Left)

Parallel Orientation Achieved at 21983 with SteeringVal = -0.01

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec LFront LCtr LRear Front FrontVar Rear Steer Output SetPt LSpd RSpd

21994 161 185 162 108 5323.48 265 -0.01 -75.00 0.30 150 0

22002 164 182 160 108 5323.48 265 0.05 -75.00 0.30 150 0

22086 166 184 159 78 5270.27 295 0.07 -75.00 0.30 150 0

22184 175 185 165 58 5243.36 352 0.10 9.55 0.30 65 84

22280 183 190 172 175 5373.00 388 0.11 9.55 0.30 65 84

22375 197 204 176 91 5247.15 436 0.21 20.28 0.30 54 95

22470 197 215 182 108 5081.33 467 0.15 20.28 0.30 54 95

22564 207 226 189 151 4952.56 518 0.18 -25.08 0.30 100 49

22663 218 233 204 52 4842.48 555 0.14 -25.08 0.30 100 49

22761 236 245 213 141 4580.68 601 0.18 -16.44 0.30 91 58

bObstacleAhead TRUE in TRACKLEFT at 22858 with frontdist 13, variance = 4694.59

In SpinTurn(CW, 90.00)

Msec Hdg pHdg Rate TRate Spd

22952 71.13 73.50 26.9 20.0 87

23052 63.70 71.13 76.5 20.0 50

23147 54.57 63.70 98.3 20.0 50

23246 48.04 54.57 67.2 20.0 50

23343 43.69 48.04 46.2 20.0 50

23441 40.42 43.69 34.1 20.0 50

23540 38.54 40.42 19.5 20.0 90

23638 37.63 38.54 9.7 20.0 130

23734 34.63 37.63 31.7 20.0 90

23833 32.15 34.63 25.5 20.0 50

23931 30.65 32.15 15.7 20.0 90

24031 30.52 30.65 1.4 20.0 130

24127 29.88 30.52 6.8 20.0 170

24225 28.04 29.88 19.3 20.0 210

24324 24.16 28.04 40.3 20.0 170

24419 20.53 24.16 39.3 20.0 130

24515 11.17 20.53 98.9 20.0 90

Slowing down to 10.00 deg/sec at 11.17 deg

24611 0.33 11.17 115.7 10.0 50

24711 -8.59 0.33 92.5 10.0 50

24713: Exiting SpinTurn() at -8.59 deg

In RotateToParallelOrientation(Left)

Parallel Orientation Achieved at 27962 with SteeringVal = -79.86

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec LFront LCtr LRear Front FrontVar Rear Steer Output SetPt LSpd RSpd

27973 313 8191 8190 230 14.32 369 -78.77 -75.00 -0.30 150 0

bStuck TRUE in TRACKLEFT at 27981 with frontdist 230, variance = 14.32

---- ExecuteStuckRecoveryManeuver

Using front distance for backup with front/reardist = 230/36

28059 231 369

28153 248 350

28249 250 327

28346 254 282

28443 255 232

finished backup with frontdist/reardist = 28444 255/23

In SpinTurn(CW, 90.00)

Msec Hdg pHdg Rate TRate Spd

30450 20.77 20.77 0.0 20.0 167

30543 19.58 20.77 13.1 20.0 207

30640 9.56 19.58 106.9 20.0 167

30736 -2.24 9.56 125.2 20.0 127

30830 -14.60 -2.24 134.4 20.0 87

30922 -22.53 -14.60 88.6 20.0 50

31019 -33.56 -22.53 118.1 20.0 50

Slowing down to 10.00 deg/sec at -33.56 deg

31117 -43.11 -33.56 99.6 10.0 50

31214 -49.46 -43.11 66.5 10.0 50

31310 -52.74 -49.46 35.0 10.0 50

31409 -53.79 -52.74 10.8 10.0 50

31510 -53.85 -53.79 0.6 10.0 90

31607 -54.34 -53.85 5.2 10.0 130

31704 -56.91 -54.34 27.5 10.0 90

31802 -62.72 -56.91 60.3 10.0 50

31805: Exiting SpinTurn() at -62.72 deg

Using front distance for fwd travel with front/reardist = 62/34

33807 62 340

33862 61 329

33959 59 346

34058 57 354

34157 52 377

34254 49 434

34354 41 453

finished forward travel with frontdist/reardist = 41/45

In SpinTurn(CCW, 90.00)

Msec Hdg pHdg Rate TRate Spd

36356 -64.92 -64.92 0.0 20.0 167

36419 -64.63 -64.92 4.8 20.0 207

36517 -56.67 -64.63 83.0 20.0 167

36616 -41.04 -56.67 163.3 20.0 127

36712 -25.33 -41.04 166.6 20.0 87

36808 -8.73 -25.33 177.6 20.0 50

Slowing down to 10.00 deg/sec at -8.73 deg

36902 7.93 -8.73 182.0 10.0 50

36998 19.87 7.93 128.6 10.0 50

37000: Exiting SpinTurn() at 19.87 deg

I received: 63

ENTERING COMMAND MODE:

0 = 180 deg CCW Turn

1 = 180 deg CW Turn

A = Back to Auto Mode

S = Stop

F = Forward

R = Reverse

Faster

Left 4 5 6 Right

Slower

In the above telemetry printout, the first obstacle encounter occurs at 7.65 sec, corresponding to about 4 sec into the video. The obstacle is recognized at 18 cm, well inside the desired offset distance of 30 cm. I believe this occurred because the robot had just started turning back toward the near wall with a target steering value of -WALL_OFFSET_TRACK_SETPOINT_LIMIT (-0.3, the maximum toward-wall steering value) which meant that the normal forward obstacle detection limit of WALL_OFFSET_TGTDIST_CM (30cm in this case) wasn’t in force and the backup limit of MIN_FRONT_OBSTACLE_DIST_CM (20cm in this case) triggered instead. This causes the following code block to execute:

if (bObstacleAhead)//bObstacleAheada updated in timer1 ISR
{
	//mySerial.printf("bStuck or bObstacleAhead TRUE in TRACKLEFT with frontdist %d, variance = %6.2f\n", frontdistval, frontvar);
	mySerial.printf("bObstacleAhead TRUE in TRACKLEFT at %lu with frontdist %d, variance = %6.2f\n", 
		millis(), frontdistval, frontvar);
	InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

	SpinTurn(false, 90);//turn 90 deg right
	PrevOpMode = MODE_NONE; //this will force wall offset recapture

	break; //don't execute the rest - just go back to GetOpMode()
}

if (bObstacleAhead)//bObstacleAheada updated in timer1 ISR

{

//mySerial.printf("bStuck or bObstacleAhead TRUE in TRACKLEFT with frontdist %d, variance = %6.2f\n", frontdistval, frontvar);

mySerial.printf("bObstacleAhead TRUE in TRACKLEFT at %lu with frontdist %d, variance = %6.2f\n",

millis(), frontdistval, frontvar);

InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

SpinTurn(false, 90);//turn 90 deg right

PrevOpMode = MODE_NONE; //this will force wall offset recapture

break; //don't execute the rest - just go back to GetOpMode()

}

As can be seen in the above code snippet, this causes the robot to make a 90º ‘spin turn’ to the right, and then restart wall tracking.

At about 12.6 sec (about 10 sec into the movie) we see it detect the upcoming wall at about 30 cm (due to a bug in the code, the printed values are incorrect). This causes the following code block to execute:

else if (WallTrackSetPoint > -WALL_OFFSET_TRACK_SETPOINT_LIMIT && frontdistval <= WALL_OFFSET_TGTDIST_CM)
{
	//DEBUG!!
	mySerial.printf("Hit Wall Offset Limit (%d cm) at %lu with frontdist = %d, Setpoint = %2.2f\n", 
		WALL_OFFSET_TGTDIST_CM, millis(), frontdistval, WallTrackSetPoint);
	//DEBUG!!

											//02/26/20 StepTurn() is now a blocking fcn - doesn't return until step-turn is complete
	StepTurn(TRACKING_LEFT);
	PrevOpMode = MODE_NONE; //this will force wall offset recapture
	break; //skip rest of processing
}

else if (WallTrackSetPoint > -WALL_OFFSET_TRACK_SETPOINT_LIMIT && frontdistval <= WALL_OFFSET_TGTDIST_CM)

{

//DEBUG!!

mySerial.printf("Hit Wall Offset Limit (%d cm) at %lu with frontdist = %d, Setpoint = %2.2f\n",

WALL_OFFSET_TGTDIST_CM, millis(), frontdistval, WallTrackSetPoint);

//DEBUG!!

//02/26/20 StepTurn() is now a blocking fcn - doesn't return until step-turn is complete

StepTurn(TRACKING_LEFT);

PrevOpMode = MODE_NONE; //this will force wall offset recapture

break; //skip rest of processing

}

This code executes a 90º ‘step turn’ (identical to a ‘spin turn’) to the right, and drops back into wall tracking mode.

At about 16 sec into the movie and 20 sec after program start, the robot again detects an upcoming obstacle at about 30cm, and again executes a 90º ‘step turn’ to the right to follow the new wall.

About 1.5 sec later, the robot detects one of the chair legs (I think it was the one nearest the wall in the movie) and tries to get away using another 90º ‘spin turn’, but then exhibits some abnormal behavior. When it attempts to find the parallel orientation to the new (non-existent) wall, it exits RotateToParallelOrientation(Left) with SteeringVal = -79.86, a very strange result. I believe this is because Wall-E2 detected the ‘stuck’ condition while it was attempting to complete the parallel orientation procedure, in this ‘while’ loop

while (abs(ToFSteeringVal) > ToFArray_PARALLEL_FIND_SETPOINT && !bIsStuck && !bObstacleAhead)
{

1 2	while (abs(ToFSteeringVal) > ToFArray_PARALLEL_FIND_SETPOINT && !bIsStuck && !bObstacleAhead) {

So, it exited abnormally, thus the odd SteeringVal number, and then re-detected it in the main tracking loop because the front distance history array isn’t re-initialized after the first detection. This, apparently, is a ‘feature’, not a bug – who knew! ;-).

After the second ‘stuck’ condition detection, the robot attempts to disengage using the ExecuteStuckRecoveryManeuver(), which, in this case tries to back up and then execute an ‘end-around’ maneuver to get past the chair leg. It finished the backup portion of the maneuver successfully with 23cm remaining rearward, and then executed a 90º ‘spin turn’. Then it went forward 21cm using the front distance sensor (not shown in the video), and halted when I took over manual control.

All in all, this was a very successful ‘sandbox’ run. Lots of good data with clear indications of where things are working well and where things need to be modified/fixed.

A bug in the telemetry display code for the ‘Wall Offset Limit’ detection printout (fixed).
In the situation at 7.65 sec where the obstacle detection occurred at 18cm vs 30cm, the robot should recognize that it needs to back up to the wall offset target before making the spin turn (done).
And, of course, porting all this new stuff to the right-side tracking sections

16 November 2020 Update:

Tonight I got the first cut done at porting the TRACKNG_LEFT algorithms over to the TRACKING_RIGHT case, and made what appears to be a successful right-side sandbox run, as shown in the following short video

And here is the telemetry from the run:

In RotateToParallelOrientation(Right)
6266	298	352	372	-0.67	127.00	0	254
6466	280	307	306	-0.26	59.72	67	186
Parallel Orientation Achieved at 6538 with SteeringVal = 0.01
WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	RFront	RCtr	RRear	Front	FrontVarSteer	Output	SetPt	LSpd	RSpd
6549	281	294	280	227	8728.96	0.01	-2.15	0.06	72	77
6554	281	294	280	227	8728.96	0.01	-2.15	0.06	72	77
6635	310	311	276	240	8520.67	0.34	-2.15	-0.11	72	77
6729	327	325	292	244	8381.43	0.35	75.00	-0.25	150	0
6828	342	337	295	240	8290.90	0.47	75.00	-0.30	150	0
6921	357	355	316	230	8240.07	0.41	75.00	-0.30	150	0
7018	360	338	328	222	8224.66	0.32	75.00	-0.30	150	0
7114	352	359	326	213	8233.74	0.26	16.46	-0.30	91	58
7211	349	356	335	206	8247.12	0.14	16.46	-0.30	91	58
7305	336	357	341	202	8234.86	-0.05	-61.79	-0.30	13	136
7402	334	349	342	199	8165.47	-0.08	-61.79	-0.30	13	136
7499	338	371	343	197	8004.15	-0.14	-4.31	-0.30	70	79
7593	345	353	335	195	7715.00	0.10	-4.31	-0.30	70	79
7695	344	359	332	192	7260.35	0.13	75.00	-0.30	150	0
7786	350	359	329	189	6597.09	0.21	75.00	-0.30	150	0
7881	347	374	338	182	5682.62	0.03	11.97	-0.30	86	63
7979	348	355	352	177	4460.83	-0.04	11.97	-0.30	86	63
8078	340	353	349	172	2874.73	-0.09	-5.40	-0.30	69	80
8170	343	362	352	170	2504.54	-0.09	-5.40	-0.30	69	80
8266	343	345	360	165	2110.81	-0.17	-1.01	-0.30	73	76
8360	352	364	342	162	1719.27	0.10	-1.01	-0.30	73	76
8460	341	369	353	158	1311.16	-0.12	43.53	-0.30	118	31
8557	354	370	351	156	926.95	0.03	43.53	-0.30	118	31
8651	344	361	359	152	772.95	-0.15	16.98	-0.30	91	58
8748	333	359	367	150	853.82	-0.34	16.98	-0.30	91	58
8845	334	342	360	147	895.27	-0.26	-17.90	-0.30	57	92
8940	329	362	351	143	927.53	-0.22	-17.90	-0.30	57	92
9037	317	357	342	139	900.06	-0.25	19.25	-0.30	94	55
9134	317	347	343	134	837.40	-0.26	19.25	-0.30	94	55
9231	315	338	332	131	767.64	-0.17	48.64	-0.30	123	26
9329	299	331	327	128	720.97	-0.28	48.64	-0.30	123	26
9426	287	334	334	125	688.38	-0.34	-43.48	-0.30	31	118
9526	288	322	334	110	719.70	-0.46	-43.48	-0.22	31	118
9621	274	313	315	109	749.19	-0.37	-22.20	-0.13	52	97
9720	264	299	301	115	743.06	-0.37	-22.20	0.01	52	97
9818	253	279	283	111	736.87	-0.26	-4.61	0.21	70	79
9914	245	270	268	110	716.83	-0.23	-4.61	0.30	70	79
10012	232	266	265	104	699.06	-0.37	-75.00	0.30	0	150
10109	224	253	248	100	679.92	-0.24	-75.00	0.30	0	150
10205	218	244	243	96	659.09	-0.25	-13.27	0.30	61	88
10301	204	221	225	91	655.60	-0.21	-13.27	0.30	61	88
10397	198	221	212	89	651.66	-0.15	-10.62	0.30	64	85
10496	199	213	207	84	658.13	-0.08	-10.62	0.30	64	85
10595	169	214	204	82	653.91	-0.23	-74.21	0.30	0	149
10691	196	198	203	81	646.92	-0.07	-74.21	0.30	0	149
10789	202	215	201	81	628.52	0.00	40.89	0.30	115	34
10889	214	226	199	79	609.31	0.15	31.44	0.30	106	43
10985	208	228	202	75	588.31	0.06	31.44	0.30	106	43
11083	225	233	206	70	575.19	0.19	2.11	0.30	77	72
11183	222	225	219	67	556.60	0.06	-62.61	0.30	12	137
11281	232	236	223	64	535.88	0.05	-62.61	0.30	12	137
11377	253	238	221	62	513.85	0.34	75.00	0.30	150	0
11475	281	278	231	61	488.39	0.43	75.00	0.22	150	0
11573	279	277	245	57	471.88	0.36	19.90	0.23	94	55
11672	287	291	261	51	463.43	0.26	19.90	0.09	94	55
11769	299	299	273	46	459.76	0.15	-47.68	0.01	27	122
11870	301	295	288	42	457.37	0.01	-44.80	0.05	30	119
11969	302	300	280	36	498.81	0.23	-44.80	0.00	30	119
12065	303	313	283	33	535.29	0.20	75.00	-0.13	150	0
12164	325	318	307	30	544.00	0.23	75.00	-0.18	150	0
12260	338	332	315	24	563.76	0.23	65.78	-0.30	140	9
bObstacleAhead TRUE in TRACKRIGHT at 12358 with frontdist 19, variance = 580.36
backing up to 30 using front distance sensor
backed up successfully to 33 using front distance sensor
In SpinTurn(CCW, 90.00)
Msec	Hdg	PrevHdg	Rate	Set	Out	Spd
13039	0.17	-1.08	12.95	45.00	200.00	0
13136	5.51	0.17	56.96	45.00	200.00	0
13234	11.65	5.51	64.88	45.00	200.00	0
13331	15.61	11.65	42.26	45.00	200.00	0
13428	16.21	15.61	6.30	45.00	-200.00	200
13528	18.80	16.21	26.83	45.00	200.00	0
13626	28.55	18.80	102.85	45.00	200.00	0
13725	33.06	28.55	46.58	45.00	200.00	0
13825	33.04	33.06	0.16	45.00	-200.00	200
13921	35.82	33.04	29.69	45.00	-200.00	255
14021	48.68	35.82	132.80	45.00	200.00	55
14117	64.01	48.68	165.27	45.00	200.00	0
14213	75.93	64.01	128.49	45.00	-13.74	13
14310	84.24	75.93	88.53	45.00	-13.74	26
14313: Exiting SpinTurn() at 84.24 deg
In RotateToParallelOrientation(Right)
Parallel Orientation Achieved at 14504 with SteeringVal = 0.10
WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	LFront	LCtr	LRear	Front	FrontVar	Rear	Steer	Output	SetPt	LSpd	RSpd
14516	391	398	376	108	11084.51	0.10	5.98	-0.30	80	69
14521	391	398	376	108	11084.51	0.10	5.98	-0.30	80	69
14605	388	400	384	106	8356.08	0.04	5.98	-0.30	80	69
14704	380	395	384	105	5067.11	0.15	66.33	-0.30	141	8
14801	402	394	383	100	1142.00	0.19	66.33	-0.30	141	8
14883	398	404	393	98	1092.24	0.04	8.35	-0.30	83	66
14980	377	402	397	97	1019.28	-0.20	8.35	-0.30	83	66
15076	384	412	399	95	935.28	-0.15	-34.20	-0.30	40	109
15174	378	392	395	92	837.84	-0.17	-34.20	-0.30	40	109
15271	370	402	395	90	748.36	-0.15	23.25	-0.30	98	51
15369	384	391	387	87	664.72	-0.03	23.25	-0.30	98	51
15464	378	394	389	86	579.21	-0.15	23.70	-0.30	98	51
15563	374	394	388	82	504.21	-0.14	23.70	-0.30	98	51
15662	360	392	393	79	448.40	-0.29	-32.27	-0.30	42	107
15760	347	391	383	75	398.56	-0.36	-32.27	-0.30	42	107
15859	325	375	376	71	354.21	-0.40	-34.57	-0.30	40	109
15958	325	341	365	68	324.42	-0.40	-34.57	-0.30	40	109
16056	331	337	347	65	296.47	-0.14	75.00	-0.30	150	0
16154	332	343	327	62	284.88	0.05	75.00	-0.30	150	0
16253	330	353	332	60	275.08	0.07	75.00	-0.30	150	0
16351	329	337	341	58	284.24	-0.12	75.00	-0.30	150	0
16447	324	345	350	55	326.19	-0.28	-75.00	-0.30	0	150
16545	317	352	367	54	343.76	-0.40	-75.00	-0.30	0	150
16648	314	349	354	51	364.72	-0.38	-28.16	-0.30	46	103
16746	306	334	343	43	390.02	-0.34	-28.16	-0.30	46	103
16843	297	328	325	39	412.51	-0.28	39.84	-0.28	114	35
16941	286	321	314	33	442.75	-0.21	39.84	-0.21	114	35
17039	287	310	312	28	476.95	-0.25	3.39	-0.10	78	71
Hit Wall Offset Limit (30 cm) at 17138 with frontdist = 25, Setpoint = -0.10
In StepTurn(Tracking Right)
In SpinTurn(CCW, 90.00)
Msec	Hdg	PrevHdg	Rate	Set	Out	Spd
17236	67.82	67.35	5.02	45.00	-200.00	255
17335	75.00	67.82	75.00	45.00	-200.00	255
17438	91.46	75.00	164.54	45.00	200.00	55
17537	113.79	91.46	235.10	45.00	200.00	0
17633	134.03	113.79	217.56	45.00	200.00	0
17732	146.96	134.03	134.52	45.00	200.00	0
17831	151.31	146.96	45.43	45.00	-200.00	200
17835: Exiting SpinTurn() at 151.31 deg
In RotateToParallelOrientation(Right)
18134	126	157	159	-0.38	86.02	40	213
Parallel Orientation Achieved at 18321 with SteeringVal = -0.02
WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	LFront	LCtr	LRear	Front	FrontVar	Rear	Steer	Output	SetPt	LSpd	RSpd
18332	131	154	128	321	5983.33	-0.02	45.43	0.30	120	29
18337	131	154	128	321	5983.33	-0.02	45.43	0.30	120	29
18418	135	158	133	206	6668.69	0.02	45.43	0.30	120	29
18512	144	154	140	208	7308.87	-0.01	-20.50	0.30	54	95
18611	146	157	140	313	9298.40	0.06	-20.50	0.30	54	95
18706	150	161	143	311	11046.16	0.01	-16.37	0.30	58	91
18800	153	163	146	308	12536.56	0.07	-16.37	0.30	58	91
18897	157	165	144	270	13305.75	0.13	25.12	0.30	100	49
18991	162	179	156	268	13898.79	0.06	25.12	0.30	100	49
19087	162	177	159	266	14304.01	0.03	-51.69	0.30	23	126
19182	162	173	161	261	14450.51	0.01	-51.69	0.30	23	126
19278	173	180	162	213	14096.38	0.11	9.74	0.30	84	65
19376	179	189	162	191	13513.93	0.17	9.74	0.30	84	65
19472	186	203	174	187	12789.90	0.12	-10.80	0.30	64	85
19569	194	196	183	184	11946.82	0.11	-10.80	0.30	64	85
19665	197	201	184	177	11022.66	0.13	-10.31	0.30	64	85
19761	204	202	185	174	9907.81	0.19	-10.31	0.30	64	85
19858	211	215	198	172	8758.23	0.13	-13.82	0.30	61	88
19953	212	216	207	166	7632.49	0.05	-13.82	0.30	61	88
20050	218	228	205	179	6549.61	0.13	-14.33	0.30	60	89
20147	232	232	213	178	5714.13	0.19	-14.33	0.30	60	89
20242	234	238	221	176	4935.26	0.13	-14.84	0.30	60	89
20336	249	243	227	172	4144.19	0.13	-14.84	0.30	60	89
20432	252	260	235	171	3394.58	0.17	0.77	0.30	75	74
20531	265	269	238	171	3167.90	0.27	28.68	0.30	103	46
20626	270	280	247	165	2857.96	0.23	28.68	0.20	103	46
20722	280	275	255	160	2505.05	0.25	-4.32	0.25	70	79
20818	281	281	269	153	2629.37	0.12	-4.32	0.19	70	79
20918	286	290	269	147	2771.09	0.17	-15.11	0.10	59	90
21017	289	273	275	144	2412.00	0.14	-15.11	0.27	59	90
21114	303	304	283	141	1992.57	0.20	35.61	-0.04	110	39
21209	307	313	289	138	1521.46	0.18	35.61	-0.13	110	39
21306	302	314	298	131	1317.75	0.04	-26.85	-0.14	48	101
21402	310	321	303	129	1074.44	0.07	-26.85	-0.21	48	101
21500	326	310	308	127	791.55	0.18	73.99	-0.10	148	1
21599	319	341	309	123	499.21	0.10	73.99	-0.30	148	1
21696	317	335	315	112	484.99	0.02	-11.05	-0.30	63	86
21790	319	334	318	109	531.44	0.01	-11.05	-0.30	63	86
21890	317	328	322	105	582.61	-0.05	7.64	-0.28	82	67
21985	325	322	326	102	631.94	-0.01	7.64	-0.22	82	67
22080	328	341	327	99	690.09	0.01	55.57	-0.30	130	19
22176	329	335	328	98	740.19	-0.02	55.57	-0.30	130	19
22275	333	343	324	176	750.18	-0.04	18.35	-0.30	93	56
22368	320	348	335	175	769.94	-0.18	18.35	-0.30	93	56
22465	316	352	329	174	756.59	-0.09	13.98	-0.30	88	61
22563	313	331	337	170	736.67	-0.28	13.98	-0.30	88	61
22658	309	328	332	167	715.82	-0.32	-65.14	-0.28	9	140
22753	305	329	328	164	699.31	-0.30	-65.14	-0.29	9	140
22851	286	314	320	159	677.04	-0.22	29.62	-0.14	104	45
22944	287	302	308	155	649.19	-0.18	29.62	-0.02	104	45
23043	285	306	310	151	629.75	-0.25	-20.95	-0.06	54	95
23141	282	312	296	145	614.81	-0.14	-20.95	-0.12	54	95
23236	269	291	295	141	607.63	-0.16	8.30	0.09	83	66
23333	278	299	287	137	605.30	-0.08	8.30	0.01	83	66
23429	307	286	284	134	605.04	0.11	75.00	0.14	150	0
23527	502	292	280	128	608.91	1.64	75.00	0.08	150	0
23626	781	316	285	127	613.53	5.28	75.00	-0.16	150	0
23721	791	326	290	128	615.44	4.93	75.00	-0.26	150	0
23821	564	319	308	131	614.25	3.09	-75.00	-0.19	0	150
23918	402	325	329	109	641.78	0.77	-75.00	-0.25	0	150
24013	558	352	317	107	669.23	2.03	-45.64	-0.30	29	120
24113	703	467	317	126	649.74	3.33	75.00	-0.30	150	0
24213	850	709	481	118	632.92	3.69	75.00	-0.30	150	0
24311	811	842	622	115	611.13	1.99	75.00	-0.30	150	0
24407	797	835	649	94	636.27	1.87	-75.00	-0.30	0	150
24502	817	732	591	89	670.60	2.44	-75.00	-0.30	0	150
24601	830	801	642	83	725.71	1.88	75.00	-0.30	150	0
24696	248	281	290	78	  781.84	8190	-0.42	75.00	0.19	0	150
24795	272	264	266	75	  823.44	8190	0.06	-75.00	0.30	150	0
24892	327	273	258	71	  853.43	8191	0.69	-75.00	0.27	150	0
24988	542	379	269	68	  878.24	8190	2.73	75.00	-0.30	0	150
25086	676	673	348	63	  902.54	8190	3.28	75.00	-0.30	0	150
25185	696	713	571	93	  822.56	8190	1.25	-75.00	-0.30	150	0
25280	696	711	633	89	  753.12	8191	0.63	-75.00	-0.30	150	0
25379	667	717	667	88	  685.37	574	0.33	-75.00	-0.30	150	0
25479	673	650	667	60	  686.83	8191	0.06	30.00	-0.30	45	105
25577	742	736	643	61	  681.45	8190	0.99	30.00	-0.30	45	105
25674	796	782	673	54	  690.36	8190	1.23	75.00	-0.30	0	150
25773	830	810	694	48	  710.01	8190	1.29	75.00	-0.30	0	150
25869	582	643	721	37	756.88	-1.08	-75.00	-0.30	0	150
25966	545	579	615	34	807.42	-0.70	-75.00	-0.30	0	150
26066	493	523	553	33	837.20	-0.56	75.00	-0.30	150	0
26166	468	488	499	22	879.26	-0.31	75.00	-0.30	150	0
26262	446	499	502	25	859.92	-0.64	75.00	-0.30	150	0
26361	441	523	508	23	900.38	-0.67	-74.26	-0.30	0	149
26459	467	519	544	22	927.31	-0.90	-74.26	-0.30	0	149
bObstacleAhead TRUE in TRACKRIGHT at 26556 with frontdist 15, variance = 874.38
backing up to 30 using front distance sensor
backed up successfully to 35 using front distance sensor
In SpinTurn(CCW, 90.00)
Msec	Hdg	PrevHdg	Rate	Set	Out	Spd

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

In RotateToParallelOrientation(Right)

6266 298 352 372 -0.67 127.00 0 254

6466 280 307 306 -0.26 59.72 67 186

Parallel Orientation Achieved at 6538 with SteeringVal = 0.01

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec RFront RCtr RRear Front FrontVarSteer Output SetPt LSpd RSpd

6549 281 294 280 227 8728.96 0.01 -2.15 0.06 72 77

6554 281 294 280 227 8728.96 0.01 -2.15 0.06 72 77

6635 310 311 276 240 8520.67 0.34 -2.15 -0.11 72 77

6729 327 325 292 244 8381.43 0.35 75.00 -0.25 150 0

6828 342 337 295 240 8290.90 0.47 75.00 -0.30 150 0

6921 357 355 316 230 8240.07 0.41 75.00 -0.30 150 0

7018 360 338 328 222 8224.66 0.32 75.00 -0.30 150 0

7114 352 359 326 213 8233.74 0.26 16.46 -0.30 91 58

7211 349 356 335 206 8247.12 0.14 16.46 -0.30 91 58

7305 336 357 341 202 8234.86 -0.05 -61.79 -0.30 13 136

7402 334 349 342 199 8165.47 -0.08 -61.79 -0.30 13 136

7499 338 371 343 197 8004.15 -0.14 -4.31 -0.30 70 79

7593 345 353 335 195 7715.00 0.10 -4.31 -0.30 70 79

7695 344 359 332 192 7260.35 0.13 75.00 -0.30 150 0

7786 350 359 329 189 6597.09 0.21 75.00 -0.30 150 0

7881 347 374 338 182 5682.62 0.03 11.97 -0.30 86 63

7979 348 355 352 177 4460.83 -0.04 11.97 -0.30 86 63

8078 340 353 349 172 2874.73 -0.09 -5.40 -0.30 69 80

8170 343 362 352 170 2504.54 -0.09 -5.40 -0.30 69 80

8266 343 345 360 165 2110.81 -0.17 -1.01 -0.30 73 76

8360 352 364 342 162 1719.27 0.10 -1.01 -0.30 73 76

8460 341 369 353 158 1311.16 -0.12 43.53 -0.30 118 31

8557 354 370 351 156 926.95 0.03 43.53 -0.30 118 31

8651 344 361 359 152 772.95 -0.15 16.98 -0.30 91 58

8748 333 359 367 150 853.82 -0.34 16.98 -0.30 91 58

8845 334 342 360 147 895.27 -0.26 -17.90 -0.30 57 92

8940 329 362 351 143 927.53 -0.22 -17.90 -0.30 57 92

9037 317 357 342 139 900.06 -0.25 19.25 -0.30 94 55

9134 317 347 343 134 837.40 -0.26 19.25 -0.30 94 55

9231 315 338 332 131 767.64 -0.17 48.64 -0.30 123 26

9329 299 331 327 128 720.97 -0.28 48.64 -0.30 123 26

9426 287 334 334 125 688.38 -0.34 -43.48 -0.30 31 118

9526 288 322 334 110 719.70 -0.46 -43.48 -0.22 31 118

9621 274 313 315 109 749.19 -0.37 -22.20 -0.13 52 97

9720 264 299 301 115 743.06 -0.37 -22.20 0.01 52 97

9818 253 279 283 111 736.87 -0.26 -4.61 0.21 70 79

9914 245 270 268 110 716.83 -0.23 -4.61 0.30 70 79

10012 232 266 265 104 699.06 -0.37 -75.00 0.30 0 150

10109 224 253 248 100 679.92 -0.24 -75.00 0.30 0 150

10205 218 244 243 96 659.09 -0.25 -13.27 0.30 61 88

10301 204 221 225 91 655.60 -0.21 -13.27 0.30 61 88

10397 198 221 212 89 651.66 -0.15 -10.62 0.30 64 85

10496 199 213 207 84 658.13 -0.08 -10.62 0.30 64 85

10595 169 214 204 82 653.91 -0.23 -74.21 0.30 0 149

10691 196 198 203 81 646.92 -0.07 -74.21 0.30 0 149

10789 202 215 201 81 628.52 0.00 40.89 0.30 115 34

10889 214 226 199 79 609.31 0.15 31.44 0.30 106 43

10985 208 228 202 75 588.31 0.06 31.44 0.30 106 43

11083 225 233 206 70 575.19 0.19 2.11 0.30 77 72

11183 222 225 219 67 556.60 0.06 -62.61 0.30 12 137

11281 232 236 223 64 535.88 0.05 -62.61 0.30 12 137

11377 253 238 221 62 513.85 0.34 75.00 0.30 150 0

11475 281 278 231 61 488.39 0.43 75.00 0.22 150 0

11573 279 277 245 57 471.88 0.36 19.90 0.23 94 55

11672 287 291 261 51 463.43 0.26 19.90 0.09 94 55

11769 299 299 273 46 459.76 0.15 -47.68 0.01 27 122

11870 301 295 288 42 457.37 0.01 -44.80 0.05 30 119

11969 302 300 280 36 498.81 0.23 -44.80 0.00 30 119

12065 303 313 283 33 535.29 0.20 75.00 -0.13 150 0

12164 325 318 307 30 544.00 0.23 75.00 -0.18 150 0

12260 338 332 315 24 563.76 0.23 65.78 -0.30 140 9

bObstacleAhead TRUE in TRACKRIGHT at 12358 with frontdist 19, variance = 580.36

backing up to 30 using front distance sensor

backed up successfully to 33 using front distance sensor

In SpinTurn(CCW, 90.00)

Msec Hdg PrevHdg Rate Set Out Spd

13039 0.17 -1.08 12.95 45.00 200.00 0

13136 5.51 0.17 56.96 45.00 200.00 0

13234 11.65 5.51 64.88 45.00 200.00 0

13331 15.61 11.65 42.26 45.00 200.00 0

13428 16.21 15.61 6.30 45.00 -200.00 200

13528 18.80 16.21 26.83 45.00 200.00 0

13626 28.55 18.80 102.85 45.00 200.00 0

13725 33.06 28.55 46.58 45.00 200.00 0

13825 33.04 33.06 0.16 45.00 -200.00 200

13921 35.82 33.04 29.69 45.00 -200.00 255

14021 48.68 35.82 132.80 45.00 200.00 55

14117 64.01 48.68 165.27 45.00 200.00 0

14213 75.93 64.01 128.49 45.00 -13.74 13

14310 84.24 75.93 88.53 45.00 -13.74 26

14313: Exiting SpinTurn() at 84.24 deg

In RotateToParallelOrientation(Right)

Parallel Orientation Achieved at 14504 with SteeringVal = 0.10

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec LFront LCtr LRear Front FrontVar Rear Steer Output SetPt LSpd RSpd

14516 391 398 376 108 11084.51 0.10 5.98 -0.30 80 69

14521 391 398 376 108 11084.51 0.10 5.98 -0.30 80 69

14605 388 400 384 106 8356.08 0.04 5.98 -0.30 80 69

14704 380 395 384 105 5067.11 0.15 66.33 -0.30 141 8

14801 402 394 383 100 1142.00 0.19 66.33 -0.30 141 8

14883 398 404 393 98 1092.24 0.04 8.35 -0.30 83 66

14980 377 402 397 97 1019.28 -0.20 8.35 -0.30 83 66

15076 384 412 399 95 935.28 -0.15 -34.20 -0.30 40 109

15174 378 392 395 92 837.84 -0.17 -34.20 -0.30 40 109

15271 370 402 395 90 748.36 -0.15 23.25 -0.30 98 51

15369 384 391 387 87 664.72 -0.03 23.25 -0.30 98 51

15464 378 394 389 86 579.21 -0.15 23.70 -0.30 98 51

15563 374 394 388 82 504.21 -0.14 23.70 -0.30 98 51

15662 360 392 393 79 448.40 -0.29 -32.27 -0.30 42 107

15760 347 391 383 75 398.56 -0.36 -32.27 -0.30 42 107

15859 325 375 376 71 354.21 -0.40 -34.57 -0.30 40 109

15958 325 341 365 68 324.42 -0.40 -34.57 -0.30 40 109

16056 331 337 347 65 296.47 -0.14 75.00 -0.30 150 0

16154 332 343 327 62 284.88 0.05 75.00 -0.30 150 0

16253 330 353 332 60 275.08 0.07 75.00 -0.30 150 0

16351 329 337 341 58 284.24 -0.12 75.00 -0.30 150 0

16447 324 345 350 55 326.19 -0.28 -75.00 -0.30 0 150

16545 317 352 367 54 343.76 -0.40 -75.00 -0.30 0 150

16648 314 349 354 51 364.72 -0.38 -28.16 -0.30 46 103

16746 306 334 343 43 390.02 -0.34 -28.16 -0.30 46 103

16843 297 328 325 39 412.51 -0.28 39.84 -0.28 114 35

16941 286 321 314 33 442.75 -0.21 39.84 -0.21 114 35

17039 287 310 312 28 476.95 -0.25 3.39 -0.10 78 71

Hit Wall Offset Limit (30 cm) at 17138 with frontdist = 25, Setpoint = -0.10

In StepTurn(Tracking Right)

In SpinTurn(CCW, 90.00)

Msec Hdg PrevHdg Rate Set Out Spd

17236 67.82 67.35 5.02 45.00 -200.00 255

17335 75.00 67.82 75.00 45.00 -200.00 255

17438 91.46 75.00 164.54 45.00 200.00 55

17537 113.79 91.46 235.10 45.00 200.00 0

17633 134.03 113.79 217.56 45.00 200.00 0

17732 146.96 134.03 134.52 45.00 200.00 0

17831 151.31 146.96 45.43 45.00 -200.00 200

17835: Exiting SpinTurn() at 151.31 deg

In RotateToParallelOrientation(Right)

18134 126 157 159 -0.38 86.02 40 213

Parallel Orientation Achieved at 18321 with SteeringVal = -0.02

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec LFront LCtr LRear Front FrontVar Rear Steer Output SetPt LSpd RSpd

18332 131 154 128 321 5983.33 -0.02 45.43 0.30 120 29

18337 131 154 128 321 5983.33 -0.02 45.43 0.30 120 29

18418 135 158 133 206 6668.69 0.02 45.43 0.30 120 29

18512 144 154 140 208 7308.87 -0.01 -20.50 0.30 54 95

18611 146 157 140 313 9298.40 0.06 -20.50 0.30 54 95

18706 150 161 143 311 11046.16 0.01 -16.37 0.30 58 91

18800 153 163 146 308 12536.56 0.07 -16.37 0.30 58 91

18897 157 165 144 270 13305.75 0.13 25.12 0.30 100 49

18991 162 179 156 268 13898.79 0.06 25.12 0.30 100 49

19087 162 177 159 266 14304.01 0.03 -51.69 0.30 23 126

19182 162 173 161 261 14450.51 0.01 -51.69 0.30 23 126

19278 173 180 162 213 14096.38 0.11 9.74 0.30 84 65

19376 179 189 162 191 13513.93 0.17 9.74 0.30 84 65

19472 186 203 174 187 12789.90 0.12 -10.80 0.30 64 85

19569 194 196 183 184 11946.82 0.11 -10.80 0.30 64 85

19665 197 201 184 177 11022.66 0.13 -10.31 0.30 64 85

19761 204 202 185 174 9907.81 0.19 -10.31 0.30 64 85

19858 211 215 198 172 8758.23 0.13 -13.82 0.30 61 88

19953 212 216 207 166 7632.49 0.05 -13.82 0.30 61 88

20050 218 228 205 179 6549.61 0.13 -14.33 0.30 60 89

20147 232 232 213 178 5714.13 0.19 -14.33 0.30 60 89

20242 234 238 221 176 4935.26 0.13 -14.84 0.30 60 89

20336 249 243 227 172 4144.19 0.13 -14.84 0.30 60 89

20432 252 260 235 171 3394.58 0.17 0.77 0.30 75 74

20531 265 269 238 171 3167.90 0.27 28.68 0.30 103 46

20626 270 280 247 165 2857.96 0.23 28.68 0.20 103 46

20722 280 275 255 160 2505.05 0.25 -4.32 0.25 70 79

20818 281 281 269 153 2629.37 0.12 -4.32 0.19 70 79

20918 286 290 269 147 2771.09 0.17 -15.11 0.10 59 90

21017 289 273 275 144 2412.00 0.14 -15.11 0.27 59 90

21114 303 304 283 141 1992.57 0.20 35.61 -0.04 110 39

21209 307 313 289 138 1521.46 0.18 35.61 -0.13 110 39

21306 302 314 298 131 1317.75 0.04 -26.85 -0.14 48 101

21402 310 321 303 129 1074.44 0.07 -26.85 -0.21 48 101

21500 326 310 308 127 791.55 0.18 73.99 -0.10 148 1

21599 319 341 309 123 499.21 0.10 73.99 -0.30 148 1

21696 317 335 315 112 484.99 0.02 -11.05 -0.30 63 86

21790 319 334 318 109 531.44 0.01 -11.05 -0.30 63 86

21890 317 328 322 105 582.61 -0.05 7.64 -0.28 82 67

21985 325 322 326 102 631.94 -0.01 7.64 -0.22 82 67

22080 328 341 327 99 690.09 0.01 55.57 -0.30 130 19

22176 329 335 328 98 740.19 -0.02 55.57 -0.30 130 19

22275 333 343 324 176 750.18 -0.04 18.35 -0.30 93 56

22368 320 348 335 175 769.94 -0.18 18.35 -0.30 93 56

22465 316 352 329 174 756.59 -0.09 13.98 -0.30 88 61

22563 313 331 337 170 736.67 -0.28 13.98 -0.30 88 61

22658 309 328 332 167 715.82 -0.32 -65.14 -0.28 9 140

22753 305 329 328 164 699.31 -0.30 -65.14 -0.29 9 140

22851 286 314 320 159 677.04 -0.22 29.62 -0.14 104 45

22944 287 302 308 155 649.19 -0.18 29.62 -0.02 104 45

23043 285 306 310 151 629.75 -0.25 -20.95 -0.06 54 95

23141 282 312 296 145 614.81 -0.14 -20.95 -0.12 54 95

23236 269 291 295 141 607.63 -0.16 8.30 0.09 83 66

23333 278 299 287 137 605.30 -0.08 8.30 0.01 83 66

23429 307 286 284 134 605.04 0.11 75.00 0.14 150 0

23527 502 292 280 128 608.91 1.64 75.00 0.08 150 0

23626 781 316 285 127 613.53 5.28 75.00 -0.16 150 0

23721 791 326 290 128 615.44 4.93 75.00 -0.26 150 0

23821 564 319 308 131 614.25 3.09 -75.00 -0.19 0 150

23918 402 325 329 109 641.78 0.77 -75.00 -0.25 0 150

24013 558 352 317 107 669.23 2.03 -45.64 -0.30 29 120

24113 703 467 317 126 649.74 3.33 75.00 -0.30 150 0

24213 850 709 481 118 632.92 3.69 75.00 -0.30 150 0

24311 811 842 622 115 611.13 1.99 75.00 -0.30 150 0

24407 797 835 649 94 636.27 1.87 -75.00 -0.30 0 150

24502 817 732 591 89 670.60 2.44 -75.00 -0.30 0 150

24601 830 801 642 83 725.71 1.88 75.00 -0.30 150 0

24696 248 281 290 78 781.84 8190 -0.42 75.00 0.19 0 150

24795 272 264 266 75 823.44 8190 0.06 -75.00 0.30 150 0

24892 327 273 258 71 853.43 8191 0.69 -75.00 0.27 150 0

24988 542 379 269 68 878.24 8190 2.73 75.00 -0.30 0 150

25086 676 673 348 63 902.54 8190 3.28 75.00 -0.30 0 150

25185 696 713 571 93 822.56 8190 1.25 -75.00 -0.30 150 0

25280 696 711 633 89 753.12 8191 0.63 -75.00 -0.30 150 0

25379 667 717 667 88 685.37 574 0.33 -75.00 -0.30 150 0

25479 673 650 667 60 686.83 8191 0.06 30.00 -0.30 45 105

25577 742 736 643 61 681.45 8190 0.99 30.00 -0.30 45 105

25674 796 782 673 54 690.36 8190 1.23 75.00 -0.30 0 150

25773 830 810 694 48 710.01 8190 1.29 75.00 -0.30 0 150

25869 582 643 721 37 756.88 -1.08 -75.00 -0.30 0 150

25966 545 579 615 34 807.42 -0.70 -75.00 -0.30 0 150

26066 493 523 553 33 837.20 -0.56 75.00 -0.30 150 0

26166 468 488 499 22 879.26 -0.31 75.00 -0.30 150 0

26262 446 499 502 25 859.92 -0.64 75.00 -0.30 150 0

26361 441 523 508 23 900.38 -0.67 -74.26 -0.30 0 149

26459 467 519 544 22 927.31 -0.90 -74.26 -0.30 0 149

bObstacleAhead TRUE in TRACKRIGHT at 26556 with frontdist 15, variance = 874.38

backing up to 30 using front distance sensor

backed up successfully to 35 using front distance sensor

In SpinTurn(CCW, 90.00)

Msec Hdg PrevHdg Rate Set Out Spd

Here’s an Excel plot of the Right side center distance and L/R motor speeds vs time.

Comparing the times from the video and the telemetry, it appears the video time is about 3-3.5 sec lower than the telemetry values. In the video, Wall-E2 detects the first upcoming wall at about 9 sec, and this corresponds with the telemetry at 12.358 where the wall is detected with front distance of 19cm. The reason the wall didn’t get detected earlier is the robot was currently tracking back toward the wall with a steering value of -0.3, and this causes the wall detection value to be reduced to suppress false positives.

The robot then takes 2 sec to back up to 33 cm and turn 90 deg CCW, and then it starts tracking the right-side wall again. It detects the next wall at 17.1 sec (14 sec in video) and 30 cm (the steering value at that point was 0.10, so no reduction in upcoming obstacle detection distance). Because the detection occurred at 30 cm, the robot doesn’t need to back up; it just makes another 90 deg spin turn CCW and starts tracking the right-side wall again. The last segment clearly shows that Wall-E2 is capable of tracking to and capturing the desired offset of 30 cm.

All in all, a very successful right-side sandbox run.

Stay tuned!

Frank

Adding a VL53L0X Rear Distance Sensor to Wall-E2

1 Reply

posted 24 October 2020

After documenting left-side wall-tracking success with Wall-E2, my autonomous wall-tracking robot (see this post and this post), I started thinking about improving Wall-E2’s obstacle avoidance performance.

Wall-E2 can encounter several distinct obstacle situations during wall tracking operations. In the simplest case, Wall-E2 approaches an upcoming corner while tracking a wall, and needs to know how to transition from tracking the current wall to tracking the upcoming wall. A more difficult situation arises when Wall-E2 is ‘stuck’ – prevented from moving forward by an obstacle that isn’t detected by its front LIDAR distance sensor; a shoe, or the curved foot of a coat rack. A third situation arises when Wall-E2 encounters an obstacle that just wasn’t there a second ago; a cat or a human foot or a bag of groceries.

In the simple wall-to-wall transition case, all Wall-E2 has to do is make a right-angle turn away from the current wall and start following the next wall; this was successfully demonstrated several times in the previous posts. This maneuver utilizes a ‘spin-turn’ technique intended to minimize the backward movement of the robot while turning. This is done to prevent Wall-E2 from backing into the currently-tracked wall while attempting to turn toward and track the upcoming wall. Unfortunately, this maneuver is not always successful, whereupon Wall-E2 tries to climb backwards up the current wall, often with disastrous results.

In the ‘stuck’ case, Wall-E2 has to first recognize that it is no longer moving forward (or in any other direction for that matter), and then figure out what to do about it. Detection is accomplished by looking at the variance of front distance measurements over time; the ‘stuck’ condition is declared when the front-distance variance falls below a pre-determined value. A typical ‘stuck’ recovery maneuver is to back up slightly, and then make a right-angle turn away from the wall currently being tracked. This maneuver, while usually successful, has the same problem as the simple wall-to-wall transition; it sometimes results in the same backward-up-the-wall climb, with similar results.

The ‘suddenly appearing obstacle’ case can be handled in a manner similar to ‘stuck’ detection, but bypassing the variance measurement stage. and the resulting avoidance maneuver is similar to the ‘stuck’ case

Wall-E2 currently handles all of the above cases fairly well, except when it backs into something while maneuvering to avoid the detected obstacle. So, my challenge was to find a way to avoid running into something while backup up from something else. The easy answer to this problem was to add a rear-distance sensor to Wall-E2, and then use that information to modify obstacle-avoidance behavior as necessary.

During the changeover from ‘ping’ style distance sensors to left and right 3-element arrays of VL53L0X time-of-flight sensors I learned quite a bit about the care and feeding of the VL53L0X, and also wound up with quite a few spares. So, I took one of the spares and installed it on the rear ‘bumper’ plate on Wall-E2, as shown in the following photo:

Since the 2nd-deck Teensy 3.5 was already handling both 3-element VL53L0X arrays, I simply added the rear sensor to the left-hand array ‘Wire2’ daisy-chain, and connected its XSHUT pin to Teensy pin 8. Then I modified the Teensy’s program to initialize and poll the rear sensor in the same manner as all the others, and tested it to make sure it was responding properly to rear-aspect obstacles.

The next step is to incorporate rear-aspect distance information into the various obstacle avoidance algorithms in the main program.

‘Stuck’ case:

The ‘stuck’ case by definition occurs when the mathematical variance of the last 3-5 seconds of forward distance measurements fall below a set value, indicating that the robot is no longer moving forward or backward. When this happens while wall tracking, the robot has to decide what to do. The current response is to back up for 1 second at half speed, execute a 90 deg ‘spin turn’ away from the nearest wall and then go back to normal operations.

I think I would like to enhance this algorithm as follows:

If the measured front distance is less than MAX_FRONT_DISTANCE_CM (currently set at 400 cm) by at least STUCK_BACKUP_DISTANCE_CM (currently set at 25), then back up by STUCK_BACKUP_DISTANCE_CM using front distance measurements as the primary means of terminating the backup maneuver. If the front distance measurement cannot be used, but the rear distance measurement is valid (less than MAX_REAR_DISTANCE_CM, currently set at 100), then back up using the rear sensor measurement. If neither measurement is available, then revert back to a 1 second half-speed movement. In all cases, use the rear distance measurement to prevent ‘reverse wall climb’ by stopping the motors if the robot gets too close to an obstacle while backing up.
Execute a ‘spin turn’ away from the nearest wall – this is the same as the current algorithm.
Execute a ‘rolling turn’ back toward the original direction of travel. This should offset the robot further away from the nearest wall, and hopefully allow it to bypass the obstacle.

Replacing Wall-E2’s L298N Motor Drivers with Adafruit DRV8871

1 Reply

Posted 07 September 2020,

I’ve been having some ‘issues’ with driving Wall-E2’s Pololu 20D 125:1 12V metal geared motors with the old L298N motor drivers, so I thought it was time to replace them with the Adafruit DRV8871 models used in ‘re-motoring’ my two-wheel robot (see this post for some of the details).

The first step was to review the work I had done earlier replacing the L298N on my two-wheel robot with the same DRV8871 Driver. The two-wheel robot uses a UNO controller, while Wall-E2 uses a Mega, but they are similar enough so porting the wiring and code should be simple enough. The two-wheel robot uses UNO pins 5, 6, 9 & 10 (all PWM lines) for direction and speed control, while Wall-E2 uses pins 8-13 and 36, 38, 40, 42, 44 & 46 for controlling the two L298N motor drivers. My plan is to try using just two motor drivers, one for both left motors, and another for both right motors. If this works, I’ll need just 4 lines, say 8-11 (If I later need to use four drivers vs two, I’ll use 12/13 for one of the ‘extras’ and 6/7 for the other one. Currently 6 is unused and 7 drives the red laser diode, so moving it shouldn’t be a big problem).

So, I replaced the two L298N modules with two DRV8871 modules, and wired both left motors into one driver and both right motors into the other, as shown in the following photos

‘after’ – Dual Adafruit DRV8871 motor drivers

Since I previously replaced an L298N with a DRV8871 in my two-wheel robot, I had already modified all the relevant motor driver code to use the DRV8871 module vs the L298N, so all I had to do was replace the low-level motor interface modules in my four-wheel robot project with the corresponding ones from my two-wheel robot project. The replaced modules were:

SetLeftMotorDirAndSpeed
SetRightMotorDirAndSpeed
StopBothMotors
MoveAhead
MoveReverse

Note that the four-wheel robot code uses separate SetLeft/RightMotorDir & SetLeft/RightMotorSpeed functions, so these needed to be modified or replaced.

SetLeft/RightMotorSpeed() is called from RunBothMotors() & SpinTurn(). I could modify SpinTurn() to call RunBothMotors() instead of calling SetLeft/RightMotorSpeed() directly.

RunBothMotors() is called from RunBothMotorsMsec(), Setup(), MoveReverse() and MoveForward(). Every call to RunBothMotors() is paired with calls to SetLeft/RightMotorDir(), so I could replace Each set of calls with a single call to SetLeft/RightMotorDirAndSpeed(). The only issue with this are the RunBothMotorsMsec() calls in Setup() & ExecDisconManeuver().

I decided to modify the RunBothMotors() & RunBothMotorsMsec() functions to take a direction parameter, and then the functions will simply call SetLeft/RightDirAndSpeed().

10 September 2020 Update:

I got lost in the details of the Wall-E2 code, so I decided to simplify things to check out the new DRV8871-based motor setup. I modified my original ‘Adafruit_DRV8871_Driver_Test’ project to run both motor sets forward and backward from the minimum PWM value (about 50) to max (255) while monitoring the total current for all four motors, as shown in the following Excel plot

Total motor current for motor speed commands from 50-255

As can be seen from the above plot, the total motor current for all four motors is right around 0.8A or about 0.4A per driver – well within the current limits for the DRV8871 module. As a side note, the TO-3 cans on the modules barely got warm, so this looks like a real winner.

Now that I have the technical issues sorted out with respect to the driver replacement project, I can get back to the main project of improving Wall-E2’s wall-following ability.

Stay tuned!

Frank

13 September 2020 Update:

Unfortunately, when I started running the full Wall-E2 code, the right set of motors would go backwards, but not forwards – awkward to say the least. After a lot of troubleshooting, it finally dawned on me that the problem was being caused by my introduction of a TIMER1-based interrupt a while ago to manage ‘stuck’ detection. TIMER1 controls PWM on pins 11 & 12, and one of those pins was being used by the right motor – bummer!

After a LOT of screwing around, I finally decided that the only way to really figure things out was to remove everything but the timer and motor driver code from the program to figure out what timer (if any) I can use for the ‘stuck’ detection interrupt and still have proper motor control.

To that end, I created a new Arduino program ‘TimerISRvsPWMTest.ino’ as shown below:

/*
    Name:       TimerISRvsPWMTest2.ino
    Created:	9/15/2020 11:25:13 AM
    Author:     FRANKNEWXPS15\Frank

		Program to test interaction between TIMER1 ISR and PWM for motor control
*/

#include <PrintEx.h> //allows printf-style printout syntax

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

#pragma region MOTOR_PINS
//09/11/20 Now using two Adafruit DRV8871 drivers for all 4 motors
const int In1_Left = 8;
const int In2_Left = 9;

//Right Motors paralleled to left DRV8871 driver
const int In1_Right = 10;
const int In2_Right = 11;
#pragma endregion Motor Pin Assignments

#define TIMER_INT_OUTPUT_PIN 31 //scope monitor pin

void setup()
{

	Serial.begin(115200);
	delay(1000);


#pragma region I/O PIN SETUP
	//09/11/20 now using two DRV8871 motor drivers for all four motors
	pinMode(In1_Left, OUTPUT);
	pinMode(In2_Left, OUTPUT);
	pinMode(In1_Right, OUTPUT);
	pinMode(In2_Right, OUTPUT);
#pragma endregion I/O PIN SETUP

	//
#pragma region TIMER_INTERRUPT
	//08/10/20 added Timer interrupt
	//set timer1 interrupt at (1000/MIN_PING_INTERVAL_MSEC)Hz 5Hz a/o 08/10/20
	cli();//stop interrupts
	TCCR1A = 0;// set entire TCCR1A register to 0
	TCCR1B = 0;// same for TCCR1B
	TCNT1 = 0;//initialize counter value to 0

	// set compare match register for 5hz increments
	OCR1A = 3124;// = (16*10^6) / (5*1024) - 1 (must be <65536)
	TCCR1B |= (1 << WGM12);// turn on CTC mode
	TCCR1B |= (1 << CS12) | (1 << CS10);// Set CS10 and CS12 bits for 1024 prescaler
	TIMSK1 |= (1 << OCIE1A);// enable timer compare interrupt
	sei();//allow interrupts

	pinMode(TIMER_INT_OUTPUT_PIN, OUTPUT);
#pragma endregion TIMER_INTERRUPT

	while (1)
	{
		digitalWrite(In1_Right, LOW);
		digitalWrite(In2_Right, LOW);
		digitalWrite(In2_Left, LOW);
		digitalWrite(In1_Left, LOW);
		delay(1000);

		digitalWrite(In1_Right, LOW);
		analogWrite(In2_Right, 128);

		digitalWrite(In2_Left, LOW);
		analogWrite(In1_Left, 128);

		delay(2000);

		digitalWrite(In2_Right, LOW);
		analogWrite(In1_Right, 128);


		digitalWrite(In1_Left, LOW);
		analogWrite(In2_Left, 128);

		delay(2000);
	}
}
ISR(TIMER1_COMPA_vect) //timer1 interrupt 1Hz toggles pin 13 (LED)
{
	digitalWrite(TIMER_INT_OUTPUT_PIN, HIGH);
	delayMicroseconds(10);
	digitalWrite(TIMER_INT_OUTPUT_PIN, LOW);
}


//ISR(TIMER5_COMPA_vect) //timer1 interrupt 1Hz toggles pin 13 (LED)
//{
//	digitalWrite(TIMER_INT_OUTPUT_PIN, HIGH);
//	delayMicroseconds(10);
//	digitalWrite(TIMER_INT_OUTPUT_PIN, LOW);
//}

void loop()
{

}

100

101

102

103

Name: TimerISRvsPWMTest2.ino

Created: 9/15/2020 11:25:13 AM

Author: FRANKNEWXPS15\Frank

Program to test interaction between TIMER1 ISR and PWM for motor control

#include <PrintEx.h> //allows printf-style printout syntax

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

#pragma region MOTOR_PINS

//09/11/20 Now using two Adafruit DRV8871 drivers for all 4 motors

const int In1_Left = 8;

const int In2_Left = 9;

//Right Motors paralleled to left DRV8871 driver

const int In1_Right = 10;

const int In2_Right = 11;

#pragma endregion Motor Pin Assignments

#define TIMER_INT_OUTPUT_PIN 31 //scope monitor pin

void setup()

{

Serial.begin(115200);

delay(1000);

#pragma region I/O PIN SETUP

//09/11/20 now using two DRV8871 motor drivers for all four motors

pinMode(In1_Left, OUTPUT);

pinMode(In2_Left, OUTPUT);

pinMode(In1_Right, OUTPUT);

pinMode(In2_Right, OUTPUT);

#pragma endregion I/O PIN SETUP

#pragma region TIMER_INTERRUPT

//08/10/20 added Timer interrupt

//set timer1 interrupt at (1000/MIN_PING_INTERVAL_MSEC)Hz 5Hz a/o 08/10/20

cli();//stop interrupts

TCCR1A = 0;// set entire TCCR1A register to 0

TCCR1B = 0;// same for TCCR1B

TCNT1 = 0;//initialize counter value to 0

// set compare match register for 5hz increments

OCR1A = 3124;// = (16*10^6) / (5*1024) - 1 (must be <65536)

TCCR1B |= (1 << WGM12);// turn on CTC mode

TCCR1B |= (1 << CS12) | (1 << CS10);// Set CS10 and CS12 bits for 1024 prescaler

TIMSK1 |= (1 << OCIE1A);// enable timer compare interrupt

sei();//allow interrupts

pinMode(TIMER_INT_OUTPUT_PIN, OUTPUT);

#pragma endregion TIMER_INTERRUPT

while (1)

{

digitalWrite(In1_Right, LOW);

digitalWrite(In2_Right, LOW);

digitalWrite(In2_Left, LOW);

digitalWrite(In1_Left, LOW);

delay(1000);

digitalWrite(In1_Right, LOW);

analogWrite(In2_Right, 128);

digitalWrite(In2_Left, LOW);

analogWrite(In1_Left, 128);

delay(2000);

digitalWrite(In2_Right, LOW);

analogWrite(In1_Right, 128);

digitalWrite(In1_Left, LOW);

analogWrite(In2_Left, 128);

delay(2000);

}

ISR(TIMER1_COMPA_vect) //timer1 interrupt 1Hz toggles pin 13 (LED)

{

digitalWrite(TIMER_INT_OUTPUT_PIN, HIGH);

delayMicroseconds(10);

digitalWrite(TIMER_INT_OUTPUT_PIN, LOW);

}

//ISR(TIMER5_COMPA_vect) //timer1 interrupt 1Hz toggles pin 13 (LED)

//{

// digitalWrite(TIMER_INT_OUTPUT_PIN, HIGH);

// delayMicroseconds(10);

// digitalWrite(TIMER_INT_OUTPUT_PIN, LOW);

//}

void loop()

{

}

With the TIMER1 setup commented out, both motors rotate forwards and backwards no problem. However, as soon as the TIMER1 code was un-commented, the motor driver on pins 10/11 would turn one way but not the other. With an O’scope I could see the PWM waveform on pin 10 but not on pin 11. This is consistent with Timer1 controlling PWM on pins 11,12 & 13.

So, I moved the wire connected to DRV8871 IN2 from 11 to 3 and changed the code to use pins 10 & 3 vs 10 & 11. Now the motor connected to these pins rotates both forward and backwards

Then I went back to my WallE2_V6 project and tried the same trick – moving the IN2 pin from 11 to 3; nope – still doesn’t work. In fact, moving from 11 to any other PWM pin doesn’t work in the WallE2 project, but does in the TimerPWM test project – weird.

So, thinking that maybe there were some timer dependencies hidden in one or more of the libraries being used for the WallE2 project, I copied them all over to the TimerPWM project folder, added them to the project in VS2019, and added them to the project code. After I got everything to compile, I ran the TimerPWM test project successfully using 10 & 7, 10 & 3, 10 & 2, etc (but 10 & 11 still doesn’t work with TIMER1 ISR enabled).

So, it’s not the libraries. Next I changed the timer interrupt code to use TIMER5 instead of TIMER1, moving the pin dependency from 11-13 to 44-46. I confirmed this by changing ‘In1_Right‘ to 44, 45, 46 in turn and moving the physical In1_Right connection to the corresponding pin, noting that the motors don’t rotate properly when driven from any of the affected pins.

Next, I changed the timer interrupt code in WallE2_V6 from TIMER1 to TIMER5 to see if I can get back pin 11 as a PWM pin. Nope – it still doesn’t work in the WallE2 code, nor does pin 7.

Back to the test program. Copied all the ‘pre-setup’ code from WallE2 to the test program; no change – test program still works properly.

After a few back-and-forths of this nature, I eventually narrowed the problem down to the driver modules themselves. A physical inspection revealed that I had forgotten to solder the second pin on both 2-pin screw terminals on one of the drivers – oops! So, like many seemingly intractable technical problems, this one was caused by two independent issues; the use of TIMER1 caused pins 10-12 to be unavailable for motor drive PWM, and the intermittent connections to the left-side motors complicated the symptoms. This was a perfect example of why ‘cutting the problem in half’ (in my case, by eliminating all the Wall-E2 hardware from the problem) is so effective in troubleshooting.

Stay tuned!

Frank