I just finished righting the code for my robot, however, I have yet to build it, or obtain the components, therefore I'm unable to test my code. I haven't written code in a while, so I'm afraid that I have kinks and that my code isn't very efficient.
If you feel like looking it over, that would be great! Let me know of any errors or things you feel could be written better. I commented mostly every line so hopefully that will help you understand what I am trying to do.
Overview:
A differential drive robot is made autonomous by taking proximity measurements using an ultrasonic sensor mounted on a servo. When the robot comes within 15cm of an obstacle, the robot brakes, the servo turns left, the sensor takes a reading, then right and takes a reading, the two measurements are compared and makes the appropriate turn. The robot continues this way until it's switched off...
I choose not to use the servo library for a few reasons, that unless you care, I won't specify, but in case you were wondering why, I thought I'd let you know that I chose not to use it.
int motorR_1 = 5; //Right motor ENABLE pin
int motorR_2 = 4; //Right motor PWN (SPEED) pin
int motorL_1 = 3; //Right motor ENABLE pin
int motorL_2 = 2; //Right motor PWN (SPEED) pin
int servoPin1 = 0; // Servo connect to arduino pin 1
int left = 600; // Pulse width of 600µs to turn left
int right = 2400; // Pulse width of 2400µs to turn right
int straight = 1500; // Pulse width of 1500µs to return straight
int pulse = 0; // Pulse to send to servo
long lastPulse = 0; // the time in milliseconds of the last pulse
int refreshTime = 20; // the time needed between pulses
int pingPin = 7; // ultrasonic sensor pin
int leftDist = 0; // distance to left obstacle
int rightDist = 0; // distance to right obstacle
void setup()
{
pinMode(motorR_1, OUTPUT); // declare motor pin as OUTPUT
pinMode(motorR_2, OUTPUT); // declare motor pin as OUTPUT
pinMode(motorL_1, OUTPUT); // declare motor pin as OUTPUT
pinMode(motorL_2, OUTPUT); // declare motor pin as OUTPUT
pinMode(servoPin1, OUTPUT); // declare servo pin as OUTPUT
pulse = straight; // face the servo forward
}
void loop()
{
long duration, cm;
digitalWrite (motorR_1, HIGH); // Move Right motor FORWARD...
digitalWrite (motorR_2, HIGH); // ...at full speed.
digitalWrite (motorL_1, HIGH); // Move Left motor FORWARD...
digitalWrite (motorL_2, HIGH); // ...at full speed.
pulse = straight; // face the sensor forward
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW); // refreash the sensor
delayMicroseconds(2);
digitalWrite(pingPin, HIGH); // send ultrasonic pulse
delayMicroseconds(5);
digitalWrite(pingPin, LOW); // turn sensor off
pinMode(pingPin, INPUT); // allow for sensor to recieve echo
duration = pulseIn(pingPin, HIGH); // wait for sensor to reieve echo
cm = microsecondsToCentimeters(duration); // record distance to cm
if (cm < 15) // if the recorded distance is less than 15cm
{
pinMode(motorL_2, INPUT); // brake left motor
pinMode(motorR_2, INPUT); // brake right motor
delay(500); // wait for the motors to stop completely
digitalWrite(motorR_1, LOW); // disable right motor
digitalWrite(motorL_1, LOW); // diable left motor
digitalWrite(pingPin, LOW); // stop the sensor
pulse = left; // turn the sensor left
delay(100); // wait for the servo to turn to the left
digitalWrite(pingPin, HIGH); // send ultrasonic pulse
delayMicroseconds(5);
digitalWrite(pingPin, LOW); // turn sensor off
pinMode(pingPin, INPUT); // allow for sensor to recieve echo
duration = pulseIn(pingPin, HIGH); // wait for sensor to reieve echo
leftDist = microsecondsToCentimeters(duration); // record distance to leftDist
digitalWrite(pingPin, LOW); // stop the sensor
pulse = right; // turn the sensor right
delay(100); // wait for servo to turn to the right
digitalWrite(pingPin, HIGH); // send ultrasonic pulse
delayMicroseconds(5);
digitalWrite(pingPin, LOW); // turn sensor off
pinMode(pingPin, INPUT); // allow for sensor to recieve echo
duration = pulseIn(pingPin, HIGH); // wait for sensor to recieve echo
rightDist = microsecondsToCentimeters(duration); // record distance to rightDist
}
if (leftDist > rightDist)
{
digitalWrite(motorR_1, HIGH); // turn right motor power on
pinMode(motorR_2, OUTPUT); // release right motor
digitalWrite(motorL_1, HIGH); // turn left motor power on
pinMode(motorL_2, OUTPUT); // release left motor
digitalWrite(motorR_2, HIGH);
digitalWrite(motorL_2, LOW);
delay(1000);
}
if (rightDist > leftDist)
{
digitalWrite(motorR_1, HIGH); // turn right motor power on
pinMode(motorR_2, OUTPUT); // release right motor
digitalWrite(motorL_1, HIGH); // turn left motor power on
pinMode(motorL_2, OUTPUT); // release left motor
digitalWrite(motorR_2, LOW);
digitalWrite(motorL_2, HIGH);
delay(1000);
}
if (millis() - lastPulse >= refreshTime)
{
digitalWrite(servoPin1, HIGH); // turn motor on
delayMicroseconds(pulse); // length of the pulse sets the motor position
digitalWrite(servoPin1, LOW); // turn motor off
lastPulse = millis();
}
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}
/*
Using the motor controller (SN754410 and 2 tri-state switches)
Pin 1 - is used to Enable or Disable the motor controller chip. Whenever the controller is disabled then it is in 'coast' mode. So if we ask the motor to go in a particular clockwise direction then we can use this pin to set the speed that it turns. If it aways high then the motor rotates at full speed. If it is always low then the motor is disconnected and so doesn't turn. By using PWM we can control the speed of the motor from 100% to 0% duty cycle. So pin 1 is the 'throttle' or 'accelerator'.
Pin 2 - Sets the direction of the motor - Forward or Reverse. If this input is high then Input 1 is high, which turns on the transistor so Input 2 is low. This makes the motor turn one way. If this input is low then Input 2 is low, the transistor is off and so input 2 is high, and the motor turns the other way. But the cool thing is that if this pin is disconnected then Input 1 and Input 2 are both in the same state which means that the motor will brake. How do you 'disconnect' a wire that is soldered in - all we do is change the micro-processor pin to be an input pin and the built in resistors make this wire 'disconnected'
*/
