Hello all,
I am having trouble coming up with code to make this robot function properly and I have tried many different ways to solve my problem, but to be honest, I don't fully understand what it is doing. I am relatively new at this and understand most of the code that I am using, but not all of it. If anyone could please look at my code and see where I am going wrong or if you have any suggestions, that would be awesome. Explanations are always appreciated. This site doesn't explain things thoroughly so, I have a hard time understanding what is happening within some of the function calls. I am using an Arduino Uno in conjuction with an Arduino motor shield, two DC motors, a servo, and an ultrasonic sensor. I tried to comment my code so it is easy to follow. Thanks for the help, much appreciated.
#include <Servo.h>
Servo roboServo;
const int servoPin = 10; //servo pin
const int trigPin = 7;
const int echoPin = 6;
const int motChanA = 12; //motor channel A pin
const int motChanB = 13; //motor channel B pin
const int brakChanA = 9; //brake channel A pin
const int brakChanB = 8; //brake channel B pin
int fwdFace = 95; //face foward value for servo
unsigned long duration, inches, distance;
int avoidDist=9;
void setup() {
//Setup Channel A
pinMode(motChanA, OUTPUT); //Initiates Motor Channel A pin
pinMode(brakChanA, OUTPUT); //Initiates Brake Channel A pin
//Setup Channel B
pinMode(motChanB, OUTPUT); //Initiates Motor Channel A pin
pinMode(brakChanB, OUTPUT); //Initiates Brake Channel A pin
//Setup Servo
roboServo.attach(servoPin);
//Setup Ultrasonic Sensor
pinMode(trigPin, OUTPUT); //Initiates Sensor Trig pin
pinMode(echoPin, OUTPUT); //Initiates Sensor Echo pin
Serial.begin(9600); // Open serial monitor at 9600 baud to see ping results.
roboServo.write(95);
}
///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
void loop() {
int chosenPath;//variable to store path that provides more distance between the robot and sensed object
int movingDist=timer(); //stores distance sensed in variable
moveForward(); //call to moveForward() function
if(movingDist<avoidDist){ //if the sensed distance is less that 9"
stopRobot();
chosenPath=path(); //path is stored in variable
switch(chosenPath){ //switch case to determine action based on chosenPath
case 1: turnLeft(); break;
case 2: turnRight(); break;
case 3: turnAround(); break;
default: chosenPath=0; movingDist=10; break; //resets variables as to not get stuck in switch case
}
}
}
///////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
//conversion from time to distance for ultrasonic sensor
long microsecondsToInches(long microseconds)
{
// According to Parallax's datasheet for the PING))), there are
// 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
// second). This gives the distance travelled by the ping, outbound
// and return, so we divide by 2 to get the distance of the obstacle.
return microseconds / 74 / 2;
}
//powers both DC motors to move robot forward
void moveForward() {
digitalWrite(motChanA, HIGH); //Establishes forward direction of Channel A
digitalWrite(brakChanA, LOW); //Disengage the Brake for Channel A
digitalWrite(motChanB, HIGH); //Establishes forward direction of Channel B
digitalWrite(brakChanB, LOW); //Disengage the Brake for Channel B
analogWrite(11, 175); //Spins the motor on Channel B at 175 of 225
analogWrite(3, 173); //Spins the motor on Channel A at 171 of 255
}
//apply brakes and stop power supply to DC motors
void stopRobot() {
digitalWrite(brakChanA, HIGH); //Engage brake for Channel A
digitalWrite(brakChanB, HIGH); //Engage brake for Channel B
analogWrite(3, 0); //stops power supply to Channel A motor
analogWrite(11, 0); //stops power supply to Channel B motor
delay(250);
}
//turn the robot around 180 degrees
void turnAround() {
moveBackward();
delay(500);
digitalWrite(motChanA, HIGH);
digitalWrite(motChanB, LOW);
digitalWrite(brakChanB, LOW);
digitalWrite(brakChanA, LOW);
analogWrite(3, 123);
analogWrite(11, 123);
delay(600);
}
//move the robot backwards
void moveBackward() {
digitalWrite(motChanA, LOW); //Establishes backward direction of Channel A
digitalWrite(brakChanA, LOW); //Disengage the Brake for Channel A
digitalWrite(motChanB, LOW); //Establishes backward direction of Channel B
digitalWrite(brakChanB, LOW); //Disengage the Brake for Channel B
analogWrite(11, 175); //Spins the motor on Channel B at 175 of 255
analogWrite(3, 171); //Spins the motor on Channel A at 171 of 255
}
//turn the robot counter clockwise 90 degrees
void turnLeft() {
digitalWrite(brakChanA, LOW);
digitalWrite(brakChanB, LOW);
digitalWrite(motChanA, HIGH);
digitalWrite(motChanB, LOW);
analogWrite(3, 150);
analogWrite(11, 150);
delay(350);
}
//turn the robot clockwise 90 degrees
void turnRight() {
digitalWrite(brakChanA, LOW);
digitalWrite(brakChanB, LOW);
digitalWrite(motChanA, LOW);
digitalWrite(motChanB, HIGH);
analogWrite(3, 150);
analogWrite(11, 150);
delay(350);
}
//orients servo so that the sensor is pointed left of the robot
void lookLeft() {
roboServo.write(180);
delay(1000);
}
//orients servo so that the sensor is pointed right of the robot
void lookRight() {
roboServo.write(0);
delay(1000);
}
//pings ultrasonic sensor, prints distance on serial terminal
int ping1() {
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);
inches = microsecondsToInches(duration);
printDistance();
return inches;
}
//print distance
void printDistance() {
Serial.print(inches);
Serial.print(" in,");
Serial.println();
}
//orients servo to point the sensor forward
void lookForward() {
roboServo.write(fwdFace);
delay(750);
}
//pings sensor every 200 milliseconds and returns distance from ping
unsigned long timer() {
unsigned long time=millis();
if (time%200==0){
distance = ping1();
}
return distance;
}
void stopSensor() {
pinMode(echoPin,OUTPUT);
delayMicroseconds(2);
}
//decides if left or right has more distance between the sensor
//and the object sensed, and defaults to turning around
int path() {
int left, right;
int route=0;
lookLeft();
left=ping1();
delay(100);
lookRight();
right=ping1();
delay(100);
if (left>right)
route=1;
else if(right>left)
route=2;
else
route=3;
return route;
}