Project showcase
MoveThisWay Heavy Payload Assistant

MoveThisWay Heavy Payload Assistant © GPL3+

MoveThisWay is an autonomous robot that transports heavy payloads on programmable paths with obstacle detection capability.

  • 4,236 views
  • 9 comments
  • 29 respects

Components and supplies

About this project

Purdue ECE Senior Design Fall 2019

MoveThisWay is an autonomous robot that can carry heavy payloads. Its main use case is in a varying office or industrial environment where there are clear paths for this robot to take. The robot is remotely programmed by the user and stores paths into its memory, making its routes autonomous and repeatable. MoveThisWay features sensors to avoid collisions and keep itself and its environment safe. It also has indicators to display the robot’s current movements, errors, and the battery level.

The goal of our team project was to create an affordable solution for businesses where robotic assistance is desired but out of reach. All parts used in the project are both easily attainable and relatively affordable.

Final Demo Video

Code

Robot CodeC/C++
Main code for the Teensy on the robot chassis
#include <Encoder.h>
#include <math.h>
#include <HX711.h>
#include <Adafruit_NeoPixel.h>
#include <EEPROM.h>
#include <PID_v1.h>
#include <SD.h>
#include <SPI.h>
#include <RH_RF69.h>

#define RFM69_RST     9
#define RFM69_CS      10
#define RFM69_INT     digitalPinToInterrupt(8)
#define RF69_FREQ     915.0

// Singleton instance of the radio driver
RH_RF69 rf69(RFM69_CS, RFM69_INT);

typedef enum {
  NORMAL, PROGRAM, PATH1, PATH2, PATH3, PATH4
} OpMode;

OpMode mode = NORMAL;
OpMode prevMode = NORMAL;
uint8_t databuff[9];
uint8_t datalen = sizeof(databuff);
unsigned int ud_remote, lr_remote;
bool prog, pb1, pb2, pb3, pb4;
bool progp1 = false;
bool progp2 = false;
bool progp3 = false;
bool progp4 = false;
bool ow_file = true;
bool follow = false;
bool reverse = false;
bool prevFollow = false;
int path_num = 0;

Sd2Card card;
SdVolume volume;
SdFile root;

const int chipSelect =  BUILTIN_SDCARD;

int arraysize = 5000;
double leftData[5000];
double rightData[5000];
int len = 0;

int indexData = 0;
int indexWrite = 0;

File myFile;
File revFile;

double Setpoint_R, Input_R, Output_R;
double Setpoint_L, Input_L, Output_L;

//Specify the links and initial tuning parameters
PID leftPID(&Input_L, &Output_L, &Setpoint_L,  1.55,   10,  0.05, DIRECT);
PID rightPID(&Input_R, &Output_R, &Setpoint_R,  1.55,   10,  0.05, DIRECT);

int address = 0;
byte value;

HX711 scale;
#define calibration_factor -8000  //This value is obtained using the SparkFun_HX711_Calibration sketch
#define DOUT  2
#define CLK  3

#define savebutton 12

#define LED_PIN   1
#define LED_COUNT 24
Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);

#define ud_pin A9
#define lr_pin A8

#define obstacle 0

#define forward_m1 4      // motor 1 forward
#define reverse_m1 5      // motor 1 reverse
#define forward_m2 6      // motor 2 forward
#define reverse_m2 7      // motor 2 reverse

#define e1_a 18            // encoder output pins
#define e1_b 19
#define e2_a 20
#define e2_b 21
Encoder knobLeft(e1_a, e1_b);
Encoder knobRight(e2_a, e2_b);

double positionLeft  = -999;
double positionRight = -999;
double left_rpm = 0;
double left_rpm_old = 0;
double right_rpm = 0;
double storeLeft = 0;
double storeRight = 0;

float time_new = 0;
float time_new_saveSD = 0;
float time_new_readSD = 0;
float time_delta = 25;

int ud = 0;
int lr = 0;
double ud_cartesian = 0;
double lr_cartesian = 0;
double r = 0;
double theta = 0;
double left_coord = 0;
double right_coord = 0;

int load = 0;

int indicator_brightness = 0;
int noload_brightness = 0;
int standby_brightness = 0;
int moving_brightness = 0;
int obstacle_brightness = 0;
int leftright_input = 0;        // INDICATOR COMMAND; off = 0, left = 1, right = 2
int ledaddress = 0;

