#include <AccelStepper.

h> // includes the library for old code

#include <NewPing.h> // includes library for ultrasonic sensors
const int MaxSonar=300; // max sonar range in cm
const int MotorEnableL=10; // pin to enable/disable easystepper(enable = drive, disable = coast)
const int MotorEnableR=9; // pin to enable/disable easystepper
const int MotorEnableC=8; // pin to enable/disable easystepper
//
// function declarations /////////////////////////////////////// These are all subroutines that will be called
later and accomplish specific tasks
//
void destL(int speed, long int pos, int dir);
void destR(int speed, long int pos, int dir);
void move(void);
int pingAve(int num, int sen);
void wheelsOff(void);
void wheelsOn(void);
void center();
void blucontrol();
void go(int dist);
void lanefollow(int dist, int dir);
void wallfollow(int dir);
void grip();
void getagrip(int dist, int dir);
void drive(int dist, int dir);
void spin(int dist, int dir1, int dir2);
void enableAll(int able);
void toBall();
void depart();
void yuck();
void autonomous(int dist);
//
// AccelStepper(# motor wires, pulse pin#, direction pin#) /////////////////////////
//
int dirL = 7; // sets an integer called dirL equal to 7
int dirR = 5; // sets an integer called dirR equal to 5
int dirC = 3; // sets an integer called dirR equal to 3
AccelStepper stepperL(1, 6, dirL); //defines the main pins and direction pins
AccelStepper stepperR(1, 4, dirR); //the second pin is the main pin and the direction pin is set
AccelStepper stepperC(1, 2, dirC); //as an integer just above and called here.
//
// ultrasonic sensors ///////////////////////////////////////////////////////////////
//

#define TRIGGER1 24
#define TRIGGER2 36
mega
# define TRIGGER3 30
# define ECHO1 25
#define ECHO2 37
#define ECHO3 31

// Defines Trigger Pin for sonar sensor left to pin 24 on arduino mega
// Defines Trigger Pin for sonar sensor center to pin 36 on arduino
// Defines Trigger Pin for sonar sensor right to pin 30 on arduino mega
// Defines Echo Pin for sonar sensor left to pin 25 on arduino mega
// Define Echo Pin for sonar sensor center to pin 37 on arduino mega
// Define Echo Pin for sonar sensor right to pin 31 on arduino mega

//Defines each object and its corresponding pins by the new ping library, each pin called is
defined above
NewPing sonarL(TRIGGER1,ECHO1, MaxSonar);
NewPing sonarC(TRIGGER2,ECHO2, MaxSonar);
NewPing sonarR(TRIGGER3,ECHO3, MaxSonar);
//
// values to control EasyDriver ////////////////////////////////////////////////////////
//These pins are used to set step sizes for the motors from full-step to quarter-step to eighthstep
#define MS1R 46 // Defines MS1 Pin for sonar sensor right to pin 46 on arduino mega
#define MS2R 47 // Defines MS2 Pin for sonar sensor right to pin 47 on arduino mega
#define MS1L 48 // Defines MS1 Pin for sonar sensor left to pin 48 on arduino mega
#define MS2L 49 // Defines MS2 Pin for sonar sensor left to pin 49 on arduino mega
#define MS1C 44 // Defines MS1 Pin for sonar sensor center to pin 44 on arduino mega
#define MS2C 45 // Defines MS2 Pin for sonar sensor center to pin 45 on arduino mega
// setup - things that happen only once at start
void setup()
{
Serial.begin(115200);//Sets all serial communication at 115200 baud, fastest speed arduino
can function at
//Sets the enable pin defined above that we can change to HIGH or LOW, HIGH disables the
wheel (coast) LOW enables the wheel (drive)
pinMode(MotorEnableR, OUTPUT); //Right Wheel
pinMode(MotorEnableL, OUTPUT); //Left Wheel
pinMode(MotorEnableC, OUTPUT);//Center Wheel
//Sets the MS pins that are defined above to output pins, enabling us to send HIGH or LOW
//MS1 & MS2 LOW = Full-Step
//MS1 & MS2 HIGH = Eighth-Step
//MS1 HIGH & MS2 LOW = Half-Step
//MS1 LOW & MS2 HIGH = Quarter-Step
pinMode(MS1R, OUTPUT); //MS1 Right
pinMode(MS2R, OUTPUT); //MS2 Right
pinMode(MS1L, OUTPUT); //MS1 Left

pinMode(MS2L, OUTPUT); //MS2 Left

pinMode(MS1C, OUTPUT); //MS1 Center
pinMode(MS2C, OUTPUT); //MS2 Center
//Sets the main pins on driver as outputs on arduino mega
pinMode(6, OUTPUT); // Pin 6 on arduino mega which is Left motor driver
pinMode(4, OUTPUT); // Pin 4 on arduino mega which is Right motor driver
//Presets Right and Left Wheels to quarter-step mode and grabber to eighth-step mode
digitalWrite(MS1R, LOW);
digitalWrite(MS2R, HIGH);
digitalWrite(MS1L, LOW);
digitalWrite(MS2L, HIGH);
digitalWrite(MS1C, HIGH);
digitalWrite(MS2C, HIGH);
}
//Main Loop/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void loop(){
go(6000); //go is the main subroutines that houses all case statements to activate other
subroutines
}

