/*

Demonstrates .run() in a function - move()

move until a sensor value is seen - move_to_sensor1()
This removes sensor and processing irregularities from
stepper motor control
*/
#include <AccelStepper.h>
#include <NewPing.h>
const
const
const
const

int
int
int
int

MaxSonar=300; // max sonar range in cm

MotorEnableL=10; // pin to enable/disable easystepper
MotorEnableR=9; // pin to enable/disable easystepper
MotorEnableC=8; // pin to enable/disable easystepper

//
// function declarations ///////////////////////////////////////////////////////
////
//
void destL(int speed, long int pos, int dir);
void destR(int speed, long int pos, int dir);
void move(void);
int move_to_sensor1(int stopdist, int movedist);
int pingAve(int num, int sen);
void wheelsOff(void);
void wheelsOn(void);
void center();
void turnL(int turnDist);
void turnR(int turnDist);
//These all serve
to define
void turnDir();
//subroutine
s as their types
void test(int togoL,int togoR,int stopdist,int movedist);
void blucontrol();
void grip();
void drop();
void autonomous();
void straitaway();
void ballGrab();
void rightLane();
void ballDrop();
//
// AccelStepper(# motor wires, pulse pin#, direction pin#) ////////////////////
/////
//

// These are the

int DirLpin = 7;
int DirRpin = 5;
int DirCpin = 3;

direction pins for each motor driver

// Defines pin 7 as the direction pin for the left motor
// Defines pin 5 as the direction pin for the right motor
// Defines pin 3 as the direction pin for the center motor

// The AccelSteppper command defines each stepper object and what pins they are
utilizing
AccelStepper stepperL(1, 6, DirLpin); // stepperL is the object, 6 is the main
output, DirLpin is
.
//the direction pin as defined above
AccelStepper stepperR(1, 4, DirRpin); //
AccelStepper stepperC(1, 2, DirCpin); //
//
// ultrasonic sensors //////////////////////////////////////////////////////////
/////
//
#define TRIGGER1 24 // defines trigger pin for sensor left
#define TRIGGER2 36 // defines trigger pin for sensor center
#define TRIGGER3 30 // defines trigger pin for sensor right
#define ECHO1 25
// defines echo pin for sensor left
#define ECHO2 37
// defines echo pin for sensor right
#define ECHO3 31
// defines echo pin for sensor center
NewPing sonarL(TRIGGER1,ECHO1, MaxSonar); //defines each sensor as a sensor and
which pins they utilize
NewPing sonarC(TRIGGER2,ECHO2, MaxSonar);
NewPing sonarR(TRIGGER3,ECHO3, MaxSonar);
//
// values to control EasyDrivers ///////////////////////////////////////////////
/////////
//
#define
#define
#define
#define
#define
#define

MS1R
MS2R
MS1L
MS2L
MS1C
MS2C

46
47
48
49
44
45

//
//
//
//
//
//

Defines
Defines
Defines
Defines
Defines
Defines

int pos = 400;

//
400 steps
int maxspeed1 = 1200;
//
int maxspeed2 = 1200;
//
int maxspeedNorm = 1200; //
int motoraccel = 700;
//
float FullStepSize=0.063;//
float FullStepDist=16.5; //
float stepPerCm=25.8;
//

the
the
the
the
the
the

right MS1 pin as 46

right MS2 pin as 47
left MS1 pin as 48
left MS2 pin as 49
grabber MS1 pin as 44
grabber MS2 pin as 45

sets a position value that was used in debugging to

sets a speed called later
sets a speed called later
sets a speed called later
sets the acceleration for
in/step
step/in
steps taken per cm

int pingDistL; // distance variable called later

int pingDistC; // distance variable called later

to 1200
to 1200
to 1200
the steppers to 700

int
int
int
int

moveDist1;
moveDist2;
pingDistR;
moveDist3;

//
//
//
//

distance
distance
distance
distance

variable
variable
variable
variable

called
called
called
called

later
later
later
later

// setup - things that happen only once at start

