Overview

Team Members: Shiv Chopra, Blake Heard, Raj Patel, Andrew Trimper

In this project, the robot maps out its environment using a LIDAR (ToF) sensor. The data from the sensor is sent to a C# GUI where it is drawn onto a map, and a user can then interact with the map to pick a path for the robot to follow. Finally, the PC sends the path info back to the robot, and the robot drives along the correct path.

Components

Parts List:

• mbed LPC1768 (https://www.sparkfun.com/products/9564)
• Shadow Bot Chassis: (https://www.sparkfun.com/products/13301)
• 2x Wheels (https://www.sparkfun.com/products/13259)
• VL53L0X ToF Sensor (https://www.adafruit.com/product/3317)
• LSM9DS1 IMU (https://www.sparkfun.com/products/13944)
• 2x DC Motors (https://www.sparkfun.com/products/13302)
• H-Bridge Motor Driver (https://www.sparkfun.com/products/14450)
• 2x Hall Effect Encoders (https://www.sparkfun.com/products/12629)
• 5V Battery Pack (https://www.sparkfun.com/products/9835)
• DC Barrel Jack Adapter (https://www.sparkfun.com/products/10811)
• Pushbutton (https://www.sparkfun.com/products/9190)
• SPDT Switch (https://www.sparkfun.com/products/102)

ToF Sensor

The LIDAR is mounted onto the Shadow Bot by a 3D printed right angle mount. It takes distance measurements (in mm) that are used to detect obstacles in an environment.

IMU

The IMU's gyroscope integrates angular velocity around the z-axis relative heading.

5V Battery Pack

The barrel jack on the battery pack connects to the barrel jack adapter on the board. The battery pack supplies additional power necessary for motor movement.

H-Bridge Motor Driver and DC Motors

The DC motors control the wheels on the Shadow Bot (one for each wheel). The H-Bridge Driver controls the speed and direction of the motors.

Encoders

Hall Effect encoders are interrupt-based counters of wheel rotation. By counting the number of rotations, the robot can be moved accurate distances.

Pushbutton

Used to initiate different stages of robot control and serial communication.

Construction

The following link shows how to construct the Shadow Chassis, including placement of the DC motors, Hall Effect encoders, and wheels:

https://learn.sparkfun.com/tutorials/assembly-guide-for-redbot-with-shadow-chassis/

The breadboard can be mounted on the top chassis plate using tape. The 5V battery pack can be mounted on the bottom chassis plate using tape.

All of the components are placed on the breadboard with the exception of the LIDAR. The LIDAR is mounted on the top chassis plate using a 3D-printed right angle bracket, nuts, and bolts.

Finished construction of the robot

Code

The code was split up into 2 parts: C++ and C#. The C++ code handled the movement of the robot, distance measurements, and serial communication between the mbed and PC. The C# code handled the GUI and also handled the serial communication between the mbed and PC.

The C# code is posted on GitHub: https://github.com/atrimper/RoboticMappingControl

Final rendition of the GUI based on obstacle detection by robot

Stage 1

In a separate thread from main, the robot initializes and pings the LIDAR for distance measurements in mm to store in a global variable, lidarDistance, unless the IMU is being used, indicated by useIMU.

void getLidarDistance() {
    DevI2C* lidarDevice = new DevI2C(p28, p27);
    XNucleo53L0A1* lidarBoard = XNucleo53L0A1::instance(lidarDevice, A2, D8, D2);
    DigitalOut _lidarShdn(p26);
    _lidarShdn = 0;
    Thread::wait(100);
    _lidarShdn = 1;
    Thread::wait(100);
    int status = lidarBoard->init_board();
    while (status) {
        status = lidarBoard->init_board();
    }
    while(1) {
        if (!useImu) {
            lidarBoard->sensor_centre->get_distance(&lidarDistance);
        }
    }
}

Here, the robot waits for a pushbutton command to initiate obstacle detection. The robot rotates 360 degrees using encoders while incrementally storing distance measurements from lidarDistance in the obstacles array.

void RobotController::detectObstacles() {
    led = 0b0001;
    while(!pb);
    Thread::wait(500);
    for (int i = 0; i < 360; i++) {
        _leftEncoder.reset();
        _rightEncoder.reset();
        leftWheel.speed(0.205);
        rightWheel.speed(-0.205);
        Thread::wait(9);
        while((_leftEncoder.read() < 1) && (_rightEncoder.read() < 1));
        leftWheel.speed(0);
        rightWheel.speed(0);
        obstacles[i] = (int)(lidarDistance*MMTOCM);
    }
    led = 0;
}

Stage 2

After the robot takes its distance measurements, the mbed waits for a pushbutton to be pressed before sending the data (this gives the user time to hook up the PC with the GUI). Once the button is pressed, all 360 distance points are sent over serial to a connected PC.

void DataDistributor::transmitObstacles() {
    led = 0b0010;
    while(!pb);
    wait(1);
    
    bool sentData = false;
    while(!sentData) {
        if(pc.writeable()) {
            pc.putc('s');
            for(int i = 0; i < 360; i++) {
                pc.putc(obstacles[i]);
                wait(0.02);
            }
            sentData = true;
        }
    }
    led = 0b0000;
}

This function receives angle-distance pairs from the PC over serial. The PC breaks both the angle and distance into two bytes each and stores them in an array. The function then waits to receive a start character before it starts reading in the pairs. Once the start character is received, the function reconstructs the angle-distance pairs and stores them in a global array that will be utilized in Stage 4. Stage 3

void DataDistributor::receiveTrajectory() {
    led = 0b0100;
    while(pc.getc() != 's');
    wait(0.1);
    trajectoryLength = pc.getc();
    wait(0.1);
    trajectory = new int[trajectoryLength * 2];
    
    bool readData = false;
    while(!readData) {
        if(pc.readable()) {
            for(int i = 0; i < trajectoryLength * 2; i += 2) {
                uint8_t aLow = pc.getc();
                wait(0.1);
                uint8_t aHigh = pc.getc();
                wait(0.1);
                uint8_t dLow = pc.getc();
                wait(0.1);
                uint8_t dHigh = pc.getc();
                uint16_t angle = ((uint16_t)aHigh << 8) | aLow;
                uint16_t dist = ((uint16_t)dHigh << 8) | dLow;
                trajectory[i] = (int)angle;
                trajectory[i + 1] = (int)dist;
            }
            readData = true;
        }
    }
    led = 0b0000;
}

Stage 4

Again, the robot waits for a pushbutton command to follow the trajectory set out by the GUI user. The robot processes the trajectory array for angle-distance pairs and then rotates said angle using the IMU and moves said distance using the encoders for each pair.

void RobotController::followTrajectory() {
    led = 0b1000;
    while(!pb);
    Thread::wait(500);
    for (int i = 0; i < trajectoryLength * 2; i = i + 2) {
        t.reset();
        yaw = 0.0;
        w1 = 0.0;
        w2 = 0.0;
        t1 = 0.0;
        t2 = 0.0;
        int angle = trajectory[i] % 360;
        float leftWheelSpeed = 0.2;
        float rightWheelSpeed = -0.2;
        if ((trajectory[i] >= 0) && (trajectory[i] <= 90)) {
            angle = angle*ROTERRI;
        } else if (trajectory[i] <= 180) {
            angle = angle*ROTERRII;
        } else if (trajectory[i] <= 360) {
            angle = (360 - angle)*ROTERRIII;
            leftWheelSpeed = -leftWheelSpeed;
            rightWheelSpeed = -rightWheelSpeed;
        }
        useImu = true;
        t.start();
        leftWheel.speed(leftWheelSpeed);
        rightWheel.speed(rightWheelSpeed);
        while(yaw < angle) {
            yaw = yaw + (((w2+w1)/2.0)*(t2-t1));
            while(!imu.gyroAvailable());
            imu.readGyro();
            w1 = w2;
            w2 = abs(imu.calcGyro(imu.gz));
            t1 = t2;
            t2 = t.read();
        }
        leftWheel.speed(0);
        rightWheel.speed(0);
        t.stop();
        useImu = false;
        _leftEncoder.reset();
        _rightEncoder.reset();
        int distance = (int)(trajectory[i + 1] * COUNTTOMM);
        leftWheel.speed(0.2);
        rightWheel.speed(0.2);
        while((_leftEncoder.read() < distance) && (_rightEncoder.read() < distance));
        leftWheel.speed(0);
        rightWheel.speed(0);
    }
    delete []trajectory;
    led = 0;
}

Demo

Potential Additions

• Adding the Adafruit HUZZAH ESP8266 for WiFi communication
  • mbed and PC could communicate through a web server so that the whole process would be completely wireless instead of having to communicate serially
• Unicycle Robot Control
  • The robot could self-balance while idle using unicycle robot control theory.
• Continuous Mapping
  • The map could expand in size as the robot moves to different waypoints specified by the user.
• Better power management