void setup() {
  Serial.begin(2000000);
  pinMode(13, OUTPUT);

  pinMode(forward_m1, OUTPUT);        // set motor PWM pins as digital outputs
  pinMode(reverse_m1, OUTPUT);
  pinMode(forward_m2, OUTPUT);
  pinMode(reverse_m2, OUTPUT);

  pinMode(3, INPUT);

  analogWrite(forward_m1, 0);         // initialize PWM outputs as zero
  analogWrite(reverse_m1, 0);
  analogWrite(forward_m2, 0);
  analogWrite(reverse_m2, 0);

  pinMode(e1_a, INPUT);
  pinMode(e1_b, INPUT);
  pinMode(e2_a, INPUT);
  pinMode(e2_b, INPUT);
  pinMode(RFM69_RST, OUTPUT);

  digitalWrite(RFM69_RST, LOW);

  Serial.println("Welcome to MoveThisWay Central Processing!");
  Serial.println();

  // Manual RFM69 Reset
  digitalWrite(RFM69_RST, HIGH);
  delay(10);
  digitalWrite(RFM69_RST, LOW);
  delay(10);

  if (!rf69.init())
    Serial.println("init failed");
  // Defaults after init are 434.0MHz, modulation GFSK_Rb250Fd250, +13dbM, No encryption
  if (!rf69.setFrequency(RF69_FREQ))
    Serial.println("setFrequency failed");

  // If you are using a high power RF69, you *must* set a Tx power in the range 14 to 20 like this:
  rf69.setTxPower(14);

  Serial.print("RFM69 radio @");  Serial.print((int)RF69_FREQ);  Serial.println(" MHz");

  // SD CARD INITIALIZATION
  if (!SD.begin(chipSelect)) {
    Serial.println("initialization failed!");
    while (1);
  }
  Serial.println("initialization done.");

  scale.begin(DOUT, CLK);
  scale.set_scale(calibration_factor); //This value is obtained by using the SparkFun_HX711_Calibration sketch
  scale.tare(); //Assuming there is no weight on the scale at start up, reset the scale to 0

  strip.begin();
  strip.show(); // Initialize all pixels to 'off'

  //initialize the variables we're linked to
  Input_L = left_rpm;
  Setpoint_L = 0;
  Input_R = right_rpm;
  Setpoint_R = 0;

  //turn the PID on
  leftPID.SetSampleTime(20);
  leftPID.SetMode(AUTOMATIC);
  leftPID.SetOutputLimits(-255, 255);

  rightPID.SetSampleTime(20);
  rightPID.SetMode(AUTOMATIC);
  rightPID.SetOutputLimits(-255, 255);
}

