#include <Servo.

h>
Servo myservo;
int trigPinOne = A0, echoPinOne =A1;
int trigPinTwo= A2, echoPinTwo= A3;
int trigPinThree= A4, echoPinThree= A5;
int motorOne =11;
int motorTwo= 10;
void setup(){
Serial.begin(9600);
pinMode(motorOne, OUTPUT);
pinMode(motorTwo, OUTPUT);
myservo.attach(9);

pinMode(trigPinOne, OUTPUT); pinMode(trigPinTwo, OUTPUT); pinMode(trigPinThre
e, OUTPUT);
pinMode(echoPinOne, INPUT); pinMode(echoPinTwo, INPUT); pinMode(echoPinThre
e, INPUT);

}
// DEFINES FUNCTIONS USED IN MAIN LOOP:
// Ultrasonic Sensor
//////////////////////////////////////////////
void useTrigger(int trigPinNumber){
digitalWrite(trigPinNumber, HIGH);
delayMicroseconds(1000);
digitalWrite(trigPinNumber, LOW);
}
void goStraight(float Speed){
// Right (1) wheel is stronger
analogWrite(motorOne, Speed-15);
analogWrite(motorTwo, Speed);
}
////////////////////////////////////////////////
float returnDistance(int echoPinNum){
float duration= pulseIn(echoPinNum, HIGH);
float distance= (duration/2.0) / 29.1; // converts to CM
return distance;
}
void moveMotorPower(int motorNum, float powerLevel){
if (powerLevel > 255){ analogWrite(motorNum, 255);
}
if (powerLevel < 0){ analogWrite(motorNum, 0);
}
else{ analogWrite(motorNum, powerLevel);
}
}
////////////////////////////////////////////////
// MAIN LOOP
void loop(){
float distanceOne, durationOne, powerLevelOne;
float distanceTwo, durationTwo, powerLevelTwo;
float distanceThree, durationThree;
float setPoint= 5.0; // stay at setPoint distance from wall
int straightSpeed = 60;
// read RIGHT Ultrasonic Sensor (1)
useTrigger(trigPinOne);
distanceOne= returnDistance(echoPinOne);
Serial.print("Distance 1: ");
Serial.print(distanceOne);
// read LEFT Ultrasonic Sensor (2)
useTrigger(trigPinTwo);
distanceTwo= returnDistance(echoPinTwo);
Serial.print(" | Distance 2: ");
Serial.print(distanceTwo);
// read FRONT Ultrasonic Sensor (3)
useTrigger(trigPinThree);
distanceThree= returnDistance(echoPinThree);
Serial.print( " | Distance 3: ");
Serial.print(distanceThree);
// start proportional calculation
float propOut = 12.3 * distanceTwo - 93; // calculated: = 14.3* dist2 -93
if (propOut > 200){ propOut= 200;
}

Serial.print( " | prop: ");
Serial.println(propOut);
// nothing infront, PID to find left
if ( distanceThree > 8.0 ){
moveMotorPower(motorTwo, straightSpeed+ (-1)*(propOut));
moveMotorPower(motorOne, straightSpeed + propOut);

if (distanceThree < 8.0) {
moveMotorPower(motorTwo, 0);
moveMotorPower(motorOne, 250);
delay(2000);
moveMotorPower(motorTwo, 100);
moveMotorPower(motorOne, 100);
delay(1000);
}


// EXTEND MECHANISm
if ( ( distanceOne < 10.0) && (distanceTwo <10.0) && (distanceThree <10.0) ){
// extends mechanism
int pos;
for(pos = 90; pos>=0; pos-=5){
myservo.write(pos);
delay(60);
}
//returns mechanism
for(pos = 0; pos < 90; pos += 1){
myservo.write(pos);
delay(40);
}
delay (2000);
}

/*
else if (distanceTwo < 10) {
moveMotorPower(motorTwo, 150);
moveMotorPower(motorOne, 10);
}

else if (distanceOne > 10 && distanceTwo > 10) {
moveMotorPower(motorTwo, 10);
moveMotorPower(motorOne, 150);
}

else if (distanceOne < 10 && distanceTwo < 10) {
moveMotorPower(motorTwo, 0);
moveMotorPower(motorOne, 255);
delay(5000);
}
}
*/



}
}