Sensors

0. Objective

The purpose of this lab is to soldering pins and tune ToF / IMU sensors - the faster the robot can sample and the more it can trust a sensor reading, the faster it is able to drive.

1. Soldering and connections setup

Consider the long term scalable and easy maintenance, I bought a SparkFun Qwiic MultiPort for make connections between Artemis board to other sensors, and a SparkFun Qwiic Cable Kit used for wiring to sensors.

Here are the two ToF sensors with right angle male pin header soldered on. One will mount at the front of the vehicle, and the other one mount at the side (Vertical mounting).

And here is the IMU with a right angle female socket header soldered on. In this case, the IMU can be placed vertically or horizontally and only need a thin space inside the vehicle.

On the Artemis Nano board side, I soldered a blue Amphenol 76382-307LF male pin header on the side which have the Qwiic port, considered connections between ToF XSHUT pins and future motor controller transmission with the Artemis, totally I need 2 XSHUT + 4 motor control logic = 6 pins. Also, a NKK AS13AP slide switch has been soldered on the PSWC pins, this makes less plugging-unplugging damage on the USB-C port when I debug sensors frequently. (I had an ESP8266 board with the micro-USB socket dead due to the frequent plugging and unplugging, solder pads on that board have been pulled-off and it’s hard to fix it and solder a new socket connector…)

All in one.

2. Time of Flight Sensors

Only one ToF

Based on the following connection setup.

Uploaded Example05_Wire_I2C, got ToF I2C address 0x29. This is what I expected, since the Pololu VL53L1X Description states

“The sensor’s 7-bit slave address defaults to 0101001b on power-up.”

Two ToFs

Based on the following connection setup.

First, define XSHUT pins.

1
2
#define TOF1_SHUTDOWN_PIN 2
#define TOF2_SHUTDOWN_PIN 3

Second, create two ToF objects.

1
2
SFEVL53L1X TOF1;
SFEVL53L1X TOF2;

Third, in the setup part, set pin mode for both XSHUT pins, and set them to logical low.

1
2
3
4
pinMode(TOF1_SHUTDOWN_PIN, OUTPUT);
pinMode(TOF2_SHUTDOWN_PIN, OUTPUT);
digitalWrite(TOF1_SHUTDOWN_PIN, LOW);
digitalWrite(TOF2_SHUTDOWN_PIN, LOW);

Fouth, start up the first ToF sensor with 0x29 I2C address.

1
2
3
4
5
6
7
8
9
delay(50);
digitalWrite(TOF1_SHUTDOWN_PIN, HIGH);
delay(50);

if (TOF1.begin() != 0) //Begin returns 0 on a good init
    Serial.println("TOF1 failed to begin. Please check wiring.");
else
    TOF1.setI2CAddress(0x29);
Serial.println("TOF1 online!");

Fifth, start up the second ToF sensor with 0x30 I2C address.

1
2
3
4
5
6
7
8
9
delay(50);
digitalWrite(TOF2_SHUTDOWN_PIN, HIGH);
delay(50);

if (TOF2.begin() != 0) //Begin returns 0 on a good init
    Serial.println("TOF2 failed to begin. Please check wiring.");
else
    TOF2.setI2CAddress(0x30);
Serial.println("TOF2 online!");

Finally, start ranging on both ToF sensors, get distance data and print out both results in centimeters to serial.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
void loop(void)
{
  TOF1.startRanging(); //Write configuration bytes to initiate measurement
  TOF2.startRanging(); //Write configuration bytes to initiate measurement
  while (!TOF1.checkForDataReady())
  {
    delay(1);
  }
  while (!TOF2.checkForDataReady())
  {
    delay(1);
  }
  int distance1 = TOF1.getDistance(); //Get the result of the measurement from the sensor
  int distance2 = TOF2.getDistance(); //Get the result of the measurement from the sensor
  TOF1.clearInterrupt();
  TOF2.clearInterrupt();
  TOF1.stopRanging();
  TOF2.stopRanging();

  Serial.print("Distance(mm): ");
  Serial.print(distance1);
  Serial.print(", ");
  Serial.print(distance2);

  Serial.println();
}

Successfully read data from both ToF sensors.

Distance Mode Experiment

Note: This experiment collaborate with Tongqing Zhang (TZ422).

We set a ruler on the ground, make the 450cm position as the zero point, use a tape to mount two sensors on the back of the laptop screen, one sensor tunes to the short mode and the other one tunes to long mode (setDistanceModeMedium() is not a callable function nor implement in the VL53L1X library according to HERE, thus we disregarded this and only test for short and long). The experiment operation is one person moves the box backward and the other person records the data.

To calibrate the laptop screen angle, make sure it is perpendicular to the ground, in order to avoid offsets when measuring long distances, we use the Bosch GLM 20 laser measure to enforce the actual distance.

Below are the data we captured. It seems the Short mode stop working when we go over 240 cm, but based on accuracy, the short mode always stands out. The long mode is inaccurate especially when measure longer distances.

Later I found this table from the VL53L1X Datasheet talking about ambient light parameter. We redo the test and found out there is no significant difference between dark and bright environments. Different color surface makes very tiny impact. But the sensor is sensitive to different materials, since they deflects light differently.

Based on this experiment, I would use the short distance mode since it gives me high accuracy.

3. Additional tasks