void loop() {

  if (millis() < 1000) {        // Prevent motors from moving
    analogWrite(forward_m1, 0); // while establishing
    analogWrite(reverse_m1, 0); // connection to the motors.
    analogWrite(forward_m2, 0); //
    analogWrite(reverse_m2, 0); //
  }

  load = scale.get_units();

  bool obstacle_exist = digitalRead(obstacle);

  if (obstacle_exist == HIGH) {
    analogWrite(forward_m1, 0);
    analogWrite(reverse_m1, 0);
    analogWrite(forward_m2, 0);
    analogWrite(reverse_m2, 0);
    leftright_input = 0;
    ledaddress = ledcount(leftright_input, ledaddress);
    obstacle_brightness = stopped(ledaddress);
  }

  else {
    /////////////////////////////////////
    ///////// TANK CONTROL CODE /////////
    /////////////////////////////////////

    ud = analogRead(ud_pin);
    lr = analogRead(lr_pin);

    // >>>> MAP EITHER UD/LR OR UD/LR REMOTE HERE <<<<
    //ud_cartesian = map(ud, 7, 1011, -500, 500) * 1; //remap the pot range
    //lr_cartesian = map(lr, 7, 1011, -500, 500) * 1; // ^^^^^^^
    ud_cartesian = map(ud_remote, 7, 1011, -500, 500) * -1; //remap the pot range
    lr_cartesian = map(lr_remote, 7, 1011, -500, 500) * 1; // ^^^^^^^

    r = sqrt(pow(ud_cartesian , 2) + pow(lr_cartesian, 2)); // polar coord conversion for R

    if (ud_cartesian >= 0) {                                 //polar coord conversion for theta
      theta = atan2(ud_cartesian, lr_cartesian);
    }
    else if (ud_cartesian < 0) {
      theta = atan2(ud_cartesian, lr_cartesian) + 2 * M_PI ;
    }

    theta = theta - M_PI / 4;             //shift the polar coordinates by 45deg (pi/4) to make a diamond
    left_coord =  r * cos(theta);         //convert back to cartesian
    right_coord = r * sin(theta);
    left_coord =  left_coord * sqrt(2) * 255 / 500 ;
    right_coord = right_coord * sqrt(2) * 255  / 500 ;
    left_coord = constrain(left_coord, -255, 255);         //limit the output range
    right_coord = constrain(right_coord, -255, 255);

    if (left_coord <= 20 && left_coord >= -20) {
      left_coord = 0;
    }
    if (right_coord <= 20 && right_coord >= -20) {
      right_coord = 0;
    }

    //////////////////////////////////
    ///////// READ PATH CODE /////////
    //////////////////////////////////

    if (follow) {
      if (millis() > time_new_saveSD + 100) {
        if (myFile) {
          if (myFile.available()) {
            char buffer[50]; // May need to be a bit bigger if you have long names
            byte index = 0;

            while (myFile.available()) {
              char c = myFile.read();
              if (c == '\n' || c == '\r') { // Test for newline
                parseAndSave(buffer);
                index = 0;
                buffer[index] = '\0'; // NULL terminate buffer
                break;
              }
              else {
                buffer[index++] = c;
                buffer[index] = '\0'; // NULL terminate buffer
              }
            }
          } else {
            myFile.close();
            reverseAndWrite();
            revFile.close();
            follow = false;
            prevFollow = true;
            indexData = 0;
          }
        } else {
          Serial.println("Error opening file!");
        }

        // Write to reverse file
        if (revFile) {
          // Reverse the direction of wheels
          double rSetpoint_L = -1 * Setpoint_L;
          double rSetpoint_R = -1 * Setpoint_R;

          leftData[len] = rSetpoint_L;
          rightData[len++] = rSetpoint_R;

        } else {
          Serial.println("Error writing to reverse file!");
        }

        Serial.print("L: ");
        Serial.print(Setpoint_L);
        Serial.print("  ");
        Serial.print(left_rpm);
        Serial.print("    R: ");
        Serial.print(Setpoint_R);
        Serial.print("  ");
        Serial.println(right_rpm);

        if (Setpoint_L <= -20 ) {
          analogWrite(forward_m1, 0);
          analogWrite(reverse_m1, abs(Setpoint_L));
        }
        else if (Setpoint_L >= 20) {
          analogWrite(reverse_m1, 0);
          analogWrite(forward_m1, abs(Setpoint_L));
        }
        else {
          analogWrite(reverse_m1, 0);
          analogWrite(forward_m1, 0);
        }
        if (Setpoint_R <= -20) {
          analogWrite(forward_m2, 0);
          analogWrite(reverse_m2, abs(Setpoint_R));
        }
        else if (Setpoint_R >= 20) {
          analogWrite(reverse_m2, 0);
          analogWrite(forward_m2, abs(Setpoint_R));
        }
        else {
          analogWrite(reverse_m2, 0);
          analogWrite(forward_m2, 0);
        }

        time_new_saveSD = millis();
      }
    }

    if (reverse) {
      if (prevFollow) {
        if (path_num == 1) {
          revFile = SD.open("rev_1.txt");
          Serial.println("Opened rev_1.txt");
        } else if (path_num == 2) {
          revFile = SD.open("rev_2.txt");
          Serial.println("Opened rev_2.txt");
        } else if (path_num == 3) {
          revFile = SD.open("rev_3.txt");
          Serial.println("Opened rev_3.txt");
        } else if (path_num == 4) {
          revFile = SD.open("rev_4.txt");
          Serial.println("Opened rev_4.txt");
        }
        prevFollow = false;
      }
      if (millis() > time_new_saveSD + 100) {
        if (revFile) {
          if (revFile.available()) {
            char buffer[50]; // May need to be a bit bigger if you have long names
            byte index = 0;

            while (revFile.available()) {
              char c = revFile.read();
              if (c == '\n' || c == '\r') { // Test for newline
                parseAndSave(buffer);
                index = 0;
                buffer[index] = '\0'; // NULL terminate buffer
                break;
              }
              else {
                buffer[index++] = c;
                buffer[index] = '\0'; // NULL terminate buffer
              }
            }
          } else {
            revFile.close();
            Serial.println("Closed Reverse File");
            SD.remove("rev_1.txt");
            SD.remove("rev_2.txt");
            SD.remove("rev_3.txt");
            SD.remove("rev_4.txt");
            //Serial.println("Removed Reverse Files");
            if (SD.exists("rev_1.txt")) { //If the file still exist display message exist
              Serial.println("The rev_1.txt exists.");
            } else { //If the file was successfully deleted display message that the file was already deleted.
              Serial.println("The rev_1.txt doesn't exist.");
            }
            reverse = false;
            indexData = 0;
          }
        } else {
          Serial.println("Error opening reverse file for reading!");
        }

        Serial.print("Rev L: ");
        Serial.print(Setpoint_L);
        Serial.print("  ");
        Serial.print(left_rpm);
        Serial.print("    Rev R: ");
        Serial.print(Setpoint_R);
        Serial.print("  ");
        Serial.println(right_rpm);

        if (Setpoint_L <= -20 ) {
          analogWrite(forward_m1, 0);
          analogWrite(reverse_m1, abs(Setpoint_L));
        }
        else if (Setpoint_L >= 20) {
          analogWrite(reverse_m1, 0);
          analogWrite(forward_m1, abs(Setpoint_L));
        }
        else {
          analogWrite(reverse_m1, 0);
          analogWrite(forward_m1, 0);
        }
        if (Setpoint_R <= -20) {
          analogWrite(forward_m2, 0);
          analogWrite(reverse_m2, abs(Setpoint_R));
        }
        else if (Setpoint_R >= 20) {
          analogWrite(reverse_m2, 0);
          analogWrite(forward_m2, abs(Setpoint_R));
        }
        else {
          analogWrite(reverse_m2, 0);
          analogWrite(forward_m2, 0);
        }

        time_new_saveSD = millis();
      }
    }

    ////////////////////////////////
    ///////// ENCODER CODE /////////
    ////////////////////////////////

    long newLeft, newRight;
    newLeft = knobLeft.read();
    newRight = knobRight.read();

    if (newLeft != positionLeft || newRight != positionRight) {
      positionLeft = newLeft;
      positionRight = newRight;
    }

    if (millis() > time_new + time_delta) {
      float left_delta = positionLeft - storeLeft;
      left_rpm = ((left_delta / 4800) / (time_delta / 1000)) * 60;
      float right_delta = positionRight - storeRight;
      right_rpm = ((right_delta / 4800) / (time_delta / 1000)) * 60 ;
      storeLeft = positionLeft;
      storeRight = positionRight;
      time_new = millis();

      float wheelspeed_delta = right_rpm - left_rpm;

      if (left_rpm == 0 && right_rpm == 0) {
        leftright_input = 0;
        ledaddress = ledcount(leftright_input, ledaddress);
      }
      else if (left_rpm >= right_rpm - 10 && left_rpm <= right_rpm + 10) {
        leftright_input = 0;
        ledaddress = ledcount(leftright_input, ledaddress);
      }
      else if (wheelspeed_delta <= -10) {
        leftright_input = 1;
        ledaddress = ledcount(leftright_input, ledaddress);
      }
      else if (wheelspeed_delta >= 10) {
        leftright_input = 2;
        ledaddress = ledcount(leftright_input, ledaddress);
      }

      if (left_rpm == 0 && right_rpm == 0) {
        if (digitalRead(obstacle) == LOW) {
          if (load <= 2) {
            noload_brightness = noload(ledaddress);
            if (prevFollow) { // Trigger return upon object removal
              reverse = true;
            }
          }
          else if (load > 2) {
            standby_brightness = standby(ledaddress);
          }
        }
        else if (digitalRead(obstacle) == HIGH) {
          obstacle_brightness = stopped(ledaddress);
        }
      }
      else if (left_rpm >= right_rpm - 10 && left_rpm <= right_rpm + 10) {
        leftright_input = 0;
        moving_brightness = moving(ledaddress);
      }
      else if (wheelspeed_delta <= -10) {
        leftright_input = 2;
        indicator_brightness = indicator(ledaddress);
      }
      else if (wheelspeed_delta >= 10) {
        leftright_input = 1;
        indicator_brightness = indicator(ledaddress);
      }
    }

    ////////////////////////////////
    ///////// RECEIVE CODE /////////
    ////////////////////////////////

    if (rf69.available()) {
      if (rf69.recv(databuff, &datalen)) {
        // Parse data buffer
        prog = (databuff[0] == 255) ? true : false;
        pb1 = (databuff[1] == 255) ? true : false;
        pb2 = (databuff[2] == 255) ? true : false;
        pb3 = (databuff[3] == 255) ? true : false;
        pb4 = (databuff[4] == 255) ? true : false;

        ud_remote = databuff[5] * 256 + databuff[6];
        lr_remote = databuff[7] * 256 + databuff[8];
      }

      // Set mode
      if (prog) {
        mode = PROGRAM;
      } else if (pb1) {
        mode = PATH1;
      } else if (pb2) {
        mode = PATH2;
      } else if (pb3) {
        mode = PATH3;
      } else if (pb4) {
        mode = PATH4;
      } else {
        mode = NORMAL;
      }
    }

    if (mode == NORMAL) {
      if (prevMode == PROGRAM) {
        progp1 = false;
        progp2 = false;
        progp3 = false;
        progp4 = false;
        ow_file = true;
      }

      // Drive Normally
      if (!follow && !reverse) {
        if (left_coord <= -20) {
          analogWrite(forward_m1, 0);
          analogWrite(reverse_m1, abs(left_coord));
        }
        else if (left_coord >= 20) {
          analogWrite(reverse_m1, 0);
          analogWrite(forward_m1, abs(left_coord));
        }
        else {
          analogWrite(reverse_m1, 0);
          analogWrite(forward_m1, 0);
        }
        if (right_coord <= -20) {
          analogWrite(forward_m2, 0);
          analogWrite(reverse_m2, abs(right_coord));
        }
        else if (right_coord >= 20) {
          analogWrite(reverse_m2, 0);
          analogWrite(forward_m2, abs(right_coord));
        }
        else {
          analogWrite(reverse_m2, 0);
          analogWrite(forward_m2, 0);
        }
      }
      prevMode = NORMAL;
    } else if (mode == PROGRAM) {
      // Wait for a path button to be selected
      if (pb1 && !progp2 && !progp3 && !progp4) {
        progp1 = true;
      } else if (pb2 && !progp1 && !progp3 && !progp4) {
        progp2 = true;
      } else if (pb3 && !progp1 && !progp2 && !progp4) {
        progp3 = true;
      } else if (pb4 && !progp1 && !progp2 && !progp3) {
        progp4 = true;
      }

      Serial.print("Program Mode:  ");
      if (progp1) {
        Serial.println("1");
      } else if (progp2) {
        Serial.println("2");
      } else if (progp3) {
        Serial.println("3");
      } else if (progp4) {
        Serial.println("4");
      } else {
        Serial.println("0");
      }

      // drive normally
      if (left_coord <= -20 ) {
        analogWrite(forward_m1, 0);
        analogWrite(reverse_m1, abs(left_coord));
      }
      else if (left_coord >= 20) {
        analogWrite(reverse_m1, 0);
        analogWrite(forward_m1, abs(left_coord));
      }
      else {
        analogWrite(reverse_m1, 0);
        analogWrite(forward_m1, 0);
      }
      if (right_coord <= -20) {
        analogWrite(forward_m2, 0);
        analogWrite(reverse_m2, abs(right_coord));
      }
      else if (right_coord >= 20) {
        analogWrite(reverse_m2, 0);
        analogWrite(forward_m2, abs(right_coord));
      }
      else {
        analogWrite(reverse_m2, 0);
        analogWrite(forward_m2, 0);
      }

      //WRITE TO CARD HERE
      if (millis() > time_new_saveSD + 100) {
        if (progp1) {
          if (ow_file) {
            SD.remove("path_1.txt");
            SD.remove("rev_1.txt");
            ow_file = false;
          }
          // Save encoder values to P1
          myFile = SD.open("path_1.txt", FILE_WRITE);

          // if the file opened okay, write to it:
          if (myFile) {
            myFile.print(left_coord);
            myFile.print(" ");
            myFile.print(right_coord);
            myFile.print('\n');

            // close the file:
            myFile.close();
          }
          else {
            Serial.println("error opening path_1.txt");
          }

        } else if (progp2) {
          // Save encoder values to P2
          if (ow_file) {
            SD.remove("path_2.txt");
            SD.remove("rev_2.txt");
            ow_file = false;
          }
          // Save encoder values to P1
          myFile = SD.open("path_2.txt", FILE_WRITE);

          // if the file opened okay, write to it:
          if (myFile) {
            myFile.print(left_coord);
            myFile.print(" ");
            myFile.print(right_coord);
            myFile.print('\n');

            // close the file:
            myFile.close();
          }
          else {
            Serial.println("error opening path_2.txt");
          }
        } else if (progp3) {
          // Save encoder values to P3
          if (ow_file) {
            SD.remove("path_3.txt");
            SD.remove("rev_3.txt");
            ow_file = false;
          }
          // Save encoder values to P1
          myFile = SD.open("path_3.txt", FILE_WRITE);

          // if the file opened okay, write to it:
          if (myFile) {
            myFile.print(left_coord);
            myFile.print(" ");
            myFile.print(right_coord);
            myFile.print('\n');

            // close the file:
            myFile.close();
          }
          else {
            Serial.println("error opening path_3.txt");
          }
        } else if (progp4) {
          // Save encoder values to P4
          if (ow_file) {
            SD.remove("path_4.txt");
            SD.remove("rev_4.txt");
            ow_file = false;
          }
          // Save encoder values to P1
          myFile = SD.open("path_4.txt", FILE_WRITE);

          // if the file opened okay, write to it:
          if (myFile) {
            myFile.print(left_coord);
            myFile.print(" ");
            myFile.print(right_coord);
            myFile.print('\n');

            // close the file:
            myFile.close();
          }
          else {
            Serial.println("error opening path_4.txt");
          }
        }

        time_new_saveSD = millis();
      }

      prevMode = PROGRAM;
      //READ FROM CARD HERE
    } else if (mode == PATH1 && !follow && !reverse) {
      revFile.close();
      follow = true;
      path_num = 1;
      SD.remove("rev_1.txt");
      Serial.println("Reverse 1 file removed!");
      myFile = SD.open("path_1.txt");
      revFile = SD.open("rev_1.txt", FILE_WRITE);
      revFile.seek(0);

    } else if (mode == PATH2 && !follow && !reverse) {
      revFile.close();
      path_num = 2;
      follow = true;
      SD.remove("rev_2.txt");
      Serial.println("Reverse 2 file removed!");
      myFile = SD.open("path_2.txt");
      revFile = SD.open("rev_2.txt", FILE_WRITE);
      revFile.seek(0);

    } else if (mode == PATH3 && !follow && !reverse) {
      revFile.close();
      path_num = 3;
      follow = true;
      SD.remove("rev_3.txt");
      Serial.println("Reverse 3 file removed!");
      myFile = SD.open("path_3.txt");
      revFile = SD.open("rev_3.txt", FILE_WRITE);
      revFile.seek(0);

    } else if (mode == PATH4 && !follow && !reverse) {
      revFile.close();
      path_num = 4;
      follow = true;
      SD.remove("rev_4.txt");
      Serial.println("Reverse 4 file removed!");
      myFile = SD.open("path_4.txt");
      revFile = SD.open("rev_4.txt", FILE_WRITE);
      revFile.seek(0);
    }
  }
}