//
// pingAve(int num, int sen) - average num pings from sensor
//
int pingAve(int num, int sen) {
int dist=0; //A holding variable for ultrasonic sensor reading inputs
int distTot=0; //Stores the total amount of distance read by the ultrasonic sensor readings
int count=0; //Counts the number of times the ultrasonic reads
int distAve; // Stores the final output that is the average of the ultrasonic readings
//
for (int x=0; x < num; x++) {//num is the integer defined above, the for loop will loop num times
if(sen == 1){ //if left sensor is called
dist = sonarL.ping()/US_ROUNDTRIP_CM; // variable dist = the ultrasonic reading
converted into cm
}
else if(sen == 2){//if right sensor is called
dist = sonarC.ping()/US_ROUNDTRIP_CM; // variable dist = the ultrasonic reading
converted into cm

}
else if(sen == 3){//if center sensor is called
dist = sonarR.ping()/US_ROUNDTRIP_CM; // variable dist = the ultrasonic reading
converted into cm
}
if (dist > 0 && dist < MaxSonar) {// if distance read is less than 300
distTot+=dist; //variable distTot is incremented by dist
count += 1; // add one to the value of count
}
else if (dist <= 0) { //if beyond 300 value returned is -1, so this sets it to 300
distTot+=MaxSonar; //variable distTot is incremented by Maxsonar
count += 1; // add one to the value of count
}
delayMicroseconds(5000); //5 milliseconds of time between pings or they jam each other,
transmits before receives the signal
}
distAve = distTot/count; //distAve is distTot which has been added to divided by count
return(distAve);
//returning the average distance
}
//
//main function///
//
void go(int dist){ //function redirects bluetooth command to proper actions
Serial.println("v - display commands"); // prints on device context of quotations
//resets wheels to quarter-step mode
digitalWrite(MS1L, LOW);
digitalWrite(MS2L, HIGH);
digitalWrite(MS1R, LOW);
digitalWrite(MS2R, HIGH);
while(1){//infinite loop
//resets wheels to quarter-step mode
digitalWrite(MS1L, LOW);
digitalWrite(MS2L, HIGH);
digitalWrite(MS1R, LOW);
digitalWrite(MS2R, HIGH);
int z = Serial.read(); // starts reading integer z for case statements
switch(z){// declares z as switch that enables us to send bluetooth commands
case 'n': //if case n is pressed...

enableAll(1); //call subroutine that disables motors

break; // ends case statement
case 'm': //if case m is pressed...
enableAll(0); //call subroutine that enables motors
Serial.println("enable motors");// prints context of quotes on device
break; //ends case statement
case ',': //if case , is pressed...
depart(); // calls subroutine negotiates intersection after grabbing ball
break; //ends case statement
case 's': //if case s is pressed...
Serial.print("left sensor: "); //print statement in quotes
Serial.println(pingAve(3,1));//prints the distance of the wall from left sensor
Serial.print("right sensor: ");//print statement in quotes
Serial.println(pingAve(3,3));//prints the distance of the wall from right sensor
Serial.print("center sensor: ");//print statement in quotes
Serial.println(pingAve(3,2));//prints the distance of the wall from center sensor
break; // ends case statement
case 'b': //if case b is pressed...
toBall(); // calls for subroutine that centers the robot until the pedestal
break; // ends case statement
case 'h': //if case h is pressed...
getagrip(250,1); //calls subroutine that grabs the ball
break; // ends case statement
case 'y': //if case y is pressed...
yuck(); // calls for subroutine that drops the ball and ensures that the ball will enter the box
break; // ends case statement
case 'g': //if case g is pressed...
lanefollow(dist, HIGH); //calls subroutine that navigates the robot near the end
break; // ends case statement
case 'v': //if case v is pressed...
//prints to the device all commands if forgotten
Serial.println(" b - to ball");
Serial.println(" h - hug ball");
Serial.println(" , - intersection turn");
Serial.println(" g - go around course");
Serial.println(" w - follow wall to sense");

Serial.println(" y - yuck, drop ball");

Serial.println(" n - no motor, disable");
Serial.println(" m - motor, enable");
Serial.println(" v - display commands menu ");
break; //ends cases statement
case 'w': //if case w is pressed...
wallfollow(1); //calls subroutine that does the final navigation to the deposit box
break; // ends the case statement
case '@': //if case @ is pressed...
autonomous(dist); //calls for subroutine that runs the entire course
break; // ends the case statement
}// end switch
} // end while
} // end go()

