Lab 3

Net ID: pp386

Objective:

This lab’s main objective is to get accustomed with the TOF sensors and the IMU and also testing them and their communication channels through I2C.

Pre-Lab and Setup:

This lab deals with the Time of Flight sensor (ToF) which is based on the VL53L1X. I started by reading the datasheet and the manual of the ToF sensor and understood how the sensor functioned and communicated with the board (I2C). Thereafter, I noted down the I2C address of the ToF sensor to be 0x52.
Since we have to use 2 ToF sensors together on the robot and the sensors both have the same address (0x52), we need to come up with a solution to separate the addresses of the 2 sensors. Therefore, I have connected one of the ToF sensors’ shut down pin to the Artemis and then on startup, I have manually shut it down using the digitalWrite(4,LOW) command. Then I manually change the address of the ToF sensor which is turned on to (0x50) using the command setI2CAddress((uint8_t)0x50); and then turn the sensor which was shutdown back on using the digitalWrite(4,HIGH). I decided to use this approach over using 2 shutdown pins on the two sensors as I would be saving 1 wire and thus the robot will have less wires. On the other hand, the address needs to be changed on startup everytime and hence has a bit of a programming overhead.
Both the ToF sensors have a range of 15 degrees to 27 degrees and hence it is a tricky situation. I tried to search up different orientation setups for ToF sensors to minimize blind spots and so far in most cases I have seen that more than 2 ToF sensors are used for the robot. For our case, I can think of 3 possible orientations:

1. ToF sensors at 90 degrees to each other; one facing the front of the car, one facing the side

2. At 180 degrees with each other; one facing the front of the car, one facing the back of the car

3. At 45 degrees with each other; both in the front, one facing Northeast and one Northwest

The ToF sensors have a horizontal range of 15 to 27 degrees and I am baffled as to which 54 degrees are the most important for our car and how do I need to place our sensors for our specific purpose in this lab to minimize crashing. I am leaving it up to the later labs to decide where to place the sensors. I also read the datasheet of the IMU and learned about its features.

The figure above depicts the circuit configuration I have used. I have permanently soldered the sensors and the battery to the connector but kept the Artemis connected with a Qwic connector which is removable. I have daisy chained the sensors.

Lab 3(a) Time of Flight Sensors

1. After running the Example1_wire, I see that the address that is detected is 0x29. This comes as a surprise since the datasheet mentions that the ToF sensor’s address is 0x52. After asking the TAs I came to know that there is a right shift happening in the address and hence it is converted from 0x52 to 0x29.
2. The ToF sensor has three distance modes: Short mode, Medium mode and Long mode. From the manual we can see that the Long mode is the default mode and works for distances upto 4m, the medium mode works for upto 3m and the short mode works for upto 1.3m . From the manual I learned that the short mode is the best for ambient immunity and the long mode is best for longer distances.
3. For testing the accuracy of the sensor, I have experimented with different obstacles at different distances from the sensor. My setup first consists of several post it notes placed 20 cm apart from each other. The image below describes the setup. I set my sensor to the short distance mode as it gives the best readings for distances less than 1.3m. The first obstacle I used was a white post it note bunch placed at a distance of 20 cm and 40cm from the sensor. The sensor was able to detect it with reasonable accuracy (~1cm). After that when I placed the white post it note at a distance of 60 cm, the sensor was not able to detect it. I put a small deodorant bottle at 60cm which was of black color and the sensor was still not able to detect it.
   
   Therefore, I decided to put a big brown box as an obstacle in front of the sensor. Now as the obstacle size increased, the sensor was able to detect the object and gave reasonable accuracy for 60cm 80cm 100cm and 120cm.
   With this I concluded that the sensor is not able to detect small objects which are more than 40 or 50 cm away.
   The graph below shows the sensor readings. For each of the 6 distances, I took a total of 34 readings from the serial monitor and plotted in Microsoft Excel.
   
   The mean and the standard deviation of the readings are:

Actual distance	Mean of sensor readings	Std dev of sensor readings
200 mm	212.0294118	1.42457424
400 mm	419	5.624621199
600 mm	613.2352941	5.146689572
800 mm	877.5588235	14.94878363
1000 mm	1063.558824	22.95079213
1200 mm	1202.852941	12.68531089

Next, I have tried to see the maximum range of the sensor. When I pointed the sensor towards the ceiling from the ground, I got a distance measurement of 0 which means that the sensor is not able to pick up the reading. The maximum reading I could get was around 2300 mm even after setting the distance mode to long. This is in stark contrast with the claimed value of 3600 mm in the datasheet which I think would only occur for some ideal cases and not real world applications.
I also tried different colours at short range but I couldnt see a lot of difference in the readings to the naked eye.
For the sensor ranging time, I noted the time value before and after sensor readings were taken and hence calculated the time taken for each sensor reading. I calculated the time taken for each sensor reading to be 54 ms with the Stop Ranging function taken into consideration and 51 ms without it.

1. Hooking up 2 ToF sensors simultaneously.

Code for 2 ToF sensors hooked up together