int ledcount(int leftright_input, int ledaddress) {
  if (leftright_input == 1) {
    if (ledaddress < 8) {
      ledaddress++;
    }
    else {
      ledaddress = 0;
    }
  }
  else if (leftright_input == 2) {
    if (ledaddress < 11) {
      ledaddress = 11;
    }
    else if (ledaddress >= 11 && ledaddress <= 20) {
      ledaddress++;
    }
    else {
      ledaddress = 12;
    }
  }
  else if (leftright_input == 0) {
    if (ledaddress <= 23) {
      ledaddress++;
    }
    else {
      ledaddress = 0;
    }
  }
  return (ledaddress);
}

int indicator(int ledaddress) {
  strip.setPixelColor(ledaddress + 0, 255, 255, 0);
  strip.setPixelColor(ledaddress + 1, 255, 255, 0);
  strip.setPixelColor(ledaddress + 2, 255, 255, 0);
  strip.setPixelColor(ledaddress + 3, 255, 255, 0);
  strip.show();
  strip.setPixelColor(ledaddress + 0, 0, 0, 0);
  strip.setPixelColor(ledaddress + 1, 0, 0, 0);
  strip.setPixelColor(ledaddress + 2, 0, 0, 0);
  strip.setPixelColor(ledaddress + 3, 0, 0, 0);
  return (0);
}