//
// follow lanes a specified distance and direction
//
void lanefollow(int dist, int dir){
int steptime = 1300; // 1400 is slow but reliable
int step = 0; // set int step to 0, this will be altered later
int skip = 1; //set int skip to 1, the variable will determine the frequency a wheel is skipped
int turn = 0; // set int turn to 0, the variable will determines the direction of the turn
int count = 0; //keep track of steps
int sensedist = 26; // determines how often we sense during lanefollow
int pingL, pingR, delta; // declares these variable integers that will be called later
int maxdelta = 10; // sets the maximum difference between left and right sensor before a big
turn
digitalWrite(dirL, dir); // HIGH(1) = forward
digitalWrite(dirR, dir); // HIGH(1) = forward
digitalWrite(MotorEnableL, LOW); //enable wheels
digitalWrite(MotorEnableR, LOW); //enable wheels
for(int x=0;x<dist;x++){//for loop that loops dist times as set above
count++; // add one to count

// decide if we need to sense

if (count == sensedist ) {
pingL = pingAve(1,1); //set pingL to reading of left sensor
pingR = pingAve(1,3); // set pingR to reading of right sensor
delta = pingL - pingR; //delta is the difference in sensor readings
count = 0; //reset count to 0
}
//sets condition for a small right turn
if( delta < 0 && delta >= -maxdelta){ // turns to the right
turn = 0; //declares right turn below
skip = (1/abs(delta))*10; //proportional control algorithm based on delta readings
//reset wheels to quarter-step mode
digitalWrite(MS1R, LOW);
digitalWrite(MS2R, HIGH);
digitalWrite(MS1L, LOW);
digitalWrite(MS2L, HIGH);
}
if( delta < -maxdelta) { // turns to the right
turn = 0; //declares right turn below
skip = 1; //condition for a calibrated big turn
//set right wheel to eighth-step and left wheel to quarter-step
digitalWrite(MS1R, HIGH);
digitalWrite(MS2R, HIGH);
digitalWrite(MS1L, LOW);
digitalWrite(MS2L, HIGH);
}
if( delta > 0 && delta <= maxdelta) { // turns to the left
turn = 1; //declares left turn below
skip = (1/abs(delta))*10; //proportional control algorithm based on delta readings
//reset wheels to quarter-step mode
digitalWrite(MS1L, LOW);
digitalWrite(MS2L, HIGH);
digitalWrite(MS1R, LOW);
digitalWrite(MS2R, HIGH);
}
if( delta > maxdelta) { // turns to the left
skip = 1; //condition for a calibrated big turn
turn = 1; // declares left turn below
//set left wheel to eighth-step and right wheel to quarter-step

digitalWrite(MS1L, HIGH);
digitalWrite(MS2L, HIGH);
digitalWrite(MS1R, LOW);
digitalWrite(MS2R, HIGH);
}
if (delta == 0) {// sensor values are equal
turn = 2; // go straight
//reset wheels to quarter-step mode
digitalWrite(MS1R, LOW);
digitalWrite(MS2R, HIGH);
digitalWrite(MS1L, LOW);
digitalWrite(MS2L, HIGH);
}
//pulses high or low for each wheel based on turn direction and when or when not to skip///
if( (step < skip) && (turn == 1) ) {
digitalWrite(motorL, HIGH);
}
if( (step < skip) && (turn == 0) ) {
digitalWrite(motorR, HIGH);
}
if( (turn == 1) || (turn == 2) ) {
digitalWrite(motorR, HIGH);
}
if( (turn == 0) || (turn == 2) ) {
digitalWrite(motorL, HIGH);
}
delayMicroseconds(steptime);
if(step < skip && turn == 0) {
digitalWrite(motorR, LOW);
}
if(step < skip && turn == 1) {
digitalWrite(motorL, LOW);
}
if(step >= skip) {//resets or adds to skip
step = 0;
} else {
step++;
}

if(turn == 1 || turn ==2){ // turning left, always pulse l

digitalWrite(motorR,LOW);
}
if(turn == 0 || turn ==2){ // turning right, always pulse r
digitalWrite(motorL,LOW);
}

if (count == sensedist) {//when count reaches sensedist read ultrasonics and reset count
count = 0;
} else {
delayMicroseconds(steptime);
}
} // end for loop
}
//
// manual control of gripper
//
void getagrip(int dist, int dir) {//grabbing subroutine that goes a passed distance and direction
digitalWrite(dirC,dir);
enableAll(0); //enables all motors
for(int x=0;x<dist;x++) {// Pulse HIGH and LOW dist times, to grab ball
digitalWrite(motorC,LOW);
delayMicroseconds(1000);
digitalWrite(motorC,HIGH);
delayMicroseconds(1000);
}
}
//
// basic move
//
void drive(int dist, int dir) {
digitalWrite(dirL,dir);
digitalWrite(dirR,dir);
for(int x=0;x<dist;x++) {//basic driving pulse that also continual grabs ball for insurance
digitalWrite(motorL,LOW);

digitalWrite(motorR,LOW);
digitalWrite(motorC,LOW);
delayMicroseconds(2500);
digitalWrite(motorL,HIGH);
digitalWrite(motorR,HIGH);
digitalWrite(motorC,HIGH);
delayMicroseconds(2500);
}
}
//
//
//
void spin(int dist, int dir1, int dir2) {//basic turning behavior that also continually grabs ball for
insurance
digitalWrite(dirL,dir1);
digitalWrite(dirR,dir2);
for(int x=0;x<dist;x++) {
digitalWrite(motorL,LOW);
digitalWrite(motorR,LOW);
digitalWrite(motorC,LOW);
delayMicroseconds(2500);
digitalWrite(motorL,HIGH);
digitalWrite(motorR,HIGH);
digitalWrite(motorC,HIGH);
delayMicroseconds(2500);
}
}