SFEVL53L1X distanceSensor2;
SFEVL53L1X distanceSensor;

//Uncomment the following line to use the optional shutdown and interrupt pins.
//SFEVL53L1X distanceSensor(Wire, SHUTDOWN_PIN, INTERRUPT_PIN);

void setup(void)
{
  Wire.begin();
  digitalWrite(4, LOW);
  distanceSensor.setI2CAddress((uint8_t)0x50);
  digitalWrite(4, HIGH);
  Serial.begin(115200);
  Serial.println("VL53L1X Qwiic Test");

  if (distanceSensor.begin() != 0) //Begin returns 0 on a good init
  {
    Serial.println("Sensor failed to begin. Please check wiring. Freezing...");
    while (1)
      ;
  }
  if (distanceSensor2.begin() != 0) //Begin returns 0 on a good init
  {
    Serial.println("Sensor failed to begin. Please check wiring. Freezing...");
    while (1)
      ;
  }
  Serial.println("Sensor online!");
  distanceSensor.setDistanceModeShort();
  distanceSensor2.setDistanceModeShort();
  //distanceSensor.setIntermeasurementPeriod(100);
  //distanceSensor2.setIntermeasurementPeriod(100);
  
}

void loop(void)
{ float time0 = millis();
  distanceSensor.startRanging();
  distanceSensor2.startRanging();//Write configuration bytes to initiate measurement
  while (!distanceSensor.checkForDataReady())
  {
    delay(1);
  }
  while (!distanceSensor2.checkForDataReady())
  {
    delay(1);
  }
  int distance = distanceSensor.getDistance(); //Get the result of the measurement from the sensor
  distanceSensor.clearInterrupt();
  float time1 = millis();
  distanceSensor.stopRanging();
  int distance2 = distanceSensor2.getDistance(); //Get the result of the measurement from the sensor
  distanceSensor2.clearInterrupt();
  distanceSensor2.stopRanging();
  float time2 = millis();
  Serial.print("Distance(mm): ");
  Serial.print(distance);
  float distanceInches = distance * 0.0393701;
  float distanceFeet = distanceInches / 12.0;
  Serial.print("\tDistance(cm): ");
  Serial.print(distance/10.0, 2);
  Serial.print("\tDistance(ft): ");
  Serial.print(distanceFeet, 3);
  Serial.print("\tDistance2(mm): ");
  Serial.print(distance2);
  float distanceInches2 = distance2 * 0.0393701;
  float distanceFeet2 = distanceInches2 / 12.0;
  Serial.print("\tDistance2(cm): ");
  Serial.print(distance2/10.0, 2);
  Serial.print("\tDistance2(ft): ");
  Serial.print(distanceFeet2, 3);
  Serial.print("\tTime(ms): ");
  Serial.print(time1-time0);
  Serial.println();
}

Additional Tasks

1. Many distance sensors are based on infrared trasmission. Discuss a couple, highlight the differences in their fuctionality and the pros/cons of each.
   Answer: The 3 sensors that I am going to discuss are ST Microelectronics VL53L1CX (one used by us) and the previous generation of IR ToF sensors from ST Microelectronics, the VL6180X and the VL53L0CX.
   All the 3 sensors are IR ToF and have different features and use-cases. The table below describes the differences:

Parameter	VL6180X	VL53L0CX	VL53L1CX (Ours)
Range Modes	1	1	3 (short, medium, long)
Range	Upto 100 mm	50-1200 mm	Upto 3600 mm
Horizontal Field of View	42 degrees	25 degrees	Programmable from 15 degrees to 27 degrees
Technology	VCSEL	VCSEL	SPAD array
Interface	I2C	I2C	I2C
Pros	Good for short distance measurements, has an inbuilt ambient light sensor which helps it to function with change in ambient lighting conditions, Very good horizontal field of view.	Better range and only specialized for white objects.	3 modes to tailor to our purpose, highest range of upto 3.6m.
Cons	Very short range, cannot be used for distances more than 10 cm	Range shrinks to 800 mm when used outdoors due to ambient light.	Limited horizontal field of view

Source: https://www.digikey.com/en/articles/fundamentals-distance-measurement-gesture-recognition-tof-sensors

1. Try out some settings, and discuss your choice of settings related to the anticipated speed of your robot. Answer: There are 2 parameters for the timing of the sensors readings which are the Timing Budget and the Intermeasurement period. The Timing budget is the time it takes for the sensor to take the reading or one range measurement. For our ToF sensors, the timing budget can be set to any value from 20ms to 1000ms. The intermeasurement period is the time the sensor gets successive readings. If the intermeasurement period is set to less than the timing budget, the sensor immediately starts the second ranging after the first one ends.
   I tried out some combinations of intermeasurement periods and sensor timing budgets and found out that the lowest timing budget of the sensors is 20 ms and the intermeasurement period is set to 0. I observe that the time in between 2 readings in a sensor can be minimized to 22ms. For the robot, I would like the readings be refreshed as fast as possible because more measurements mean more optimizations to the robot path and hence, the robot can follow a better optimal path and never crash. It might also be a bit overkill, but being safe is better than being sorry.
