MoveThisWay Heavy Payload Assistant © GPL3+
MoveThisWay is an autonomous robot that transports heavy payloads on programmable paths with obstacle detection capability.
- 7,113 views
- 10 comments
- 48 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.
Code
#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;
}
}
#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
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
Comments
Four Guys (ECE 49022, Team 2)
Published on
December 10, 2019Members who respect this project
you might like