//subroutine that does the last bit of navigating to the dropoff box based on distance from center
sensor reading, uses the same logic as lane follow, except it keeps the right wall at a specified
distance away, instead of setting right and left distance equal
void wallfollow(int dir){
int steptime = 1400;
int step = 0;
int skip = 1;
int turn = 0;
int count = 0; //keep track of steps
int sensedist = 26;
int pingL, pingR, delta;

int dist = pingAve(3,2) * 28;

int maxdelta = 10; // 10 for narrow, 14 for wide lanes ????
digitalWrite(dirL, dir); // HIGH(1) = forward
digitalWrite(dirR, dir); // HIGH(1) = forward
enableAll(0);
step = 0;
for(int x=0;x<dist;x++){
count++; // sense every 1/2 cm
// decide if we need to turn
if (count == sensedist ) {
pingR = pingAve(1,3);
delta = pingR - 4;
count = 0;
}
//change delta from 10 to 14 for wide lanes ????
if( pingR > 4){ // turns to the right
turn = 0;
skip = (1/abs(delta))*10;
digitalWrite(MS1R, LOW);
digitalWrite(MS2R, HIGH);
digitalWrite(MS1L, LOW);
digitalWrite(MS2L, HIGH);
}
if( pingR < 4) { // turns to the left
turn = 1;
skip = (1/abs(delta))*10;
digitalWrite(MS1L, LOW);
digitalWrite(MS2L, HIGH);
digitalWrite(MS1R, LOW);
digitalWrite(MS2R, HIGH);
}
if (pingR == 4) {
turn = 2;
digitalWrite(MS1R, LOW);
digitalWrite(MS2R, HIGH);
digitalWrite(MS1L, LOW);
digitalWrite(MS2L, HIGH);

if( (step < skip) && (turn == 1) ) {

digitalWrite(motorL, HIGH);
}
if( (step < skip) && (turn == 0) ) {
digitalWrite(motorR, HIGH);
}
if( (turn == 1) || (turn == 2) ) {
digitalWrite(motorR, HIGH);
}
if( (turn == 0) || (turn == 2) ) {
digitalWrite(motorL, HIGH);
}
delayMicroseconds(steptime);
if(step < skip && turn == 0) {
digitalWrite(motorR, LOW);
}
if(step < skip && turn == 1) {
digitalWrite(motorL, LOW);
}
if(step >= skip) {
step = 0;
} else {
step++;
}
if(turn == 1 || turn ==2){ // turning right, always pulse r
digitalWrite(motorR,LOW);
}
if(turn == 0 || turn ==2){ // turning right, always pulse r
digitalWrite(motorL,LOW);
}

if (count == sensedist) {
count = 0;
digitalWrite(motorC,LOW);
} else {

delayMicroseconds(steptime);
}
} // end for loop
}
void enableAll(int able){// enables or disables motors based on integer passed to it( 1 = disable,
0 = enable)
digitalWrite(MotorEnableL, able);
digitalWrite(MotorEnableR, able);
digitalWrite(MotorEnableC, able);
}

