// define the pins used by the Robot Shield
int lock = 5; // lock is disable input - program will not run if lock is in place
// NOTE: DO NOT use pins 0 or 1 for the lock as this will stop
// you being able to program your robot!
int speedA = 6; // pin 6 sets the speed of motor A (this is a PWM output)
int speedB = 9; // pin 9 sets the speed of motor B (this is a PWM output)
int dirA = 8; // pin 8 sets the direction of motor A
int dirB = 7; // pin 7 sets the direction of motor B
int lightleft;
int lightright;
int photoresistorleft = 5; // define the pin for left photoresistor
int photoresistorright = 4; //define the pin for right photoresistor
int IRpinLeft = 2; // analog pin for reading the IR sensor
int IRpinRight = 3; // analog pin for reading the IR sensor
int motorPin1 = 3; // pin 2 on L293D
int motorPin2 = 5; // pin 7 on L293D
int enablePin = 1; // pin 1 on L293D
// define the direction of motor rotation - this allows you change the direction without effecting the hardware
int fwdA = LOW; // this sketch assumes that motor A is the right-hand motor of your robot (looking from the back of your robot)
int revA = HIGH; // (note this should ALWAYS be opposite the fwdA)
int fwdB = LOW; //
int revB = HIGH; // (note this should ALWAYS be opposite the fwdB)
// define variables used
int dist;
int angle;
int vel;
void setup() { // sets up the pinModes for the pins we are using
pinMode (dirA, OUTPUT);
pinMode (dirB, OUTPUT);
pinMode (speedA, OUTPUT);
pinMode (speedB, OUTPUT);
pinMode(motorPin1, OUTPUT);
pinMode(motorPin2, OUTPUT);
pinMode(enablePin, OUTPUT);
digitalWrite(enablePin, HIGH);
}
void stop(int dist, int vel) { // stop: force both motor speeds to 0 (low)
digitalWrite (speedA, LOW);
digitalWrite (speedB, LOW);
}
void forward(int dist, int vel) { // forward: both motors set to forward direction
digitalWrite (dirA, fwdA);
digitalWrite (dirB, fwdB);
analogWrite (speedA, vel); // both motors set to same speed
analogWrite (speedB, vel);
delay (dist); // wait for a while (dist in mSeconds)
}
void reverse(int dist, int vel) { // reverse: both motors set to reverse direction
digitalWrite (dirA, revA);
digitalWrite (dirB, revB);
analogWrite (speedA, vel); // both motors set to same speed
analogWrite (speedB, vel);
delay (dist); // wait for a while (dist in mSeconds)
}
void rot_cw (int angle, int vel) { // rotate clock-wise: right-hand motor reversed, left-hand motor forward
digitalWrite (dirA, revA);
digitalWrite (dirB, fwdB);
analogWrite (speedA, vel); // both motors set to same speed
analogWrite (speedB, vel);
delay (angle); // wait for a while (angle in mSeconds)
}
void rot_ccw (int angle, int vel) { // rotate counter-clock-wise: right-hand motor forward, left-hand motor reversed
digitalWrite (dirA, fwdA);
digitalWrite (dirB, revB);
analogWrite (speedA, vel); // both motors set to same speed
analogWrite (speedB, vel);
delay (angle); // wait for a while (angle in mSeconds)
}
void turn_right (int angle, int vel) { // turn right: right-hand motor stopped, left-hand motor forward
digitalWrite (dirA, revA);
digitalWrite (dirB, fwdB);
digitalWrite (speedA, LOW); // right-hand motor speed set to zero
analogWrite (speedB, vel);
delay (angle); // wait for a while (angle in mSeconds)
}
void turn_left (int angle, int vel) { // turn left: left-hand motor stopped, right-hand motor forward
digitalWrite (dirA, fwdA);
digitalWrite (dirB, revB);
analogWrite (speedA, vel);
digitalWrite (speedB, LOW); // left-hand motor speed set to zero
delay (angle); // wait for a while (angle in mSeconds)
}
void motor3reverse(int dist) {
digitalWrite (motorPin1, HIGH);
digitalWrite (motorPin2, LOW);
delay (dist);
motor3stop();
}
void motor3forward(int dist) {
digitalWrite (motorPin1, LOW);
digitalWrite (motorPin2, HIGH);
delay (dist);
motor3stop();
}
void motor3stop() {
digitalWrite (motorPin1, LOW);
digitalWrite (motorPin2, LOW);
}
void loop() {
lightleft = analogRead(photoresistorleft);
lightright = analogRead(photoresistorright);
float voltsLeft = analogRead(IRpinLeft)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distanceLeft = 65*pow(voltsLeft, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
float voltsRight = analogRead(IRpinRight)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distanceRight = 65*pow(voltsRight, -1.10);
motor3stop();
if (lightleft - lightright > 30)
{
rot_ccw (500, 75); // rotate clock-wise for 500 mS at speed 75
}
else if (lightright - lightleft > 30)
{
rot_cw (500, 75); // rotate clock-wise for 500 mS at speed 75
}
else if (lightright && lightleft > 600)
{
stop(5, 0);
motor3forward (100);
delay(200);
turn_left (300, 75);
delay(200);
motor3reverse (100);
delay (500);
}
else if (distanceLeft < 35)
{
reverse(500, 75); // reverse for 500 at 75
rot_cw (500,75); // rotate clockwise for 500 at 75
}
else if (distanceRight < 35)
{
reverse (500,75); // reverse for 500 at 75
rot_ccw (500,75); // rotate countercloackwise for 500 at 75
}
else if (distanceLeft < 35 and distanceRight < 35)
{
reverse (500, 75); // reverse for one second at speed 75
}
else
{
forward (200, 75); // move forward for half a second (2000 ms) at speed 75 (75/255ths of full speed)
}
}