Lab 5(a): Obstacle Avoidance

Prelab

Advantages include low cost, and usage of minimum power, whereas, these sensors have low data transfer rate and range of usage is relatively shorter. I would use them in my robot for cases where lighting conditions are not to my favor.

Objective

Proximity Sensor:

Installed the SparkFun VCNL4040 Proximity Sensor Library, and hooked up the VCNL4040 to the Artemis board using a QWIIC connector as shown below:
Ran the code to scan the I2C channel to find the sensor, as shown in datasheet:
Test the proximity sensor:
Shown below is the plot of the sensor readings vs the actual distance. The figure shows sensor readings for multiple colors (blue,red,black), and for the color blue it shows two variations, one with bright lighting, and the other with dark lighting. As shown, the sensor is sensitive to the color of the material it is sensing, however, the lighting conditions did not affect the sensor readings considering the difference in the light blue and dark blue curves is very minimal.

Texture:
- Black: Plastic (Phone case)
- Blue: PLA (3d printer material)
- Red: Leather (Wallet)
Used the timestamp on the serial monitor and changing the position of the object in front of the sensor to determine the delay in the sensing, and also used it to determine the frequency of the reading process:
Delay: 4.673 s
Frequency: Every ~67 ms

Time of Flight Sensor:

Installed the SparkFun VL53L1X 4m laser distance sensor Library, and hooked up the VL53L1X to the Artemis board using a QWIIC connector as shown below:

Ran the code to scan the I2C channel to find the sensor, as shown in datasheet:

Tested the ToF sensor using "Example 1: Read Distance" which showed around 80 mm to be the distance which is clearly incorrect so I used "Example7: Calibration" to calibrate the sensor, however, the code would not run even if the distance was less than 10 cm. After debugging the code, I noticed that the "distanceSensor.checkForDataReady()" reads zero and results in an infinite loop. After adding startRanging and stopRanging as shown below, this problem went away and the calibration was successfully completed.



while (tLowDistanceCount < 20)
{
	while (!distanceSensor.checkForDataReady())
	{
	distanceSensor.startRanging();
	delay(1);
	}
	if (distanceSensor.getDistance() < 100)
	{
	tLowDistanceCount++;
	}
	else
	{
	tLowDistanceCount = 0;
	}
	distanceSensor.stopRanging();
	distanceSensor.clearInterrupt();
}

The calibrated depth values from the ToF sensor:

The maximum speed is known to be 0.8636 m/s as determined in lab 3. The Intermeasurement Period is the alotted time the sensor will wait for the laser pulse to return. Consdering the maximum speed of the robot is relatively high, I would be more inclined to choose a lower value for this period since it increases the sampling rate. Even though it decreases the accuracy at longer range, I prioritize the behavior closer to the wall than further away from it to prevent object collision. After testing, the optimal value resulted to be 20 ms. The test was conducted by rapidly moving the robot at varying periods, and determining which one would detect the change in distance the quickest. There is obviously a source of human error in this, however, among the spectrum of possible values, this was the lowest chosen period.

The timing budget has the following possible values: 15, 20, 33, 50, 100, 200, and 500. The timing budget is the amount of time over which the measurement is taken. The intermeasurement period has to be greater than or equal to the timing budget, therefore, the timing budget can be chosen to be either 15 or 20.

To confirm this, given the speed to be 0.8638 m/s and the Int. period to be 30 ms, the total distance over which a measurement is sampled is 26 mm. If the Int. period were to be higher, then this distance would also be higher which means the robot would not be able to detect an object in time before a collision.

The setup code to test the different settings:


uint16_t budget = 15;
uint16_t period = 500;

distanceSensor.setIntermeasurementPeriod(period); 
distanceSensor.setTimingBudgetInMs(budget);

