Done, working much better now thanks, just need to get an IR LED and IR phototransistors.
const int lightSensor1 = A0;
const int lightSensor2 = A1;
const int forward = 5;
const int backward = 3;
const int left = 10;
const int right = 11;
const int topFlasher = 13;
const int trigPin = 12;
const int echoPin = 4;
int forwardAnalog = 0;
int targetSpeed = 6;
int CMPSSelector = 0;
int CMPSDistanceAverage = 0;
int CMPSStorage1;
int CMPSStorage2;
int CMPSStorage3;
int CMPSStorage4;
int currentLight1;
int currentLight2;
int gapValue;
int highest;
int lowest;
int CMPSStorageSelector = 1;
long duration, distance;
void setup(){
Serial.begin(9600); // Start printing to Serial.
// Set all pinModes.
pinMode(forward, OUTPUT);
pinMode(backward, OUTPUT);
pinMode(left, OUTPUT);
pinMode(right, OUTPUT);
pinMode(topFlasher, OUTPUT);
pinMode(lightSensor1, INPUT);
pinMode(lightSensor2, INPUT);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
// Set default position of outputs.
digitalWrite(forward, LOW);
digitalWrite(backward, LOW);
digitalWrite(left, LOW);
digitalWrite(right, LOW);
digitalWrite(topFlasher, LOW);
// Determine the gap between both light sensors.
if(lightSensor1 > lightSensor2){
highest = lightSensor1;
lowest = lightSensor2;
} else {
highest = lightSensor2;
lowest = lightSensor1;
}
gapValue = (highest - lowest) + 20;
}
void loop(){
ultraSonic();
CMPS();
// Set variable to read light sensors, and print to Serial.
currentLight1 = analogRead(lightSensor1);
currentLight2 = analogRead(lightSensor2);
/* Serial.print(currentLight1);
Serial.print(" - ");
Serial.print(currentLight2);
Serial.print(" - ");
Serial.print(gapValue);
Serial.println(" "); */
// Drive.
forwardAction();
// Line following code.
/*if(currentLight2 > currentLight1 + gapValue){
digitalWrite(left, HIGH);
digitalWrite(right, LOW);
topBeacon();
} else if (currentLight1 > currentLight2 + gapValue) {
digitalWrite(right, HIGH);
digitalWrite(left, LOW);
topBeacon();
} else {
digitalWrite(right, LOW);
digitalWrite(left, LOW);
} */
}
// Flashing light on roof, and pin 13.
void topBeacon(){
if( (millis() / 1000 % 2) == 0){
digitalWrite(topFlasher, HIGH);
} else {
digitalWrite(topFlasher, LOW);
}
}
void ultraSonic(){
digitalWrite(trigPin, LOW); // Added this line
delayMicroseconds(2); // Added this line
digitalWrite(trigPin, HIGH);
// delayMicroseconds(1000); - Removed this line
delayMicroseconds(10); // Added this line
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
if(distance >= 400){
distance = 0;
}
if(distance <= 4){
distance = 0;
}
}
void CMPS(){
if(CMPSStorageSelector == 1 && (millis() / 100 % 2) == 0){
CMPSStorage1 = distance;
Serial.print("Storage #1 : ");
Serial.print(CMPSStorage1);
Serial.println(" ");
CMPSStorageSelector = 2;
}
if(CMPSStorageSelector == 2 && (millis() / 100 % 2) != 0){
CMPSStorage2 = distance;
Serial.print("Storage #2 : ");
Serial.print(CMPSStorage2);
Serial.println(" ");
CMPSStorageSelector = 3;
}
if(CMPSStorageSelector == 3 && (millis() / 100 % 2) == 0){
CMPSStorage3 = distance;
Serial.print("Storage #3 : ");
Serial.print(CMPSStorage3);
Serial.println(" ");
CMPSStorageSelector = 4;
}
if(CMPSStorageSelector == 4 && (millis() / 100 % 2) != 0){
CMPSStorage4 = distance;
Serial.print("Storage #4 : ");
Serial.print(CMPSStorage4);
Serial.println(" ");
CMPSStorageSelector = 1;
}
if((millis() / 200 % 2) == 0 && CMPSSelector == 0){
CMPSDistanceAverage = (CMPSStorage1 - CMPSStorage2) + (CMPSStorage3 - CMPSStorage4);
CMPSDistanceAverage = CMPSDistanceAverage * -1;
Serial.println(CMPSDistanceAverage);
CMPSSelector = 1;
}
if((millis() / 200 % 2) != 0 && CMPSSelector == 1){
CMPSSelector = 0;
}
}
void forwardAction(){
if(distance < 40){
digitalWrite(forward, LOW);
} else if(CMPSDistanceAverage < targetSpeed){
forwardAnalog++;
analogWrite(forward, forwardAnalog);
delayMicroseconds(15000);
} else if(CMPSDistanceAverage > targetSpeed){
forwardAnalog = forwardAnalog - 1;
analogWrite(forward, forwardAnalog);
delayMicroseconds(100000);
}
}