2. Failure Modes: From the user manual, I see the following definitions: Sigma is expressed in mm and is the estimation of the standard deviation of the measurement.
   The signal rate measurement, expressed in MCPS, represents the amplitude of the signal reflected from the target and detected by the device. I tried moving my hand jerkily and super fast towards the sensors and saw a wrapped mode failure and signal fail.
   
   This means that the sensor failed to get a reliable reading at that moment and hence the reading is not appropriate for our calculations. Therefore the reading may be discarded.

Lab 3(b) IMU

IMU or inertial measurement unit is a sensor which has a gyroscope, accelerometer and the magnetometer. For our lab, we are using the ICM20948 9DoF Motion Sensor which is a 9 Degrees of Freedom IMU. Setting up the IMU:
For setting up the IMU, I have used its example code and Arduino Library. For the AD0_VAL it should be 1 by definition but I got a data underflow using it as 1 and hence set AD0_VAL to 0. According to the sketch, when the ADR jumper is closed, the value becomes 0.
The figure below depicts what I get when I tried to rotate, flip and accelerate the board. When I tried to rotate it, the gyroscope and the magnetometer readings changed and when I tried to accelerate it, the accelerometer value changed and we can see in the image.

Accelerometer

For the Pitch and Roll calculations, I have used the equations given in the class. Pitch= atan2(ay,az)180/pi Roll= atan2(ax,az)180.pi 90degrees ROLL

-90 degrees ROll

0 degrees ROLL and PITCH

90 degrees PITCH

-90 degrees PITCH

The accelerometer works pretty well considering the outputs we have gotten except the curious case of faulty roll or pitch values at 90 degrees or -90 degrees for 2 cases. This can be because of the sensor’s error.

Code:

float Pitch= atan2(sensor->accX(),sensor->accZ())*180/M_PI;
  //SERIAL_PORT.print("Pitch:");
  printFormattedFloat(Pitch,3,2);
  float Roll= atan2(sensor->accY(),sensor->accZ())*180/M_PI;
  SERIAL_PORT.print("\t");
  printFormattedFloat(Roll,3,2);
  SERIAL_PORT.println();

Tapping on the IMU and plotting the accelerometer frequency response

I tapped on the IMU sensor for 2 seconds out of the 10 seconds and got the following time domain and frequency domain results. The sampling time which I calculated was 1000/33 or 30ms. Therefore now I will compute the RC constant for filter. The cutoff frequency should be around 5 Hz from intuition. RC=1/2pif
RC=0.0318 alpha=0.485 Designing low pass filter with alpha is possible with the code given below.

float Pitch= atan2(sensor->accX(),sensor->accZ())*180/M_PI;
SERIAL_PORT.print("Pitch:");
printFormattedFloat(Pitch,3,2);
float Roll= atan2(sensor->accY(),sensor->accZ())*180/M_PI;
SERIAL_PORT.print("\t");
printFormattedFloat(Roll,3,2);
float Roll_Filtered=0.485*Roll + (1-0.485)*Roll_Previous;
Roll_Previous=Roll_Filtered;
float Pitch_Filtered=0.485*Pitch + (1-0.485)*Pitch_Previous;
Pitch_Previous=Pitch_Filtered;

Low pass filter essentially only allows the lower frequencies to pass and attenuate the higher frequencies which are higher than the cutoff frequency. Therefore, if we set the cutoff frequency to around ###Gyroscope Pitch, roll and yaw values can be computed from the gyroscope readings by integrating each of the values over some time.

current_time=millis();
delta_t=current_time- previous_time;
float Roll_Gyro=Roll_Gyro- (sensor->gyrX())*delta_t/1000;
previous_time=current_time;

The values of roll and pitch calculated from the gyroscope were trash and unreliable. Just seeing those values above, I am never going to make the mistake of using a gyroscope for calculating roll and pitch.

Now using a complementary filter for removing the error, I am going to use the Roll and Pitch values with an alpha of 0.485. Like the previous output for accelerometer.

float Roll_Filtered_Gyro=0.485*Roll_Gyro + (1-0.485)*Roll_Previous_Gyro;
Roll_Previous=Roll_Filtered;
float Pitch_Filtered_Gyro=0.485*Pitch_Gyro + (1-0.485)*Pitch_Previous_Gyro;
Pitch_Previous=Pitch_Filtered;

Magnetometer (additional task)

Yaw calculation using the given equation.
My magnetometer seems to be faulty as after some time the magnetometer values freeze and dont change anymore even after moving it around a lot.
I couldnt do the magnetometer north calculations due to the faulty magnetometer.

xm = myICM.magX()*cos(pitch) - myICM.magY()*sin(roll)*sin(pitch) + myICM.magZ()*cos(roll)*sin(pitch); 
ym = myICM.magY()*cos(roll) + myICM.magZ()*sin(roll_rad); 
yaw = atan2(ym, xm);

Calculation of Yaw:

Back to Home