Out of the three modes, I would consider the high mode the most appropriate for the surrounding where the real robot will be tested (which is my room). After running "setDistanceModeMedium()", the program did not consider it as a code, therefore, I disregarded this mode and ran tests for the other two modes. The priority is to determine the difference in sampling rates between the two modes, since we already know that the long mode provides a higher distance range of 4 m relative to the 1.3 m for the short mode. The setup is as follows: The calibrated depth values from the ToF sensor:

Period/Sampling rate:

Considering the maximum speed is 0.8638 m/s, the maximum measuring distance for each mode is:

Accuracy at 1.3 meters:

Accuracy at 2 meters:

These values prove that the advantage of using the long mode is the increased range only, and the con is the low sampling rate. Even at distances within the range of the short mode, the accuracy of the long mode is close to that of the short mode. However, considering the robot's relatively fast speed in a small space, it would be beneficial to get a faster sampling rate, therefore, I expect to use short mode.

As shown below, after moving my hands rapidly in front of the sensor, the signal failed. This will impact motion implementation in future labs due to the lack of the robot's ability to move quickly. If the robot moves quickly, the sensor measurements will fail which means I will need to implement methods to avoid this from happening. This will affect localization and path planning since our measured data will be faulty.

As shown below, the sensor is not very sensitive to varying colors, and it is relatively accurate to the actual values. The ToF sensor range was measured by slowly increasing the distance between the object and the sensor until an unacceptable distance is reached, and in this case, the range came out to be from 0 to ~ 13 feet. Using the values shown in the figure below, the accuracy was calculated to be 1.726287544 %, and the repeatability is around 2% also. The ranging time was computed by putting micro() around the startRange and StopRange calls.

Obstacle Avoidance:

Discussion: As shown below, the robot is capable of detecting objects and avoiding a collision with them. Everything worked great on the stationary platform, but when I put it down, the robot's motors found it difficult to fight against the floor friction so the robot could not rotate as smoothly. As I was ramping up the speed, I noticed the response time needed to be quicker, therefore, I increased the threshold at which it would detect an object and stop. I did not have enough time to record the speed relative to a piece of tape.

Setup:

Video of stationary robot:

Video of Moving robot:

Code:



						
// Motors
#include 
#include 
#include "SCMD.h"
#include "SCMD_config.h" //Contains #defines for common SCMD register names and values
#include "Wire.h"

#define LEDPIN 13

SCMD myMotorDriver; //This creates the main object of one motor driver and connected slaves.

// Distance sensor

#include 
#include 
#include 
#include 
#include 
#include "SparkFun_VL53L1X.h" //Click here to get the library: http://librarymanager/All#SparkFun_VL53L1X

SFEVL53L1X distanceSensor;


void setup() {
  // ------------------------ Motor Driver ------------------------ //

  Serial.begin(115200);
  pinMode(LEDPIN, OUTPUT);

  //***** Configure the Motor Driver's Settings   *****//
  myMotorDriver.settings.commInterface = I2C_MODE;

  //  set address if I2C configuration selected with the config jumpers
  myMotorDriver.settings.I2CAddress = 0x5D; //config pattern "0101" on board for address 0x5A

  //  set chip select if SPI selected with the config jumpers
  myMotorDriver.settings.chipSelectPin = 10;

  //*****initialize the driver get wait for idle*****//
  while ( myMotorDriver.begin() != 0xA9 ) //Wait until a valid ID word is returned
  {
    delay(500);
  }

  //  Check to make sure the driver is done looking for slaves before beginning
  while ( myMotorDriver.ready() == false );

  //*****Set application settings and enable driver*****//

  while ( myMotorDriver.busy() );
  myMotorDriver.enable();

  // ------------------------ Distance Sensor  ------------------------ //
  Wire.begin();

  if (distanceSensor.begin() != 0) //Begin returns 0 on a good init
  {
    while (1)
      ;
  }

  distanceSensor.setDistanceModeShort();

}

// Motors
#define LEFT_MOTOR 0
#define RIGHT_MOTOR 1
int Rightspeed;
int Leftspeed;
int Speed = 120;
int CloseEnough = 400; // mm

