By: Haydon Chiu

Link to all Firmware

/* PROPOSALBOT Final Code
* 05/10/2020
* EE 400D
* Professor Gary Hill
* Group members: Ethan Thiessa, Haydon Chiu, Tyler Galgas, and Jonathan Valdez
* Written by: Haydon Chiu (Omnidirectional movement) with additional code written by Ethan Thiessa and Jonathan Valdez
* 
* The code will allow ProposalBot to move omnidirectionally so that the 
* bot can spell the words "MARRY ME". The bot utilizes the Accelstepper library.
* It will call upon the functions that have combinations 
* of wheel movements that allow for the function to move omnidirectionally.
* The step format of the movement in void loop() can be seen below.
* 
* Steps:
* 1. The first part to initiate movement is to call the time (time1,time2,time3,etc.).
* 2. The time variable(s) will be put into a while loop and will be subtracted as
* millis()-time, this will then be compared (less than) to a number depending on how long 
* one wants the bot to move in that direction.
* 3. Call a function (moveRightforward, moveBackward, moveForward)
* 4. Call .runSpeed() on each wheel to initiate the movement of the
* previous function
* 5. After the while loop, a delay will be called so that the code will have a chance
* to pause before the next while loop gets called.
*/


#include 
#include 
// Define the stepper motors and the pins the will use
AccelStepper LeftBackWheel(1, 13, 12); // (Type:driver, STEP, DIR) - Stepper1
AccelStepper LeftFrontWheel(1, 11, 10); // Stepper2
AccelStepper RightBackWheel(1, 9, 8); // Stepper3
AccelStepper RightFrontWheel(1, 7, 6); // Stepper4

int wheelSpeed = 150;
int servoPin1 = 3;
int servoPin2 = 5;
Servo servo,ringServo; 

int servoAngle = 0; // servo position in degrees

unsigned long time,time1,time2,time3,time4,time5,time6,time7,time8,time9,time10,time11,time12,time13,time14, //These variables are going to be used as the start of the timer for our robot movements.
time15,time16,time17,time18,time19,time20,time21,time22,time23,time24,time25,
time26,time27,time28,time29,time30,time31,time32,time33,time34,time35,time36,time37,time38,time39;



void setup() {
// put your setup code here, to run once:
//.setMaxSpeed() is used to set the wheels for maximum movement in the attempt that the wheels speed to match movements
Serial.begin(9600); 
servo.attach(servoPin1);
ringServo.attach(servoPin2);
LeftFrontWheel.setMaxSpeed(3000);
LeftBackWheel.setMaxSpeed(3000);
RightFrontWheel.setMaxSpeed(3000);
RightBackWheel.setMaxSpeed(3000);
servo.write(90);
ringServo.write(0);
}

