#include <Servo.

h>
// Robot Project
// Attach the left and right servo control wires (white)
// to pins 6 and 7 respectively.
#define SERV0_LEFT 5
#define SERVO_RIGHT 6
// Attach the left and right wheel encoder wires
// to pins A4 and A11
#define LEFT_WHEEL_SENSOR A11
#define RIGHT_WHEEL_SENSOR A4
// Attach the front and side IR Sensor wires
// to pins A0 and A8
#define FRONT_IR_SENSOR A0
#define SIDE_IR_SENSOR A3

#define FORWARDSTRAIGHTPOWERLEFT 40
#define FORWARDSTRAIGHTPOWERRIGHT 40
#define STOP_DISTANCE 12
//direction argument defines
#define FORWARD 1
#define REVERSE 2
#define BACKWARD 2
#define TURNLEFT 3
#define TURNRIGHT 4
///
#define BEARLEFTPOWERLEFT 1.375
#define BEARLEFTPOWERRIGHT 8.625
#define BEARRIGHTPOWERRIGHT 3.33
#define BEARRIGHTPOWERLEFT 5.675
//Pulse counts necessary to get the servos to stop

#define LSERVOREST 1505

#define RSERVOREST 1480

// Upper Schmidt trigger threshold

#define UPPER_ENCODER_THRESHOLD 800
// Lower Schmidt trigger threshold
#define LOWER_ENCODER_THRESHOLD 700

// Note: A maximum of eight servo objects can be created

Servo myservo_left;// create servo object to control a servo
Servo myservo_right;

//function prototypes
void forward(int speed, int stopDist);
void forward(int speed);
void driveEncoder(int SpokeTransitions, int servoSpeed, int robotDirection);
float sideSensor2cm(float analogValue);
float frontSensor2cm(float analogValue);
void wallFollow(float dist, int stopDist , int speed);
void leftToWall(int speed);
void rightToWall(int speed);
void stop_robot();
void setup() {
// attaches the left wheel servo on pin 6 to servo object
myservo_left.attach(SERV0_LEFT);
// attaches the right wheel servo on pin 7 to servo object
myservo_right.attach(SERVO_RIGHT);

Serial.begin(19200); //for serial IO to screen

Serial.println("Welcome to my servo tester!");

}
void loop() {
driveEncoder(7, 10, TURNRIGHT);
delay(500);
forward(10, STOP_DISTANCE);
delay(500);
driveEncoder(8, 10, TURNRIGHT);
delay(500);
forward(10, STOP_DISTANCE);
delay(500);
driveEncoder(8, 10, TURNLEFT);
delay(500);
forward(10, 15);
delay(500);
driveEncoder(8, 10, TURNLEFT);
delay(500);
wallFollow(15, STOP_DISTANCE, 10);
stop_robot();
delay(2000);
// float dist = frontSensor2cm(analogRead(FRONT_IR_SENSOR));
// while (dist < STOP_DISTANCE){
//

stop_robot();

//

float dist = frontSensor2cm(analogRead(FRONT_IR_SENSOR));

//

}
forward(10, STOP_DISTANCE);
delay(300);
driveEncoder(8, 10, TURNLEFT);
delay(300);
forward(10, STOP_DISTANCE);

}
//Function driveEncoder- moves the robot a determined
//

number of spoke transitions and speed

//

in the forward, reverse, turn left, or

//

turn right direction

// Input args:
//

int forwardSpokeTransitions- number of spoke transitions to move forward

//

int servoSpeed- speed to move forward, 1 is slowest, 10 is fastest

//

int robotDirection- 1 to move robot forward, 2 to move in reverese,

//

3 for left turn, 4 for right turn