void loop() {


    // ------------------------ Motion ------------------------ //
    // Move forward
    digitalWrite( LEDPIN, LEFT_MOTOR );
    digitalWrite( LEDPIN, RIGHT_MOTOR );
    myMotorDriver.setDrive( LEFT_MOTOR, 0, Speed); //Drive Left wheel forward at Speed
    myMotorDriver.setDrive( RIGHT_MOTOR, 1, Speed); //Drive Right wheel forward at Speed

    // ------------------------ Distance Sensor  ------------------------ //

    distanceSensor.startRanging(); //Write configuration bytes to initiate measurement
    while (!distanceSensor.checkForDataReady())
    {
      delay(1);
    }
    int distance = distanceSensor.getDistance(); //Get the result of the measurement from the sensor


    // If close to obstacle stop
    if (distance < CloseEnough) {

      digitalWrite( LEDPIN, LEFT_MOTOR );
      digitalWrite( LEDPIN, RIGHT_MOTOR );

      // Stop Robot
      myMotorDriver.setDrive( LEFT_MOTOR, 1, 0); // Stop Left
      myMotorDriver.setDrive( RIGHT_MOTOR, 1, 0); // Stop Right
      delay(200);

      // Move backwards and curve Counter clockwise
      Rightspeed = Speed;  Leftspeed = 3*Speed/2;
      myMotorDriver.setDrive( LEFT_MOTOR, 1, Leftspeed);
      myMotorDriver.setDrive( RIGHT_MOTOR, 0, Rightspeed);
      delay(250);

      // Stop Robot
      myMotorDriver.setDrive( LEFT_MOTOR, 1, 0); // Stop Left
      myMotorDriver.setDrive( RIGHT_MOTOR, 1, 0); // Stop Right
      delay(200);

      // Rotate Counter clockwise
      myMotorDriver.setDrive( LEFT_MOTOR, 1, Speed); // Left goes backwards
      myMotorDriver.setDrive( RIGHT_MOTOR, 1, Speed); // Right goes forwards
      delay(3000);

      // Stop Robot
      myMotorDriver.setDrive( LEFT_MOTOR, 1, 0); // Stop Left
      myMotorDriver.setDrive( RIGHT_MOTOR, 1, 0); // Stop Right
      delay(200);

    }

    Serial.print("Distance(mm): ");
    Serial.println(distance);
    distanceSensor.clearInterrupt();
    distanceSensor.stopRanging();


}

Lab 5(b): Obstacle Avoidance on the virtual robot

Simulation:

Code:


def perform_obstacle_avoidance(robot):
while True:
	# Obstacle avoidance code goes here
	
	# Move forward
	robot.set_vel(0.5,0)

	# If close to obstacle
	if robot.get_laser_data() < 0.75:
		
		# Stop Robot
		robot.set_vel(0,0)
		
		# Move backward and rotate
		robot.set_vel(-0.3,0.5)
		time.sleep(0.5)
		
		# Turn 90 degrees
		robot.set_vel(0,0.2)
		time.sleep(0.5)
	
		
perform_obstacle_avoidance(robot)

Questions

The duration of a velocity command is 0.5 as shown in the code above.
The robot curves backwards in a counter-clock direction, and then rotates for another 0.5 seconds at 0.2 angular velocity. Therefore, in total it rotates by 0.7 radians in the counter-clock direction.
The virtual robot managed to sense the distance at a linear speed of 0.5. If the velocity was any higher, the robot would fail to sense the wall in time to avoid it.
The closest the virtual robot can get to the walls (obstacles) is about 0.3. This is because of the thickness of the robot itself. As shown in the video, the robot gets very close to the wall sometimes. The threshold picked to check with the sensors is 0.75, to ensure this collision does not take place.
After running it multiple times, there has not been an instance where it has not worked. However, I suspect that the robot may crash when an edge is placed in front of it such that the depth is much higher since it cannot measure the depth to the edge itself. The robot got close but never collided.