/*
* Braitenberg Vehicle
*
* A device with two light sensors, each controlling one of two motors.
* When the left sensor sees more light, it drives the right motor faster
* so the vehicle turns to the left.
* When the right sensor sees more light, it drives the left motor faster
* so the vehicle turns to the right.
*
* Two DC motors are controlled by a single H-bridge,
* with the motor control inputs of the H-bridge
* connected to analog output (PWM) pins
* and the analog inputs for speed scaled to a range of 0-255.
*
* The brighter the light, the faster the device moves toward the light.
*
*
* Created by Corinna Sherman
* September 30, 2010
*/

int LEFT = 0; // index for lightSensorPins array corresponding to left sensor
int RIGHT = 1; // index for lightSensorPins array corresponding to right sensor

int CURRENT = 0;// index for lightSensorValues array corresponding to a given sensor's current value
int MIN = 1; // index for lightSensorValues array corresponding to a given sensor's minimum value
int MAX = 2; // index for lightSensorValues array corresponding to a given sensor's maximum value

int enableSwitchPin = 8; // enable switch input pin to turn robot ON/OFF

int lightSensorPins[] = {0, 1}; // analog input pins for photocells controlling left and right motors

// The outer array in the multidimensional array lightSensorValues
// holds sensor values for {leftSensorValues, rightSensorValues};
// Each inner array element is {currentSensorValue, minimumSensorValue, maximumSensorValue}
// for a given sensor.
int lightSensorValues[2][3] = {
{ 0,0,0 },
{ 0,0,0 }
};

int motorLSpeed = 0; // variable to hold the left motor's speed as a PWM in a range of 0-255
int motorRSpeed = 0; // variable to hold the right motor's speed as a PWM in a range of 0-255

int enablePinL = 9; // H-bridge enable pin 1 (1,2EN) to enable left motor
int enablePinR = 10; // H-bridge enable pin 2 (3,4EN) to enable right motor

int motorLPin1 = 3; // left motor logic pin 1 on H-bridge (1A) with PWM
int motorLPin2 = 5; // left motor logic pin 2 on H-bridge (2A) with PWM
int motorRPin1 = 6; // right motor logic pin 1 on H-bridge (4A) with PWM
int motorRPin2 = 11; // right motor logic pin 2 on H-bridge (3A) with PWM

void setup() {

// Initialize serial communication.
Serial.begin(9600);

// Set enable switch pin as input pin.
pinMode(enableSwitchPin, INPUT);

// Set other pins as output pins.
pinMode(enablePinL, OUTPUT);
pinMode(enablePinR, OUTPUT);
pinMode(motorLPin1, OUTPUT);
pinMode(motorLPin2, OUTPUT);
pinMode(motorRPin1, OUTPUT);
pinMode(motorRPin2, OUTPUT);

// Disable motor by default until the speed is calculated in the loop function.
disable();
}

void loop() {

// Get the current, minimum, and maximum light levels
// in the environment from the light sensors.
calibrate();

// Calculate the device's speed for analog output (pulse width modulation ranging from 0 to 255)
// using the current light level seen by the light sensor
// as analog input (ranging from min light sensed to max light sensed).
setMotorSpeed();

// Go forward by default.
goForward();

// If the switch is on, the motor turns in one direction.
if (digitalRead(enableSwitchPin) == HIGH) {
// Set enablePins to HIGH to turn on both motors.
//Serial.println("go");
enable();
}
else {
//Serial.println("stop");
disable();
}

}

// Calibrate the light sensors on the device.
void calibrate() {
calibrateLightSensor(LEFT);
calibrateLightSensor(RIGHT);
}

// Calibrate the light sensor with the given pin index into the lightSensorPins array.
void calibrateLightSensor(int pinIndex) {

// Read current value from the light sensor.
lightSensorValues[pinIndex][CURRENT] = analogRead(lightSensorPins[pinIndex]);

// For light sensor calibration:
// Set the maximum light sensor value if the current reading exceeds the previous maximum.
// Else, set the minimum light sensor value if the current reading is less than the previous minimum.
if (lightSensorValues[pinIndex][CURRENT] > lightSensorValues[pinIndex][MAX]) {
lightSensorValues[pinIndex][MAX] = lightSensorValues[pinIndex][CURRENT];
}
else if (lightSensorValues[pinIndex][CURRENT] < lightSensorValues[pinIndex][MIN]) {
lightSensorValues[pinIndex][MIN] = lightSensorValues[pinIndex][CURRENT];
}

}

void enable() {
// Set enable pins to HIGH to turn on both motors.
digitalWrite(enablePinL, HIGH);
digitalWrite(enablePinR, HIGH);
}

void disable() {
// Set enable pins to LOW to turn off both motors.
digitalWrite(enablePinL, LOW);
digitalWrite(enablePinR, LOW);
}

void goForward() {
// Set left motor's direction
analogWrite(motorLPin1, 0); // Set H-bridge 1A pin to low
analogWrite(motorLPin2, motorLSpeed); // Set H-bridge 2A pin to a speed based on light level

// Set right motor's direction
analogWrite(motorRPin1, 0); // Set H-bridge 4A pin to low
analogWrite(motorRPin2, motorRSpeed); // Set H-bridge 3A pin to a speed based on light level
}

void goBackward() {
// Set left motor's direction
analogWrite(motorLPin1, motorLSpeed); // Set H-bridge 1A pin to a speed based on light level
analogWrite(motorLPin2, 0); // Set H-bridge 2A pin to low

// Set right motor's direction
analogWrite(motorRPin1, motorRSpeed); // Set H-bridge 4A pin to a speed based on light level
analogWrite(motorRPin2, 0); // Set H-bridge 3A pin to low
}

// Set the speed variables for both left and right motors
// based on their corresponding light sensor readings
void setMotorSpeed() {

// Read current value from each light sensor.
lightSensorValues[LEFT][CURRENT] = analogRead(lightSensorPins[LEFT]);
lightSensorValues[RIGHT][CURRENT] = analogRead(lightSensorPins[RIGHT]);

// Calculate the motor speed for analog output (pulse width modulation ranging from 0 to 255)
// using the current light level as seen by the light sensor
// as analog input (ranging from min light sensed to max light sensed).
motorLSpeed = constrain(map(lightSensorValues[LEFT][CURRENT], lightSensorValues[LEFT][MIN], lightSensorValues[LEFT][MAX], 0, 255), 0, 255);
motorRSpeed = constrain(map(lightSensorValues[RIGHT][CURRENT], lightSensorValues[RIGHT][MIN], lightSensorValues[RIGHT][MAX], 0, 255), 0, 255);

//Serial.print("left sensor value = ");
//Serial.print(lightSensorValues[LEFT][CURRENT]);
//Serial.print(", right sensor value = ");
//Serial.println(lightSensorValues[RIGHT][CURRENT]);
//Serial.print("left motor speed = ");
//Serial.print(motorLSpeed);
//Serial.print(", right motor speed = ");
//Serial.println(motorRSpeed);
}