MoveThisWay Heavy Payload Assistant

Project showcase

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

Please sign in to continue:

Share

Embed the widget on your own site

MoveThisWay Heavy Payload Assistant © GPL3+

Components and supplies

About this project

Code

Robot CodeC/C++

Sensor System CodeC/C++

Custom parts and enclosures

Power Enclosure

Drive PCB Enclosure Box

Body Panel Rear

Front Sensor Panel

Rear Right Corner

Front Right Corner

Front Right LED Bracket

Front Right LED Housing

Side Panel

Remote Control Box

Remote Control Lid

Corner Sensor Cover

Drive PCB Enclosure Lid

Sensor System Enclosure

Sensor System Lid

Wheel Mount

Motor Mount

Wheel

Power Enclosure Lid

Power Enclosure Lid (Battery)

Body Panel Front

Schematics

Drive System Schematic

Remote Control Schematic

Sensor System Schematic

Power System Schematic

Comments

Published on

Members who respect this project

Table of contents

Similar projects you might like

Good to see you again