//include libraries
#include <Servo.h>
//set-up wheel variables
const int rightfrontmotorOne = 2; // H-bridge1 Right Front leg 1(pin 2, 1A)
const int rightfrontmotorTwo = 3; // H-bridge1 Right Front leg 2(pin 7, 2A)
const int rightrearmotorOne = 0; // H-bridge1 Right Rear leg 1(pin 15, 1A)
const int rightrearmotorTwo = 1; // H-bridge1 Right Rear leg 2(pin 10, 2A)
const int leftfrontmotorOne = 7; // H-bridge1 Left Front leg 1(pin 2, 1A)
const int leftfrontmotorTwo = 8; // H-bridge1 Left Front leg 2(pin 7, 2A)
const int leftrearmotorOne = 4; // H-bridge2 Left Rear leg 1(pin 15, 1A)
const int leftrearmotorTwo = 5; // H-bridge2 Left Rear leg 2(pin 10, 2A)
const int enablePinRF = 6; // H-bridge1 Right Front enable pin
const int enablePinRR = 9; // H-bridge1 Right Rear enable pin
const int enablePinLF = 10; // H-bridge2 Left Front enable pin
const int enablePinLR = 11; // H-bridge2 Left Rear enable pin
//extra variables
boolean forwardDrive;
//sensor variables
int sensorPin = 0;
int sensorVal = 0;
boolean carOK = true;
//servo variables
Servo mainServo;
int servoPin = 12;
//wheel functions
void controlRightFront(boolean one, boolean two, int x);
void controlRightRear(boolean one, boolean two, int x);
void controlLeftFront(boolean one, boolean two, int x);
void controlLeftRear(boolean one, boolean two, int x);
void forward(int x);
void reverse(int x);
void slowDown(boolean one, boolean two, int x);
void halt();
//sensor functions
boolean viewIsClear();
//servo functions
void setup() {
// set all the other pins you're using as outputs:
pinMode(rightfrontmotorOne, OUTPUT);
pinMode(rightfrontmotorTwo, OUTPUT);
pinMode(rightrearmotorOne, OUTPUT);
pinMode(rightrearmotorTwo, OUTPUT);
pinMode(leftfrontmotorOne, OUTPUT);
pinMode(leftfrontmotorTwo, OUTPUT);
pinMode(leftrearmotorOne, OUTPUT);
pinMode(leftrearmotorTwo, OUTPUT);
pinMode(enablePinRF, OUTPUT);
pinMode(enablePinRR, OUTPUT);
pinMode(enablePinLF, OUTPUT);
pinMode(enablePinLR, OUTPUT);
// set enablePin high so that motor can turn on:
analogWrite(enablePinRF, 0);
analogWrite(enablePinRR, 0);
analogWrite(enablePinLF, 0);
analogWrite(enablePinLR, 0);
}
void loop() {
if (viewIsClear())
{
forward(100);
}
else
{
reverse(155);
}
}
void controlRightFront(boolean one, boolean two, int x)
{
analogWrite(enablePinRF, x);
digitalWrite(rightfrontmotorOne, one);
digitalWrite(rightfrontmotorTwo, two);
}
void controlRightRear(boolean one, boolean two, int x)
{
Serial.println("I was called!");
analogWrite(enablePinRR, x);
digitalWrite(rightrearmotorOne, one);
digitalWrite(rightrearmotorTwo, two);
}
void controlLeftFront(boolean one, boolean two, int x)
{
analogWrite(enablePinLF, x);
digitalWrite(leftfrontmotorOne, one);
digitalWrite(leftfrontmotorTwo, two);
}
void controlLeftRear(boolean one, boolean two, int x)
{
analogWrite(enablePinLR, x);
digitalWrite(leftrearmotorOne, one);
digitalWrite(leftrearmotorTwo, two);
}
void reverse(int x)
{
if (forwardDrive == true)
{
halt();
delay(400);
}
controlRightFront(LOW,HIGH,x);
controlRightRear(LOW,HIGH,x);
controlLeftFront(LOW,HIGH,x);
controlLeftRear(LOW,HIGH,x);
}
void forward(int x)
{
forwardDrive = true;
controlRightFront(HIGH,LOW,x);
controlRightRear(HIGH,LOW,x);
controlLeftFront(HIGH,LOW,x);
controlLeftRear(HIGH,LOW,x);
}
void halt()
{
controlRightFront(LOW,LOW,0);
controlRightRear(LOW,LOW,0);
controlLeftFront(LOW,LOW,0);
controlLeftRear(LOW,LOW,0);
}
void slowDown(boolean one, boolean two, int x)
{
controlRightFront(one,two,x);
controlRightRear(one,two,x);
controlLeftFront(one,two,x);
controlLeftRear(one,two,x);
}
boolean viewIsClear()
{
sensorVal = analogRead(sensorPin);
if (sensorVal > 300)
{
carOK = false;
return false;
}
else
return true;
}