1. Ultrasonic distance sensor emits high-frequency sound waves towards the target object and receives the reflects back, using the time differences to calculate the distance. They are not affected by object colors or ambient lights, but the low resolution, slow refresh rate, and limited working distances would be a pain, and they are not suitable for fast-moving objects. LiDAR measures the range of targets through light waves from a laser, instead sound waves. This type sensors have high accuracy no matter it’s in extremely dark or bright environments, also has the higest refresh rate for fast-moving objects. But they are expensive cost and since it emitting laser, can damage human eyes.

2. Based on data sheet and the Qwiic Distance Sensor Hookup Guide, the timing budget is the amount of time over which a measurement is taken. This can be set to any of the following: 20, 33, 50, 100 (default), 200, 500. The 20ms is only available in short distance mode. On the other hand, the setIntermeasurementPeriod() allows to change the time alotted for a measurement, default is 100ms.

3. The goal is to get high sampling rate, avoid Signal Fail event, like overclocking CPU. The default settings of measurement period and timing budget have low sampling rate, after serval test with these two functions, I found out the below settings would be a stable one and gives me 25 Hz.

1
2
3
distanceSensor.setDistanceModeShort();
distanceSensor.setTimingBudgetInMs(20);
distanceSensor.setIntermeasurementPeriod(50);

The range status keeps “Good” while the distance changes rapidly. To enlarge this screenshot, right click it and open in a separate window.

4. IMU

Setup the IMU

First, here is the wiring setup.

Second, I flashed Example05_Wire_I2C.ino first to detect IMU address, and I got 0x68.

Third, open the \Arduino\libraries\SparkFun_ICM-20948\SparkFun_ICM-20948_ArduinoLibrary-master\examples\Arduino\Example1_Basics. On the line #22, the AD0_VAL (address value at the zero bit) should be set to 0, since the LSB of 0x68 is zero. Also, there is a ADDR+1jumper on the back side of the IMU, the jumper is closed by default.

1
#define AD0_VAL 0

There is a reference frames sign on the IMU surface.

I discovered when I give a acceleration towards to one of the arrow direction, the acceleration on that axis reading is negative. When I move the IMU suddenly to one direction, the gyroscope generates a pulse-liked curve. It will first goes positive(negative) side then negative(positive) side and finally goes back to zero. Also, the gyroscopehas no reading when it is idle(no movement), but accelerometer has the reading, since the g-force pulls down.

Accelerometer

1. Use atan2 and M_PI to convert accelerometer data into pitch and roll.

1
2
3
4
5
6
7
8
9
float pitch = 180 * atan2(sensor->accX(), sensor->accZ()) / M_PI;
float roll = 180 * atan2(sensor->accY(), sensor->accZ()) / M_PI;

SERIAL_PORT.print("Pitch: ");
printFormattedFloat(pitch, 3, 2);
SERIAL_PORT.print("°    Roll: ");
printFormattedFloat(roll, 3, 2);
SERIAL_PORT.print("°");
SERIAL_PORT.println();

I put the IMU on my desk and hold it on my side of desk to measure {09, 0, 90} cases.

Here are the results I get.

Do the calibration according to my results:

1
2
float pitch = ((180 * atan2(sensor->accX(), sensor->accZ()) / M_PI) - 88.33)*180 / (-88.79 + 87.18) - 90;
float roll = ((180 * atan2(sensor->accY(), sensor->accZ()) / M_PI) - 89.25)*180 / (88.50-89.25) - 90;

1. Tapping the sensor

   Gyroscope

   In order to compute pitch, roll and yaw angles, here is the code I use.

1
2
3
4
5
6
7
8
9
10
11
12
dt = millis() - last_time;
last_time = millis();

pitch = 180 * atan2(sensor->accX(), sensor->accZ()) / M_PI;
roll = 180 * atan2(sensor->accY(), sensor->accZ()) / M_PI;
pitch_g += pitch*dt/1000;
roll_g += roll*dt/1000;
SERIAL_PORT.print("Pitch_g: ");
printFormattedFloat(pitch_g, 3, 2);
SERIAL_PORT.print("°\tRoll_g: ");
printFormattedFloat(roll_g, 3, 2);
SERIAL_PORT.print("°");

Test result chart and plot:

It’s extremely unstable, need a low pass filter. Tried alpha = 0.4, getting better.

Overall I’d trust accelerometer to calculate the pitch, roll and yaw, comparing to the gyroscope it has better accuracy and dependable readings.

5. Additional tasks

I use the code provided by instructions.

1
2
3
4
5
float pitch = 180 * atan2(sensor->magX(), sensor->magZ()) / M_PI;
float roll = 180 * atan2(sensor->magY(), sensor->magZ()) / M_PI;
float xm = myICM.magX()* cos(pitch) - myICM.magY()* sin(roll)* sin(pitch) + myICM.magZ()* cos(roll)* sin(pitch);
float ym = myICM.magY()* cos(roll) + myICM.magZ()* sin(roll/180*M_PI);
float yaw = 180 * atan2(ym, xm)/M_PI;

But the result shows it’s totally unreliable, I tried in my room, kitchen, but it stills has too much interferences…

Magnetic north: I put my IMU on my desk surface, and first let X axis pointing to magnetic north (red box below), then let it pointing to the magnetic south(green box below). The IMU reading have 10 degrees offset when I facing the North, and when I facing the South, it gives me roughly 46 or 47 degrees reading. Also, this test is still unstable like the previous one calculating yaw, when I make a small changes in pitch or roll, readings start bouncing.