I'm trying to convert my parallax Boe Bot into a Arduino Due version. Unfortunately I'm running into something I cannot explain. The servos start to rotate as expected, but after a few seconds the right servo just stops while the other one keeps running. I cannot find a reason for this, even the 'rightServo.read()' tells me it's running at full speed. What could be the cause of this? I've unplugged the USB connection and connected it to a wall outlet but the results are the same.
#include <Servo.h>
Servo pingServo;
Servo leftServo;
Servo rightServo;
int pos = 90; // variable to store the servo position
boolean countUpwards = true;
unsigned long currentTime;
unsigned long loopTime;
const int PIN_PING_SERVO = 8;
const int PIN_LEFT_SERVO = 10;
const int PIN_RIGHT_SERVO = 9;
const int PIN_PING = 31;
const int PIN_PUSH_BUTTON = 53;
int distancesByAngle[] = {};
int distance = 999;
int oldButtonState = 0;
boolean buttonPressed = false;
boolean programActive = false;
void setup()
{
Serial.begin(9600);
pinMode(PIN_LEFT_SERVO, OUTPUT);
pinMode(PIN_RIGHT_SERVO, OUTPUT);
pinMode(PIN_PUSH_BUTTON, INPUT);
pingServo.attach(PIN_PING_SERVO);
leftServo.attach(PIN_LEFT_SERVO);
rightServo.attach(PIN_RIGHT_SERVO);
pingServo.write(pos);
leftServo.write(90);
rightServo.write(90);
}
void loop()
{
int buttonState = digitalRead(PIN_PUSH_BUTTON);
currentTime = millis();
if(currentTime >= (loopTime + 500)){
if(countUpwards){
pos = pos + 20;
if(pos>=170){
countUpwards = false;
}
}else{
pos = pos - 20;
if(pos<=10){
countUpwards = true;
}
}
distance = checkDistance();
//pingServo.write(pos);
distancesByAngle[pos] = distance;
if(distance<30){
moveRight();
}
loopTime = currentTime; // Updates loopTime
}
moveForward();
oldButtonState = buttonState;
}
long checkDistance(){
// establish variables for duration of the ping,
// and the distance result in inches and centimeters:
long duration, cm;
// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
pinMode(PIN_PING, OUTPUT);
digitalWrite(PIN_PING, LOW);
delayMicroseconds(2);
digitalWrite(PIN_PING, HIGH);
delayMicroseconds(5);
digitalWrite(PIN_PING, LOW);
pinMode(PIN_PING, INPUT);
duration = pulseIn(PIN_PING, HIGH);
cm = microsecondsToCentimeters(duration);
//Serial.print(cm);
//Serial.print("cm on pos: ");
//Serial.print(pos);
//Serial.println();
return cm;
}
void fullStop(){
//Serial.println("FULL STOP");
pingServo.write(90);
leftServo.write(90);
rightServo.write(90);
delay(15);
}
void moveForward(){
//Serial.println("FORWARD");
rightServo.write(0);
leftServo.write(180);
delay(15);
Serial.print("rightServo: ");
Serial.print(rightServo.read());
Serial.print(", leftServo: ");
Serial.print(leftServo.read());
Serial.println();
}
void moveRight(){
//Serial.println("RIGHT");
leftServo.write(180);
rightServo.write(90);
delay(15);
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}