/*

* for demo purposes

*/

const byte led = 15; // aka A1

const byte speaker = 3;
const byte lMotorA = 5;
const byte lMotorB = 9;
const byte rMotorA = 6;
const byte rMotorB = 10;
const byte lTracker = 16;
const byte rTracker = 17;

void setup() {
// put your setup code here, to run once:
pinMode(led, OUTPUT);
pinMode(speaker, OUTPUT);
pinMode(lMotorA, OUTPUT);
pinMode(lMotorB, OUTPUT);
pinMode(rMotorA, OUTPUT);
pinMode(rMotorB, OUTPUT);
pinMode(lTracker, INPUT);
pinMode(rTracker, INPUT);
}

void loop() {
// put your main code here, to run repeatedly:

if(distance(2,4)>10) {
if(leftTracker()==0 && rightTracker()==0){
forward(40);
} else {

if(leftTracker()==1 && rightTracker()==0){

left(38,5); }

if(leftTracker()==0 && rightTracker()==1){

right(38,5); }

if(leftTracker()==1 && rightTracker()==1){

stop(); }
}

} else if (distance(2,4)<10){
right(40,300);
stop();
delay(700);
forward(40);
delay(700);
left(40,400);
stop();
delay(500);
}
}
void ledOn() {
digitalWrite(led, HIGH);
}

void ledOff() {
 digitalWrite(led, LOW);
}

void beep(byte power, int duration) {

analogWrite(speaker, power);
delay(duration);
analogWrite(speaker, 0);
}

void setLeftMotor(int power) {

byte p=abs(power*2.55);
if (power<0) {
analogWrite(lMotorA, 0);
analogWrite(lMotorB, p);
} else {
analogWrite(lMotorA, p);
analogWrite(lMotorB, 0);
}

void setRightMotor(int power) {

byte p=abs(power*2.55);
if (power<0) {
analogWrite(rMotorA, 0);
analogWrite(rMotorB, p);
} else {
analogWrite(rMotorA, p);
analogWrite(rMotorB, 0);
}
}

void stop() {
setLeftMotor(0);
setRightMotor(0);
}

void forward(byte power) {

setLeftMotor(power);
setRightMotor(power);
}

void reverse(byte power) {

setLeftMotor(0-power);
setRightMotor(0-power);
}

void left(byte power, int duration) {

setLeftMotor(0-power);
setRightMotor(power);
delay(duration);
stop();
}

void right(byte power, int duration) {

setLeftMotor(power);
setRightMotor(0-power);
delay(duration);
stop();
}

float distance(byte trigPin, byte echoPin) {

float dist,duration;
// set the pin modes
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);

digitalWrite(trigPin, LOW); // clean up

delayMicroseconds(2); // wait for 2 microseconds
digitalWrite(trigPin, HIGH); // trigger signal
delayMicroseconds(10); // wait for 10 microseconds
digitalWrite(trigPin, LOW); // remove the trigger signal

// wait for echo pin to go HIGH and save time in microseconds

duration = pulseIn(echoPin, HIGH);
dist = (duration/2) / 29.15; // 29.15 microseconds per centimeter
return dist;
}

byte leftTracker() {
return digitalRead(lTracker);
}

byte rightTracker() {
return digitalRead(rTracker);
}