void setup()
{
Serial.begin(115200);//this is used to define the speed at which serial comman
ds are read
stepperL.setMaxSpeed(maxspeedNorm);//sets the maxspeed for stepperL to the val
ue of maxspeedNorm
stepperL.setAcceleration(motoraccel);//sets the stepperL acceleration to the v
alue of motoraccel
stepperR.setMaxSpeed(maxspeedNorm); //sets the maxspeed for the stepperR to va
lue of maxspeedNorm
stepperR.setAcceleration(motoraccel);//sets the stepperR acceleration to the v
alue of motoraccel
stepperC.setMaxSpeed(maxspeedNorm);//sets the stepperC(grabber) to the value o
f maxspeedNorm
stepperC.setAcceleration(motoraccel);//sets the stepperC(grabber) to the value
of motoraccel
pinMode(MotorEnableR, OUTPUT); //sets each enable pin for each driver as outp
ut pins
pinMode(MotorEnableL, OUTPUT); //
pinMode(MotorEnableC, OUTPUT); //
pinMode(MS1R, OUTPUT); // sets each MS(2) pin for each stepper as output
pinMode(MS2R, OUTPUT); //
pinMode(MS1L, OUTPUT); //
pinMode(MS2L, OUTPUT); //
pinMode(MS1C, OUTPUT); //
pinMode(MS2C, OUTPUT); //
// DEBUG changed from LOW to HIGH
digitalWrite(MS1R, LOW); //defining each MS1 pin as low and each MS2 pin as
high
digitalWrite(MS2R, HIGH); //sets each motor in 1/2 step mode with more torque
and
digitalWrite(MS1L, LOW); //precision than whole steps
digitalWrite(MS2L, HIGH); //
digitalWrite(MS1C, HIGH); //Both MS pins high puts it into 1/8 step mode with
the most
digitalWrite(MS2C, HIGH); //torque for grabbing
digitalWrite(MotorEnableC, HIGH); //Setting each enable pin as high cuts the p
ower to
digitalWrite(MotorEnableR, HIGH); //each stepper and lets it coast
digitalWrite(MotorEnableL, HIGH); //
}
//
// loop forever
//
void loop(){
blucontrol(); //this is the supreme subroutine that enables us to command ever
ything with bluetooth commands

}
//
// set speed, destination, dir for left wheel///////////////////////////////////
//////
//
void destL(int speed, long int pos, int dir) {
stepperL.setMaxSpeed(speed); //sets the speed for the left stepper to whatever
we pass it (int speed)
stepperL.moveTo(pos); //sets the distance for the left stepper to rotate to wh
atever we pass it(long int pos)
}
//
// set speed, dest, dir for right wheel/////////////////////////////////////////
//////
//
void destR(int speed, long int pos, int dir) {
stepperR.setMaxSpeed(speed); //sets the speed for the right stepper to whateve
r we pass it (int speed)
stepperR.moveTo(pos); //sets the distance for the right stepper to rotate to w
hatever we pass it(long int pos)
}
//
// set speed, dest, dir for gripper
//
void destC(int speed, long int pos, int dir) {
stepperC.setMaxSpeed(speed); //sets the speed for the grabber to whatever we p
ass it(int speed)
stepperC.moveTo(pos); //sets the distance for the grabber to rotate to whateve
r we pass it(long int pos)
}
//
// move both wheels a predefined distance///////////////////////////////////////
/////
//
void move(){
wheelsOn();//this subroutine sets the enable pins for the wheels to LOW so tha
t they won't coast and we can control them
while (stepperL.distanceToGo() !=0 || stepperR.distanceToGo() != 0) { //loop w
hile both steppers still have a distance to rotate
stepperL.run(); //this command from the library tells the steppers to rotate
stepperR.run(); // to the distance set and the speed set
}
wheelsOff();//this subroutine sets the enable pins for the wheels to HIGH so t
hat they can coast again and stop drawing current
}
//
// grip

// moves the claw a predefined distance/////////////////////////////////////////