int noload(int ledaddress) {
  strip.setPixelColor(ledaddress + 0, 255, 51, 153);
  strip.setPixelColor(ledaddress + 1, 255, 51, 153);
  strip.setPixelColor(ledaddress + 2, 255, 51, 153);
  strip.setPixelColor(ledaddress + 3, 255, 51, 153);
  strip.show();
  strip.setPixelColor(ledaddress + 0, 0, 0, 0);
  strip.setPixelColor(ledaddress + 1, 0, 0, 0);
  strip.setPixelColor(ledaddress + 2, 0, 0, 0);
  strip.setPixelColor(ledaddress + 3, 0, 0, 0);
  return (0);
}

int standby(int ledaddress) {
  strip.setPixelColor(ledaddress + 0, 0, 255, 255);
  strip.setPixelColor(ledaddress + 1, 0, 255, 255);
  strip.setPixelColor(ledaddress + 2, 0, 255, 255);
  strip.setPixelColor(ledaddress + 3, 0, 255, 255);
  strip.show();
  strip.setPixelColor(ledaddress + 0, 0, 0, 0);
  strip.setPixelColor(ledaddress + 1, 0, 0, 0);
  strip.setPixelColor(ledaddress + 2, 0, 0, 0);
  strip.setPixelColor(ledaddress + 3, 0, 0, 0);
  return (0);
}


