Hi, I'm trying to make an obstacle avoiding robot. I'm using micro servos as the motors for the wheels. I'm only using two at the moment and there's a mini caster as the turning wheel. I got the code online and I've tested it so I know for sure it works.
The thing is, Id like to use four micro servos for the wheels. I've tried messing around with the code, but I can't get it to work (I'm very new at this). I'm using an ardunio uno. Here's the code.
#include <Servo.h> //include Servo library
const int RForward = 0;
const int RBackward = 180;
const int LForward = RBackward;
const int LBackward = RForward;
const int RNeutral = 90;
const int LNeutral = 90; //constants for motor speed
const int pingPin = 7;
const int dangerThresh = 10; //threshold for obstacles (in cm)
int leftDistance, rightDistance; //distances on either side
Servo panMotor;
Servo leftMotor;
Servo rightMotor; //declare motors
long duration; //time it takes to recieve PING))) signal
void setup()
{
rightMotor.attach(11);
leftMotor.attach(10);
panMotor.attach(6); //attach motors to proper pins
panMotor.write(90); //set PING))) pan to center
}
void loop()
{
int distanceFwd = ping();
if (distanceFwd>dangerThresh) //if path is clear
{
leftMotor.write(LForward);
rightMotor.write(RForward); //move forward
delay(100);
}
else //if path is blocked
{
leftMotor.write(LNeutral);
rightMotor.write(RNeutral);
panMotor.write(0);
delay(500);
rightDistance = ping(); //scan to the right
delay(500);
panMotor.write(180);
delay(700);
leftDistance = ping(); //scan to the left
delay(500);
panMotor.write(90); //return to center
delay(100);
compareDistance();
}
}
void compareDistance()
{
if (leftDistance>rightDistance) //if left is less obstructed
{
leftMotor.write(LBackward);
rightMotor.write(RForward); //turn left
delay(500);
}
else if (rightDistance>leftDistance) //if right is less obstructed
{
leftMotor.write(LForward);
rightMotor.write(RBackward); //turn right
delay(500);
}
else //if they are equally obstructed
{
leftMotor.write(LForward);
rightMotor.write(RBackward); //turn 180 degrees
delay(1000);
}
}
long ping()
{
// Send out PING))) signal pulse
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
//Get duration it takes to receive echo
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
//Convert duration into distance
return duration / 29 / 2;
}
RFoward and LForward are the two servos that already work as the wheels and the Servo pan motor is the servor that turns the ping sensor.
So yeah, I'm trying to add two more servo motors, two for the right and two for the left. I was going to use pins 5 and 3 for the two additional servos. I might not have any idea what I'm doing, maybe I need a motor shield, I don't know, I just thought that using 5 and 3 might work.
So, can anyone tell me how I can do this or adjust the code so that it works.