///
//
void grip(){
digitalWrite(MotorEnableC, LOW); //this enables the grabber to be controlled
while (stepperC.distanceToGo() !=0 ) {//loop if the grabber still has a distan
ce to rotate
stepperC.run(); //tells the stepper to rotate to the distance set based on
the speed set
}
}
//
// pingAve(int num, int sen) - average num pings from which sensor
//
int pingAve(int num, int sen) { //subroutine pingAve to receive an average readi
ng from whichever called sensor
int
int
int
int

dist=0;
distTot=0;
count=0;
distAve;

if(sen == 1){
for (int x=0; x < num; x++) { //loops the reading "num" amount of times
dist = sonarL.ping()/US_ROUNDTRIP_CM; //sets dist as the reading of the le
ft sensor
if (dist > 0 && dist < MaxSonar) { //if reading is greater than 0 and less
than max do
distTot+=dist; //disto is set equal to its current value plus dist
count += 1; //count is set equal to its current value plus one
}
else if (dist <= 0) { //deal with things on the edge of MaxSonar range
distTot+=MaxSonar; // distto equals its current value plus max sonar p
ossible reading
count += 1; // count is set equal to its current value plus one
}
delayMicroseconds(5000); //min time between pings or they jam each other
}
}
else if(sen == 2){ //completely the same as above except it calls center senso
r instead
for (int x=0; x < num; x++) {
dist = sonarC.ping()/US_ROUNDTRIP_CM;
if (dist > 0 && dist < MaxSonar) {
distTot+=dist;
count += 1;
}
else if (dist <= 0) {
distTot+=MaxSonar;
count += 1;
}
delayMicroseconds(5000);
}
}

else{ //same except it calls right sensor instead

for (int x=0; x < num; x++) {
dist = sonarR.ping()/US_ROUNDTRIP_CM;
if (dist > 0 && dist < MaxSonar) {
distTot+=dist;
count += 1;
}
else if (dist <= 0) {
distTot+=MaxSonar;
count += 1;
}
delayMicroseconds(5000);
}
}
distAve = distTot/count; //distto is now sum of all reading from one sensor
return(distAve); //this divides it by the amount of times it's added together
to find average and returns the average
}

