Hi there,
I've basically finished with my project to get my Rover 5 to move around autonomously using the parallax ping))) sensor mounted onto a servo, however, in my first test I noticed that one of the four motors on my Rover 5 wasn't working!!!
After many hours trying to troubleshoot this problem I determined that it was somewhere in my code. Finally after a few more hours by breaking it down I figured out that it was actually one line of code. By writing "Servo.attach()", one of my motors would stop working! I though this was weird and tried changing around my motor inputs to different digital I/O ports however, this worsened the problem and now two of my motors are not spinning! I've made sure that if haven't doubled up any thing accidentally and I just can't see any correlation between attaching my servo and running the motor, both of which are connected to separate I/O ports.
Can anyone see anything wrong with my code that I’m somehow overlooking?
#include <Servo.h>
Servo SonarServo;
const int pingPin = 2;
const int SafeDist = 12; //cm
int ServoAngle1[] = {
1200,1500,1750,1500,1200}; //define angles in microseconds for object avoidance sweep
int ServoAngle2[] = {
0,45,90,135,180,135,90,45,0}; //define angles for distance sweep
int MyArray1[9] = {
0}; //distance values saved into array
int mspeed;
int i;
int q;
#define DIR1 12 // motor 1
#define PwM1 11
#define DIR2 13 // motor 2 does not spin when servo is attached
#define PwM2 10
#define DIR3 7 // motor 3 also does not spin when servo is attached
#define PwM3 9
#define DIR4 6 // motor 4
#define PwM4 5
long duration, inches, cm, m;
void setup()
{
Serial.begin(9600);
pinMode( DIR1, OUTPUT);
pinMode( PwM1, OUTPUT);
pinMode( DIR2, OUTPUT);
pinMode( PwM2, OUTPUT);
pinMode( DIR3, OUTPUT);
pinMode( PwM3, OUTPUT);
pinMode( DIR4, OUTPUT);
pinMode( PwM4, OUTPUT); //set all pins attatched to motor driver to output
mspeed = 100; //motor speed
SonarServo.attach(3); //HERE IS THE PROBLEM!!!!!!!!!! ------------------------------------------------------------------
}
void loop(){ //sends rover5 forward whilst performaing object avoidance sweep
Serial.print("motor forward");
forward();
delay(500);
for(int x = 0; x < 4; x++){
SonarServo.writeMicroseconds(ServoAngle1[x]); // write servo in microseconds through angles in array
//Serial.print(ServoAngle1[x]);
// Serial.print(" Degrees-");
delay(500); //allow servo to reach position
SonarScan(); //scan for objects
//Serial.print(cm);
// Serial.print("cm ");
delay(300); //wait a bit more
if (cm < SafeDist && cm > 0){ //min sensor range is 3cm however a bad reading of 0cm is sometimes recorded
Serial.println("stop!");
Stop();
DistanceSweep(); //scan and determine best direction to travel next
}
}
}
void DistanceSweep(){
for(int a = 0; a < 8; a++){
SonarServo.write(ServoAngle2[a]); //Write servo through values saved in array
delay(500); //give servo enough time to reach position
SonarScan(); //scan for distance measurements
delay(400); //wait long enough for sound to return before moving to next position
MyArray1[a] = cm; //save reading to array
}
q = getIndexOfMaximumValue(MyArray1, 9);
Serial.print("max distance is at index --");
Serial.print(q);
Serial.print(" with value of - ");
Serial.println(MyArray1[q]);
Serial.println(" ");
if (q == 0){ //the following determines how far left or right the rover5 must turn depending on the maximum index value
right(); //I havn't tested this properly yet to see if it's accurate or works correctly
delay(3200);
}
else if (q== 1){
right();
delay(1600);
}
else if (q == 2){
loop();
}
else if (q== 3){
left();
delay(1600);
}
else if (q==4){
left();
delay(3200);
}
else if (q == 5){
right();
delay(3200);
}
else if (q== 6){
right();
delay(1600);
}
else if (q == 7){
loop();
}
else if (q== 8){
left();
delay(1600);
}
}
void SonarScan()
{
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
cm = microsecondsToCentimeters(duration);
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}
int getIndexOfMaximumValue(int* array, int size){ //determines the maximum value of an array and its index value
int maxIndex = 0;
int max = array[maxIndex];
for (int i=1; i<size; i++){
if (max<array[i]){
max = array[i];
maxIndex = i;
}
}
return maxIndex;
}
void forward(){
digitalWrite(DIR1, HIGH); //back left
analogWrite(PwM1, mspeed); //back left
digitalWrite(DIR3, HIGH); //front left
analogWrite(PwM3, mspeed); //front left
digitalWrite(DIR2, LOW); //BR
analogWrite(PwM2, mspeed); //BR
digitalWrite(DIR4, HIGH);
analogWrite(PwM4, mspeed);
}
void back(){
digitalWrite(DIR1, LOW); //BL
analogWrite(PwM1, mspeed); //BL
digitalWrite(DIR3, LOW); //FL
analogWrite(PwM3, mspeed); //FL
digitalWrite(DIR2, HIGH); //BR
analogWrite(PwM2,mspeed); //BR
digitalWrite(DIR4, LOW);
analogWrite(PwM4, mspeed);
}
void left(){
digitalWrite(DIR1, HIGH); //BL
analogWrite(PwM1, 100); //BL
digitalWrite(DIR3, HIGH); //FL
analogWrite(PwM3, 100); //FL
digitalWrite(DIR2, HIGH); //BR
analogWrite(PwM2,100); //BR
digitalWrite(DIR4, LOW); //FR For some reason the wires for this motor came reversed
analogWrite(PwM4, 100); //FR
}
void right(){
digitalWrite(DIR1, LOW); //BL
analogWrite(PwM1, 100); //BL
digitalWrite(DIR3, LOW); //FL
analogWrite(PwM3, 100); //FL
digitalWrite(DIR2, LOW); //BR
analogWrite(PwM2,100); //BR
digitalWrite(DIR4, HIGH); //FR For some reason the wires for this motor came reversed
analogWrite(PwM4, 100); //FR
}
void Stop(){
analogWrite(PwM1, 0);
analogWrite(PwM3, 0);
analogWrite(PwM2,0);
analogWrite(PwM4, 0);
}