void loop() {
//Beginning of M
servo.write(180);
delay(1000);
time = millis();
while(millis()-time < 800){
moveRightForward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time1 = millis();
while(millis()-time1 <400){
moveRightBackward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time2 = millis();
while(millis()-time2 < 400){
moveRightForward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time3 = millis();
while(millis()-time3 < 800){
moveRightBackward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};
//End of M

/*The movement of moveSidewaysRight() will act as 
* the space bar.
*/
delay(500);
servo.write(90);
delay(1000);
time4 = millis();
while(millis()-time4 < 500){
moveSidewaysRight();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

//End of Space
delay(500);
servo.write(180);
//Beginning of A

delay(1000);
time5 = millis();
while(millis()-time5 < 500){
moveRightForward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time6 = millis();
while(millis()-time6 < 600){
moveSidewaysRight();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time7 = millis();
while(millis()-time7 < 600){
moveSidewaysLeft();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time8 = millis();
while(millis()-time8 < 400){
moveRightForward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time9 = millis();
while(millis()-time9 < 800){
moveRightBackward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

//End of A

delay(500);
//Beginning of Space
servo.write(90);
delay(500);
time10 = millis();
while(millis()-time10 < 500){
moveSidewaysRight();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

//End of Space

delay(500);
//Beginning of R
servo.write(180);
delay(500);
time11 = millis();
while(millis()-time11 < 700){
moveForward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time12 = millis();
while(millis()-time12 < 400){
moveRightBackward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time13 = millis();
while(millis()-time13 < 500){
moveLeftBackward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time14 = millis();
while(millis()-time14 < 600){
moveRightBackward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};
//End of R

delay(500);
//Beginning of space
servo.write(90);
delay(500);
time15 = millis();
while(millis()-time15 < 500){
moveSidewaysRight();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
servo.write(180);

//Beginning of R
delay(500);
time16 = millis();
while(millis()-time16 < 700){
moveForward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time17 = millis();
while(millis()-time17 < 400){
moveRightBackward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time18 = millis();
while(millis()-time18 < 500){
moveLeftBackward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time19 = millis();
while(millis()-time19 < 600){
moveRightBackward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};
//End of R

delay(500);
servo.write(90);

//Beginning of space
delay(500);
time20 = millis();
while(millis()-time20 < 500){
moveSidewaysRight();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};
//End of space

delay(500);
servo.write(180);

//Beginning of Y
delay(500);
time21 = millis();
while(millis()-time21 < 400){
moveForward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time22 = millis();
while(millis()-time22 < 400){
moveLeftForward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time23 = millis();
while(millis()-time23 < 400){
moveRightBackward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time24 = millis();
while(millis()-time24 < 400){
moveRightForward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
servo.write(90);
/*Since Y doesn't end at the bottom of a letter
* it wil be necessary to moveSidewaysRight()
* and moveBackward()
*/
delay(500);
time25 = millis();
while(millis()-time25 < 500){
moveSidewaysRight();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time26 = millis();
while(millis()-time26 < 800){
moveBackward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

//End of space movement
delay(500);
servo.write(180);
//M
delay(500);
time27 = millis();
while(millis()-time27 < 800){
moveRightForward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time28 = millis();
while(millis()-time28 <400){
moveRightBackward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time29 = millis();
while(millis()-time29 < 400){
moveRightForward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time30 = millis();
while(millis()-time30 < 800){
moveRightBackward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

//End of M

delay(500);
servo.write(90);

//Beginning of space
delay(500);
time31 = millis();
//Serial.println(time);
while(millis()-time31 < 500){
moveSidewaysRight();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};
//End of space

delay(500);
servo.write(180);

//Beginning of E

delay(500);
time32 = millis();
while(millis()-time32 < 600){
moveSidewaysRight();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time33 = millis();
while(millis()-time33 < 600){
moveSidewaysLeft();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time34 = millis();
while(millis()-time34 < 400){
moveForward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time35 = millis();
while(millis()-time35 < 600){
moveSidewaysRight();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time36 = millis();;
while(millis()-time36 < 600){
moveSidewaysLeft();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time37 = millis();
while(millis()-time37 < 400){
moveForward();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};

delay(500);
time38 = millis();
while(millis()-time38 < 600){
moveSidewaysRight();
LeftFrontWheel.runSpeed();
LeftBackWheel.runSpeed();
RightFrontWheel.runSpeed();
RightBackWheel.runSpeed();
};


delay(1000);
ringServo.write(100);
delay(5000);

}
/*
* These functions will be called in order
* to get ProposalBot to move omnidirectionally
*/
void moveForward() {
LeftFrontWheel.setSpeed(-wheelSpeed);
LeftBackWheel.setSpeed(-wheelSpeed);
RightFrontWheel.setSpeed(wheelSpeed);
RightBackWheel.setSpeed(wheelSpeed);
}
void moveBackward() {
LeftFrontWheel.setSpeed(wheelSpeed);
LeftBackWheel.setSpeed(wheelSpeed);
RightFrontWheel.setSpeed(-wheelSpeed);
RightBackWheel.setSpeed(-wheelSpeed);
}
void moveSidewaysRight() {
LeftFrontWheel.setSpeed(-wheelSpeed);
LeftBackWheel.setSpeed(wheelSpeed);
RightFrontWheel.setSpeed(-wheelSpeed);
RightBackWheel.setSpeed(wheelSpeed);
}
void moveSidewaysLeft() {
LeftFrontWheel.setSpeed(wheelSpeed);
LeftBackWheel.setSpeed(-wheelSpeed);
RightFrontWheel.setSpeed(wheelSpeed);
RightBackWheel.setSpeed(-wheelSpeed);
}
void rotateLeft() {
LeftFrontWheel.setSpeed(wheelSpeed);
LeftBackWheel.setSpeed(wheelSpeed);
RightFrontWheel.setSpeed(wheelSpeed);
RightBackWheel.setSpeed(wheelSpeed);
}
void rotateRight() {
LeftFrontWheel.setSpeed(wheelSpeed);
LeftBackWheel.setSpeed(wheelSpeed);
RightFrontWheel.setSpeed(-wheelSpeed);
RightBackWheel.setSpeed(-wheelSpeed);
}
void moveRightForward() {
LeftFrontWheel.setSpeed(-wheelSpeed);
LeftBackWheel.setSpeed(0);
RightFrontWheel.setSpeed(0);
RightBackWheel.setSpeed(wheelSpeed);
}
void moveRightBackward() {
LeftFrontWheel.setSpeed(0);
LeftBackWheel.setSpeed(wheelSpeed);
RightFrontWheel.setSpeed(-wheelSpeed);
RightBackWheel.setSpeed(0);
}
void moveLeftForward() {
LeftFrontWheel.setSpeed(0);
LeftBackWheel.setSpeed(-wheelSpeed);
RightFrontWheel.setSpeed(wheelSpeed);
RightBackWheel.setSpeed(0);
}
void moveLeftBackward() {
LeftFrontWheel.setSpeed(wheelSpeed);
LeftBackWheel.setSpeed(0);
RightFrontWheel.setSpeed(0);
RightBackWheel.setSpeed(-wheelSpeed);
}
void stopMoving() {
LeftFrontWheel.setSpeed(0);
LeftBackWheel.setSpeed(0);
RightFrontWheel.setSpeed(0);
RightBackWheel.setSpeed(0);
}