//
// disable wheels to save power. Freewheels. //////////////////////////////////
//
void wheelsOff(void) {//void means that we're not passing it any variables
digitalWrite(MotorEnableR, HIGH); //sets the enable pins high to stop current
draw and
digitalWrite(MotorEnableL, HIGH); //allow coasting
}
//
// enable wheels. Uses power to sit still, won't roll /////////////////////////
//
void wheelsOn(void) {
digitalWrite(MotorEnableR, LOW); //sets the enable pins low to resume current
draw and
digitalWrite(MotorEnableL, LOW); //allow control
}
//
// Centering while driving ///////////////////////////////////////////////
//
void center(){ //this subroutine keeps the bot centered through the lanes and a
cts as navigation
stepperL.setCurrentPosition(0);
//sets the current position of each whee
l to 0
stepperR.setCurrentPosition(0);
//the distance they travel is relative t
o their current position
pingDistL=pingAve(5,1);
// Reads left sensor
pingDistR=pingAve(5,3);
// Reads right sensor
if (pingDistL > pingDistR){
// curve left if left wall is farth
er than right wall
moveDist1 = 40;
// distance for the left motor to travel
is 40 steps
moveDist2 = 125;
// distance for the right motor to trave
l is 125

}
else if (pingDistL < pingDistR){
// curve right if left wall is closer
than right wall
moveDist1 = 125;
// distance for left motor to travel is
125 steps
moveDist2 = 40;
// distance for right motor to travel is
40 steps
}
else{
// go strait if walls are equal d
istance apart
moveDist1 = 125;
// both distances to travel are 125
moveDist2 = 125;
}
destL(maxspeed1, moveDist1, 1);
// this subroutine sets the speeds and
destR(maxspeed1, moveDist2, 1);
// and accelerations we want each wheel
to spin at
move();
// executes the .run function of the lib
rary
pingDistC=pingAve(5,2);
// reads front sensor
if (pingDistC > 4){
// loops until it hits a wall close
r than 4 cm
center();
// run the centering subroutine
}
}
//
// Serial console (Bluetooth) keyboard control
//
// j - backward,
k - forward
// h - left turn,
l - right turn
// 1 - full step,
2 - half step
// 3 - MS1 L, MS2 H, 4 - MS1 H, MS2 L
// w- increase speed, q - decrease speed
// a - decrease turn, s - increase turn
// z - decrease dist, x - increase dist
// o - decrease accel, p - increase accel
void blucontrol(){ //subroutine start here
int speed = 800; // speed variable called throughout routine
int y = 50; // distance to turn called in the turn case statements
int q = 100; // called as a variable to increase by
int R = 0; // starting distance for right wheel to go
int L = 0; // starting distance for left wheel to go
int g = 350; //starting grabber distance to go
int gSpeed = 200; //grabber speed
int j = 0;
// grabber starting position
Serial.println("v - display commands"); // press v to print all other commands
Serial.println("");
while(1){ //loop forever
stepperL.setCurrentPosition(0); //reset current position for both motors
stepperR.setCurrentPosition(0);
L = 0; //starting distances for both wheels is 0
R = 0;
int z = Serial.read(); //reads for case statements

switch(z){
case 'k': //if 'k' is pressed...
R += q; //increase the distance for both wheels to rotate by the value of
q
L += q;
Serial.println(y); // print distance
break;// ends case statement
case 'j': //if j is pressed..
R -= q; //decrease the distance for both wheels to rotate by the value of
q
L -= q;
Serial.println(y); //print distance
break;
case 'h': //if h is pressed...
L += y; //increase left distance to go by value y
R -= y; //decrease right distance to go by value y
Serial.println("Turning Left"); //print "turning left"
break;
case 'l': // if l is pressed...
R += y; //increase right distance to go by value y
L -= y; //decrease left distance to go by value y
Serial.println("Turning Right"); //print "turning right"
break;
case 'w': //if w is pressed...
maxspeed1 += 50; //increase the maxspeed called in Dest(R/L) subroutines
by 50
Serial.print("speed now:"); Serial.println(maxspeed1); //prints "speed now
" followed by the actual value of the speed
break;
case 'q'://if q is pressed...
maxspeed1 -= 50; //decrease the maxpeed called in Dest(R/L) subroutines by
50
Serial.print("speed now:"); Serial.println(maxspeed1); //prints "speed now
" followed bu the actual value of the speed
break;
case 's': //if s is pressed...
y += 10; //the value of y is increased by 10
Serial.print("turn now:"); Serial.println(y); // prints "turn now" followe
d by the value of y
break;
case 'a': //if a is pressed...
y -= 10; //the value of y in decreased by 10
Serial.print("turn now:"); Serial.println(y); //prints "turn now" followed
by the value of y
break;

case 'x': //if x is pressed...

q += 10; //the value of q is increased by 10
Serial.print("dist now:"); Serial.println(q); //prints "dist now" followed
by the value of q
break;
case 'z': //if z is pressed...
q -= 10; //the value of q is decreased by 10
Serial.print("dist now:"); Serial.println(q); //prints "dist now" followed
by the value of q
break;
case 'p': //if p is pressed...
motoraccel += 50; //increases the motor's acceleration by 50
Serial.print("accel now:"); Serial.println(motoraccel);//prints "accel now
" followed by value of motoraccel
break;
case 'o': //if o is pressed...
motoraccel -= 50; //decrease the motor's accelerations by 50
Serial.print("accel now:"); Serial.println(motoraccel); //prints "accel no
w" followed value of motoraccel
break;
case ',': //if , is pressed...
j -= g;// j is decreased by the value of g
Serial.print("grip now:"); Serial.println(g); //prints g
break;
case '.'://if . is pressed...
j += g;// j is increased by the value of g
Serial.print("grip now:"); Serial.println(g); //prints g
break;
case '2'://if 2 is pressed...
digitalWrite(MS1R, HIGH); //this allows us to change the step size of all
steppers
digitalWrite(MS2R, HIGH); //to 1/8 of the full step step
digitalWrite(MS1L, HIGH);
digitalWrite(MS2L, HIGH);
digitalWrite(MS1C, HIGH);
digitalWrite(MS2C, HIGH);
Serial.println("1/8 step");
break;
case '1'://if 1 is
digitalWrite(MS1R,
digitalWrite(MS2R,
digitalWrite(MS1L,
digitalWrite(MS2L,
digitalWrite(MS1C,
digitalWrite(MS2C,

pressed...
LOW); //this sets the step size to full
LOW);
LOW);
LOW);
LOW);
LOW);

Serial.println("full Step");
break;
case '3'://if 3 is pressed...
digitalWrite(MS1R, LOW); //this sets the step size to 1/4 step
digitalWrite(MS2R, HIGH);
digitalWrite(MS1L, LOW);
digitalWrite(MS2L, HIGH);
digitalWrite(MS1C, LOW);
digitalWrite(MS2C, HIGH);
Serial.println("1/4 step");
break;
case '4'://if 4 is pressed...
digitalWrite(MS1R, HIGH); //this sets the step size to 1/2 step
digitalWrite(MS2R, LOW);
digitalWrite(MS1L, HIGH);
digitalWrite(MS2L, LOW);
digitalWrite(MS1C, HIGH);
digitalWrite(MS2C, LOW);
Serial.println("1/2 step");
break;
case 'n'://if n is pressed...
digitalWrite(MotorEnableC, HIGH);//disable grabber
Serial.println("Disable Grab");
break;
case 'm': //if m is pressed...
digitalWrite(MotorEnableC, LOW);//enable grabber
Serial.println("Enable Grab");
break;
case 'u': //if u is pressed...
g = g -5; //variable g is decreased by 5
Serial.println(g);
break;
case 'i': //if i is pressed...
g = g + 5;//variable g is increased by 5
Serial.println(g);
break;
case 'y': //if y is pressed...
Serial.print(pingAve(5,1));
//take reading of all sensors and print a
ll values
Serial.println(" left sensor");
Serial.print(pingAve(5,2));
Serial.println(" Center sensor");
Serial.print(pingAve(5,3));
Serial.println(" Right sensor");
break;

case 'r'://if r is pressed...

L = pingAve(5, 2) * 25.8; //distance to go for wheels equals the distanc
e read by front sensor
R = L;
move();
Serial.println("Moving to sensor ");
Serial.print(R);
Serial.print(" cm");
break;

case 'v': //if case v is pressed...

Serial.println(" j - backward,
k - forward ");
//print all c
ommands
Serial.println(" h - left turn,
l - right turn ");
//and their f
unctions
Serial.println(" 1 - full step,
2 - half step ");
Serial.println(" 3 - MS1 L, MS2 H, 4 - MS1 H, MS2 L ");
Serial.println(" w - increase speed, q- decrease speed ");
Serial.println(" a - decrease turn, s - increase turn ");
Serial.println(" z - decrease dist, x - increase dist ");
Serial.println(" o - decrease accel, p - increase accel ");
Serial.println(" , - drop, . - grab ");
Serial.println(" u - decrease grab dist, i - increase grab dist ");
Serial.println(" v - display commands menu ");
Serial.println(" @ - autonomous mode ");
break;
case '@': //if @ is pressed...
Serial.println("autonomous mode");
autonomous();//run autonomous mode
break;
}
stepperL.setAcceleration(motoraccel); // set all aspects of wheels to variable
changed
stepperR.setAcceleration(motoraccel); // in the case statements
destL(maxspeed1, L, 1);
destR(maxspeed1, R, 1);
destC(gSpeed, j, 1); //then run wheels & grabber based on those aspects
move();
grip();
}
}
void autonomous(){ //the autonomous subroutine
straitaway();
// drives up to ball
ballGrab();
// grabs ball and turns into right lane
rightLane();
// centers the bot until the front sensor is trigg
ered
ballDrop();
// drops the ball into the drop off

}
void straitaway(){
moveDist1 = 25.8 * pingAve(5,2);//converts the distance in cm to the number of
steps
destL(maxspeed1,moveDist1,1);
destR(maxspeed1,moveDist1,1);
move();
// moves up to wall
}
void ballGrab(){
stepperR.setCurrentPosition(0);
stepperL.setCurrentPosition(0);
stepperC.setCurrentPosition(0);
destC(200,250,1);
grip();
destR(maxspeed1,-220,1);
destL(maxspeed1,-220,1);
move();
destL(maxspeed1,200,1);
move();

// grabs ball
// backs up
// turns right

}
void rightLane(){
center();
lane
stepperR.setCurrentPosition(0);
stepperL.setCurrentPosition(0);
destL(maxspeed1,100,1);
destR(maxspeed1,100,1);
move();
}
void ballDrop(){
destC(200,0,1);
grip();
digitalWrite(MotorEnableC, HIGH);
destL(maxspeed1,105,1);
destR(maxspeed1,105,1);
move();
ssary
stepperR.setCurrentPosition(0);
stepperL.setCurrentPosition(0);
destL(maxspeed1,100,1);
destR(maxspeed1,-100,1);
move();
stepperR.setCurrentPosition(0);
stepperL.setCurrentPosition(0);
for(int x=0;x<3;x++){
destL(maxspeed1,50,1);
destR(maxspeed1,50,1);
move();
destL(maxspeed1,-50,1);
destR(maxspeed1,-50,1);
move();

// keeps the bot in the center of the

// goes a bit extra just in case

// drops ball

// pushes the ball into the box if nece

// tips it in if still necessary

// victory dance (derp dance)

}
}