int moving(int ledaddress) {
  strip.setPixelColor(ledaddress + 0, 0, 255, 0);
  strip.setPixelColor(ledaddress + 1, 0, 255, 0);
  strip.setPixelColor(ledaddress + 2, 0, 255, 0);
  strip.setPixelColor(ledaddress + 3, 0, 255, 0);
  strip.show();
  strip.setPixelColor(ledaddress + 0, 0, 0, 0);
  strip.setPixelColor(ledaddress + 1, 0, 0, 0);
  strip.setPixelColor(ledaddress + 2, 0, 0, 0);
  strip.setPixelColor(ledaddress + 3, 0, 0, 0);
  return (0);
}

int stopped(int ledaddress) {
  strip.setPixelColor(ledaddress + 0, 255, 0, 0);
  strip.setPixelColor(ledaddress + 1, 255, 0, 0);
  strip.setPixelColor(ledaddress + 2, 255, 0, 0);
  strip.setPixelColor(ledaddress + 3, 255, 0, 0);
  strip.setPixelColor(ledaddress + 4, 255, 0, 0);
  strip.setPixelColor(ledaddress + 5, 255, 0, 0);
  strip.setPixelColor(ledaddress + 6, 255, 0, 0);
  strip.setPixelColor(ledaddress + 7, 255, 0, 0);
  strip.setPixelColor(ledaddress + 8, 255, 0, 0);
  strip.setPixelColor(ledaddress + 9, 255, 0, 0);
  strip.show();
  strip.setPixelColor(ledaddress + 0, 0, 0, 0);
  strip.setPixelColor(ledaddress + 1, 0, 0, 0);
  strip.setPixelColor(ledaddress + 2, 0, 0, 0);
  strip.setPixelColor(ledaddress + 3, 0, 0, 0);
  strip.setPixelColor(ledaddress + 4, 0, 0, 0);
  strip.setPixelColor(ledaddress + 5, 0, 0, 0);
  strip.setPixelColor(ledaddress + 6, 0, 0, 0);
  strip.setPixelColor(ledaddress + 7, 0, 0, 0);
  strip.setPixelColor(ledaddress + 8, 0, 0, 0);
  strip.setPixelColor(ledaddress + 9, 0, 0, 0);
  strip.setPixelColor(ledaddress + 10, 0, 0, 0);
}

