My servos are moving in very weird ways, I'm building a robot that rolls forward with leftServo and rightServo and when it is 10 inches away from an object, the myservo, which has the Ping sitting on top turns right and left and determines which length is the furthest from it, it then turns that direction and moves that way. There are couple problems with these continuous rotation servos:
- In the main loop, when the Ping doesn't see anything, they move one way for no certain amount of seconds an then move the other way. They also go at various speeds.
2)When they are supposed to stop when the Ping sees an object, they don't. This is very frustrating.
I slowed the loop way down to see if the drive servos were even listening to their commands and they still went at full speed. They should almost not be moving at the speed I have them going at. If anyone sees anything wrong with my code, please say something!.
PS. The servos ground and power are all looped together if that has anything to do with it.
Thanks!
#include <MegaServo.h>
#define NBR_SERVOS 3
#define FIRST_SERVO_PIN 8
MegaServo myservo;
MegaServo leftServo;
MegaServo rightServo;
int pos = 90;
int pos1 = 0;
int pos2 = 0;
int pingPin = 7;
const int ledPin = 13;
int rightDist;
int leftDist;
int launchPing()
{
pinMode(pingPin, OUTPUT); //|||
digitalWrite(pingPin, LOW); //|||
delayMicroseconds(2); //giving short burst telling Ping to Send distance
digitalWrite(pingPin, HIGH);//|||
delayMicroseconds(5); //|||
digitalWrite(pingPin, LOW); //|||
pinMode(pingPin, INPUT); // make Ping an input
return pulseIn(pingPin,HIGH);
}
void setup()
{
Serial.begin(9600);
pinMode (ledPin, OUTPUT);
myservo.attach(9);
leftServo.attach(8);
rightServo.attach(10);
leftServo.write(0);
rightServo.write(0);
delay(1000);
}
void loop()
{
pos1+=1;
pos2-=1;
leftServo.write(pos1);
rightServo.write(pos2);
long duration, inches, cm;
duration = launchPing();
inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
Serial.print(inches);
Serial.print("in, ");
Serial.print(cm);
Serial.print("cm");
Serial.println();
if (inches >= 10) {
digitalWrite (ledPin, LOW);
pos=94;
myservo.write(pos);
}
else {
digitalWrite (ledPin, HIGH);
delay(250);
//start of left scan
pos = 180;
myservo.write(pos);
delay(1000);
duration = launchPing();
inches = microsecondsToInches(duration); //converting time into distance
cm = microsecondsToCentimeters(duration);//converting time into distance
rightDist = inches;
Serial.print("right duration = ");//|||
Serial.print(duration);//sends serial data to computer
Serial.println(); //|||
// end of leftt scan
//beginning of right scan
pos = 15;
myservo.write(pos);
delay(1000);
duration = launchPing();
inches = microsecondsToInches(duration); //converting time into distance
cm = microsecondsToCentimeters(duration);//converting time into distance
leftDist = inches;
Serial.print("left duration = ");//|||
Serial.print(duration);//sends serial data to computer
Serial.println(); //|||
//end of right scan
//beginning of mathematics to determine which direction gives robot
//the optimal distance to travel before another object
if (leftDist < rightDist) {
Serial.print("right side is longer");
pos1-180;
leftServo.write(pos1);
pos=94;
myservo.write(pos);
delay(2000);//wait for servo
}
else {
Serial.print("left side is longer");
pos2+180;
rightServo.write(pos2);
pos=94;
myservo.write(pos);
delay(2000);//wait for servo
}
}
delay(1000);
}
long microsecondsToInches(long microseconds)
{
return microseconds / 74/ 2;
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}