void toBall(){ // subroutine that navigates to the ball based on front sensor reading utilizing the
already existing lanefollow subroutine
int ball; // used to calculate how far ball is
ball = pingAve(3,2);
Serial.print("going forward: ");Serial.println(ball*26.7);
lanefollow(ball*26.7, HIGH);
}
void depart(){ //negotiate the intersection after grabbing the ball utilizing calibrated drive and
spin subroutines distances and directions
drive(400,0);
drive(200,2);
spin(150,1,0);
drive(250,2);
spin(80,1,0);
}
void yuck(){ // utilizes getagrip subroutine to drop ball and drive and spin subroutines to ensure
the ball is in the dropoff box
getagrip(200,0);
spin(10,0,1);
spin(20,1,0);
spin(20,0,1);
spin(10,1,0);
drive(50,1);
spin(10,0,1);
spin(20,1,0);
spin(20,0,1);
spin(10,1,0);

drive(50,1);
spin(10,0,1);
spin(20,1,0);
spin(20,0,1);
spin(10,1,0);
drive(50,1);
enableAll(1);
}
void autonomous(int dist){//calls the series of subroutines to run the entire course
toBall();
toBall();
getagrip(250,1);
depart();
lanefollow(dist, 1);
wallfollow(1);
yuck();
}
//end all