void onLED() {
  digitalWrite(13, LOW);
  if (millis() > time_new + time_delta) {
    digitalWrite(13, HIGH);
  }
  digitalWrite(13, LOW);
}

int parseAndSave(char *buff) {
  Serial.print(indexData);
  Serial.print(" ");
  char *l_string = strtok(buff, " ");
  char *r_string = strtok(NULL, "\0");

  if (l_string && r_string) {
    Setpoint_L = atof(l_string);
    Setpoint_R = atof(r_string);
    indexData++;
  }
  return (indexData);
}

void reverseAndWrite() {
  for (int i = 0, j = len - 1; i < len / 2; i++, j--) {
    double temp1 = leftData[i];
    leftData[i] = leftData[j];
    leftData[j] = temp1;

    double temp2 = rightData[i];
    rightData[i] = rightData[j];
    rightData[j] = temp2;
  }

  for (int i = 0; i < len; i++) {
    revFile.print(leftData[i]);
    revFile.print(" ");
    revFile.print(rightData[i]);
    revFile.print('\n');
  }
 

  for (int i = 0; i < len; i++) {
    leftData[i] = 0;
    rightData[i] = 0;
  }
}
Sensor System CodeC/C++
Code for obstacle detection
#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lox = Adafruit_VL53L0X();

#define controlA 5
#define controlB 6
#define hazardPin 7
#define echoPin 8
#define trigPin 9
#define echoPin2 10
#define echoPin3 11
//#define echoPin4 12

int IRdistance1;
int hazardCycles;
int allClear;
long duration;
int distance1;
int distance2;
int distance3;
//int distance4;

void setup() {
  Serial.begin(115200);
  // wait until serial port opens for native USB devices
  while (! Serial) {
    delay(1);
  }

  pinMode(controlA, OUTPUT);
  pinMode(controlB, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(echoPin2, INPUT);
  pinMode(echoPin3, INPUT);
  //  pinMode(echoPin4, INPUT);
  pinMode(hazardPin, OUTPUT);
  hazardCycles = 0;
  allClear = 0;

  digitalWrite(controlA, HIGH);
  digitalWrite(controlB, LOW);
  Serial.println("a high b low");

  Serial.println("Adafruit VL53L0X test");
  if (!lox.begin()) {
    Serial.println(F("Failed to boot VL53L0X1"));
    while (1);
  }
  Serial.println("IR 1 booted");

  digitalWrite(controlA, LOW);
  digitalWrite(controlB, HIGH);
  Serial.println("switched");
  if (!lox.begin()) {
    Serial.println(F("Failed to boot VL53L0X2"));
    while (1);
  }
  Serial.println("setup complete");
}

void loop() {
  Serial.println("loop");
  // Ultrasound sensors
  // Clear the trigPin by setting it LOW:
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);

  // Trigger the sensor by setting the trigPin high for 10 microseconds:
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the echoPin, pulseIn() returns the duration (length of the pulse) in microseconds:
  duration = pulseIn(echoPin, HIGH);
  // Calculate the distance:
  distance1 = duration * 0.034 / 2;

  // Print the distance on the Serial Monitor (Ctrl+Shift+M):
  Serial.print("Distance1 = ");
  Serial.print(distance1);
  Serial.println(" cm\n");
  //  delay(1000);


  // Clear the trigPin by setting it LOW:
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);

  // Trigger the sensor by setting the trigPin high for 10 microseconds:
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the echoPin, pulseIn() returns the duration (length of the pulse) in microseconds:
  duration = pulseIn(echoPin2, HIGH);
  // Calculate the distance:
  distance2 = duration * 0.034 / 2;

  // Print the distance on the Serial Monitor (Ctrl+Shift+M):
  Serial.print("Distance2 = ");
  Serial.print(distance2);
  Serial.println(" cm\n");


  // Clear the trigPin by setting it LOW:
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);

  // Trigger the sensor by setting the trigPin high for 10 microseconds:
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the echoPin, pulseIn() returns the duration (length of the pulse) in microseconds:
  duration = pulseIn(echoPin3, HIGH);
  // Calculate the distance:
  distance3 = duration * 0.034 / 2;

  // Print the distance on the Serial Monitor (Ctrl+Shift+M):
  Serial.print("Distance3 = ");
  Serial.print(distance3);
  Serial.println(" cm\n");


  //  // Clear the trigPin by setting it LOW:
  //  digitalWrite(trigPin, LOW);
  //  delayMicroseconds(5);
  //
  //  // Trigger the sensor by setting the trigPin high for 10 microseconds:
  //  digitalWrite(trigPin, HIGH);
  //  delayMicroseconds(10);
  //  digitalWrite(trigPin, LOW);
  //
  //  // Read the echoPin, pulseIn() returns the duration (length of the pulse) in microseconds:
  //  duration = pulseIn(echoPin4, HIGH);
  //  // Calculate the distance:
  //  distance4 = duration * 0.034 / 2;
  //
  //  // Print the distance on the Serial Monitor (Ctrl+Shift+M):
  ////  Serial.print("Distance4 = ");
  ////  Serial.print(distance4);
  ////  Serial.println(" cm\n");
  ////
  ////  delay(2000);



  //IR sensor
  VL53L0X_RangingMeasurementData_t measure;
  digitalWrite(controlA, digitalRead(controlB));
  digitalWrite(controlB, !digitalRead(controlB));

  Serial.print("Reading a measurement... ");
  lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

  IRdistance1 = measure.RangeMilliMeter;

  if (measure.RangeStatus != 4) {  // phase failures have incorrect data
    Serial.print("IR Distance A (mm) = "); Serial.println(IRdistance1);
  }
  else {
    Serial.println(" out of range ");
  }


  if ((IRdistance1 <= 750) || (distance1 <= 50 && distance1 >= 1) || (distance2 <= 80 && distance2 >= 1) || (distance3 <= 50 && distance3 >= 1)) {
    Serial.print("Hazard detected");
    digitalWrite(hazardPin, HIGH);
    hazardCycles = hazardCycles + 1;
    allClear = hazardCycles;

  }
  else {
    hazardCycles = hazardCycles + 1;
    if ((hazardCycles - allClear >= 2)) {
      digitalWrite(hazardPin, LOW);
      Serial.print("No hazard detected");
    }
  }
}

