Kevin LiuNate LongoJoel PetersonJosiah Rudge
Published © GPL3+

MoveThisWay Heavy Payload Assistant

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

IntermediateShowcase (no instructions)5 days9,115
MoveThisWay Heavy Payload Assistant

Things used in this project

Hardware components

Teensy 4.0 Development Board
Teensy 4.0 Development Board
×1
Arduino Nano R3
Arduino Nano R3
×1
General Purpose Transistor NPN
General Purpose Transistor NPN
×4
Buffer / Line Driver, 74HC125
Buffer / Line Driver, 74HC125
×1
Logic Level FET N-Channel
Logic Level FET N-Channel
×1
SparkFun Load Cell Amplifier - HX711
SparkFun Load Cell Amplifier - HX711
×1
WS2812 Addressable LED Strip
Digilent WS2812 Addressable LED Strip
×1
Pololu 37D Metal Gearmotor 150:1
×2
Texas Instruments LM2576T-ADJ
×1
Texas Instruments LM336LP
×1
SparkFun RFM69HCW 915MHz
×2

Story

Read more

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

Wheel

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

Remote Control Schematic

Schematic for remote controller

Sensor System Schematic

Schematic for Sensor System

Power System Schematic

Schematic for Power System

Code

Robot Code

C/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 Code

C/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");
    }
  }
}

Credits

Kevin Liu

Kevin Liu

1 project • 8 followers
Nate Longo

Nate Longo

1 project • 8 followers
Joel Peterson

Joel Peterson

1 project • 7 followers
Josiah Rudge

Josiah Rudge

1 project • 7 followers

Comments