// Returns a void
void driveEncoder(int SpokeTransitions, int servoSpeed, int robotDirection) {
int pos = 0;

// variable to store the servo position

boolean LeftWheelSensorWasHigh = false,

RightWheelSensorWasHigh = false;
int LeftWheelSensorValue,
LeftWheelSpokeCount = 0,
RightWheelSensorValue;

// Get initial left value and determine if high

LeftWheelSensorValue = analogRead(LEFT_WHEEL_SENSOR);
if (LeftWheelSensorValue > UPPER_ENCODER_THRESHOLD)
LeftWheelSensorWasHigh = true;

// Get initial right value and determine if high

RightWheelSensorValue = analogRead(RIGHT_WHEEL_SENSOR);
if (RightWheelSensorValue > UPPER_ENCODER_THRESHOLD)
RightWheelSensorWasHigh = true;

if (servoSpeed < 1) servoSpeed=1;

if (servoSpeed > 10) servoSpeed = 10;

// start robot moving

if (robotDirection == FORWARD) //forward
{
myservo_left.
writeMicroseconds(LSERVOREST+FORWARDSTRAIGHTPOWERLEFT*servoSpeed);
myservo_right. writeMicroseconds(RSERVORESTFORWARDSTRAIGHTPOWERRIGHT*servoSpeed);
}
else if (robotDirection == REVERSE) //reverse
{
myservo_left. writeMicroseconds(LSERVORESTFORWARDSTRAIGHTPOWERLEFT*servoSpeed);
myservo_right.
writeMicroseconds(RSERVOREST+FORWARDSTRAIGHTPOWERRIGHT*servoSpeed);
}
else if (robotDirection == TURNLEFT) //left turn
{
myservo_left. writeMicroseconds(LSERVORESTFORWARDSTRAIGHTPOWERLEFT*servoSpeed);
myservo_right. writeMicroseconds(RSERVORESTFORWARDSTRAIGHTPOWERRIGHT*servoSpeed);
}
else if (robotDirection == TURNRIGHT) //right turn
{
myservo_left.
writeMicroseconds(LSERVOREST+FORWARDSTRAIGHTPOWERLEFT*servoSpeed);
myservo_right.
writeMicroseconds(RSERVOREST+FORWARDSTRAIGHTPOWERRIGHT*servoSpeed);
}

while (LeftWheelSpokeCount < SpokeTransitions) {

// Get new sensor values
LeftWheelSensorValue = analogRead(LEFT_WHEEL_SENSOR);
RightWheelSensorValue = analogRead(RIGHT_WHEEL_SENSOR);

// Determine if spoke transition

if (LeftWheelSensorWasHigh) {
if (LeftWheelSensorValue < LOWER_ENCODER_THRESHOLD) {
LeftWheelSpokeCount++;
LeftWheelSensorWasHigh = false;
Serial.print("Spoke edges Went Lo, spoke count = ");
Serial.println(LeftWheelSpokeCount,DEC); //print decimal value
}
}
else {
if (LeftWheelSensorValue > UPPER_ENCODER_THRESHOLD) {
LeftWheelSpokeCount++;
LeftWheelSensorWasHigh = true;
Serial.print("Spoke edges Went Hi, spoke count = ");
Serial.println(LeftWheelSpokeCount,DEC); //print decimal value
}
}
}

myservo_left. writeMicroseconds (LSERVOREST);

myservo_right. writeMicroseconds (RSERVOREST);
}

float frontSensor2cm(float analogFront){

float cntFront = float(analogFront);

return 37620*pow(cntFront, -1.256);
}
float sideSensor2cm(float analogSide){
float cntSide = float(analogSide);
return 30336*pow(cntSide, -1.222);
}
//print decimal value
// forward function that drives the robot before
// an obstacle is encountered
void forward(int speed, int stopDist){
int analogValue;

if (speed < 1) speed=1;

if (speed > 10) speed = 10;
while(1){
analogValue = analogRead(FRONT_IR_SENSOR);
if(frontSensor2cm(analogValue) <= stopDist){
stop_robot() ;
break;
}
myservo_left.writeMicroseconds(LSERVOREST+FORWARDSTRAIGHTPOWERLEFT*spe
ed);
myservo_right.writeMicroseconds(RSERVORESTFORWARDSTRAIGHTPOWERRIGHT*speed);
}
}

void wallFollow( float wallDist, int stopDist, int speed) {

if (speed < 1) speed = 1;

if (speed > 10) speed = 10;
float distance = frontSensor2cm(analogRead(FRONT_IR_SENSOR));
while (distance > stopDist){
int analogValue = analogRead(SIDE_IR_SENSOR);
distance = frontSensor2cm(analogRead(FRONT_IR_SENSOR));
if(sideSensor2cm(analogValue) > wallDist + 2){
rightToWall(speed);
}
else if(sideSensor2cm(analogValue) < wallDist - 2){
leftToWall(speed);
}
else{
forward(speed);
}
}
}

void stop_robot(){
myservo_left.writeMicroseconds(LSERVOREST);
myservo_right.writeMicroseconds(RSERVOREST);
}

void forward(int speed){

if (speed < 1) speed = 1;
if (speed > 10) speed = 10;
myservo_left.writeMicroseconds(LSERVOREST+FORWARDSTRAIGHTPOWERLEFT*spe
ed);

myservo_right.writeMicroseconds(RSERVORESTFORWARDSTRAIGHTPOWERRIGHT*speed);
}

void leftToWall(int speed){

if (speed < 1) speed = 1;
if (speed > 10) speed = 10;
myservo_left.writeMicroseconds(LSERVOREST+BEARLEFTPOWERLEFT*speed);
myservo_right.writeMicroseconds(RSERVOREST-BEARLEFTPOWERRIGHT*speed);
}

void rightToWall(int speed){

if (speed < 1) speed = 1;
if (speed > 10) speed = 10;
myservo_left.writeMicroseconds(LSERVOREST+BEARRIGHTPOWERLEFT*speed);
myservo_right.writeMicroseconds(RSERVOREST-BEARRIGHTPOWERRIGHT*speed);
}