Custom parts and enclosures

Power Enclosure
Housing for battery and power PCB
Drive PCB Enclosure Box
Houses the Drive System PCB
Body Panel Rear
Rear body panel
Front Sensor Panel
Sensor cover panel for front bodywork
Rear Right Corner
Flip in CURA for RearLeft
Front Right Corner
Flip in CURA for opposite side
Front Right LED Bracket
LED bracket for FR and RL corners. Flip in CURA for opposite sides.
Front Right LED Housing
LED housing for FR and RL corners. Flip in CURA for opposite sides.
Side Panel
Flip in CURA for opposite sides.
Remote Control Box
Box for remote controller
Remote Control Lid
Lid for remote controller
Corner Sensor Cover
Ultrasonic sensor bracket for corner panels
Drive PCB Enclosure Lid
Drive System enclosure lid
Sensor System Enclosure
enclosure for sensor system
Sensor System Lid
Lid for sensor system
Wheel Mount
Bracket for mounting non-driven wheels
Motor Mount
Mounting bracket for Pololu 37D metal gearmotors
Hard plastic wheels for non-motorized wheels
Power Enclosure Lid
Big lid for power system housing
Power Enclosure Lid (Battery)
Battery lid for power system enclosure
Body Panel Front
Front body panel

Schematics

Drive System Schematic
Schematic for the Drive System
Schematic jtq8jq7xjz
Remote Control Schematic
Schematic for remote controller
Schematic l2ak5mup3d
Sensor System Schematic
Schematic for Sensor System
Schematic mfcwq8db94
Power System Schematic
Schematic for Power System
Schematic zogqonmies

Comments

Similar projects you might like

Child Assistant

Project tutorial by Md. Khairul Alam

  • 2,183 views
  • 0 comments
  • 20 respects

Autonomous Home Assistant Robot

Project tutorial by Juan Miguel Jimeno

  • 12,671 views
  • 7 comments
  • 95 respects

Arduino Self-Balancing Robot

Project showcase by Debreczeni Tamas

  • 7,268 views
  • 4 comments
  • 57 respects

Dual Axis Servo Control with Joystick

Project tutorial by Arnov Sharma

  • 3,462 views
  • 0 comments
  • 13 respects

Microcontrollers Lab

Project tutorial by raul

  • 11,615 views
  • 8 comments
  • 61 respects

How to Make a Gesture Control Robot at Home

Project tutorial by Shubham Shinganapure

  • 8,780 views
  • 13 comments
  • 59 respects
Add projectSign up / Login