I have checked and double
I have checked and double checked the wiring and made a few adjustments to the power, the Nano now gets 9v from an alkaline PP3 to VIn and the Motor Driver (and 2 x DC Motors) gets 10.5v from 7AA Alkaline Batteries.
The servo now operates correctly i.e it scans forward initially and if nothing is in front it tells the motors to go forward but still only 1 x motor is operating. When it detects an obstacle forwad (my hand as its on the bench with the wheels off of the ground) it stops the motor and scans left and right.
The motor that operates is whizzing round at a rapid rate of knots as well (a lot faster than before) so there is plent of power (I guess).
Here is the code, I have commented out the turn left and right bits but the forward and backward should still spin both motors? I have also commented out some of the code that used names for pins and inserted pin assignment names for debugging.
If anyone can help sort this out I would appreciated it, I just want to see it work before I go and spend £30 on rechargables and a charger!!
#include <Servo.h> //Add the Servo library
//Set the constants for the Left and Right Motor pins to avoid using non sensical names (Commented out to check pin outs)
//#define LeftMotorSpeed 10
//#define LeftMotorForward 9
//#define LeftMotorBackward 8
//#define RightMotorSpeed 5
//#define RightMotorForward 7
//#define RightMotorBackward 6
//Set the sensor trigger and echo pins
#define trigPin 12
#define echoPin 13
int enA = 10;
int in1 = 9;
int in2 = 8;
// motor two
int enB = 5;
int in3 = 7;
int in4 = 6;
Servo myservo; //Name the servo
//int pos = 0;
//Set unsigned integer variables to be used
unsigned int duration;
unsigned int distance;
unsigned int FrontDistance;
unsigned int LeftDistance;
unsigned int RightDistance;
unsigned int Time;
unsigned int CollisionCounter;
void setup() //This block happens once at startup
{
Serial.begin (9600); //Included serial communication to show distances in serial monitor for demonstration purposes
//Below set all the pin modes as OUTPUT as they will all be outputting data
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
//pinMode(LeftMotorSpeed, OUTPUT);
//pinMode(LeftMotorForward, OUTPUT);
//pinMode(LeftMotorBackward, OUTPUT);
//pinMode(RightMotorSpeed, OUTPUT);
//pinMode(RightMotorForward, OUTPUT);
//pinMode(RightMotorBackward, OUTPUT);
myservo.attach(4); //Attaches the servo to pin 4
}
void loop()
{
//int rangeToTarget;
//for(pos = 60; pos < 120; pos += 30) {// sends servo from 30 to 120 degrees
//rangeToTarget = getReading();
//myservo.write(pos);
myservo.write(90); //Set the servo to face front
scan(); //Call the Scan() procedure to check for obtacles to the front
FrontDistance = distance; //Whatever distance is returned by the Scan() procedure is set to the vaiable FrontDistance
Serial.println(“Front distance = “); //Print routines for debugging
Serial.print(distance); //Print routines for debugging
Serial.println(” cm”); //Print routines for debugging
if(FrontDistance > 40 || FrontDistance ==0) //If anything in front is less than 40cm away move forward
{
Forward();
}
else //If there is an obstacle less than 40cm away stop and select a safe path
{
CollisionCounter = CollisionCounter +1;
Stop();
Select_Path();
}
}
// for(pos = 120; pos >= 60; pos-= 30) {
// rangeToTarget = getReading();
// myservo.write(pos);
// }
//}
void Forward() //This function tells the robot to go forward
{
Serial.println("");
Serial.println(“Moving forward”);
// turn on motor A
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
// set speed to 200 out of possible range 0~255
analogWrite(enA, 200);
// turn on motor B
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
// set speed to 200 out of possible range 0~255
analogWrite(enB, 200);
delay(2000);
//digitalWrite(LeftMotorForward, HIGH);
//digitalWrite(LeftMotorBackward, LOW);
//analogWrite(LeftMotorSpeed, 200); //Set left motor speed to 200 out of 0-255
//digitalWrite(RightMotorForward, HIGH);
//digitalWrite(RightMotorBackward, LOW);
//analogWrite(RightMotorSpeed, 200); //Set right motor speed to 200 out of 0-255
}
void Backward() //This function tells the robot to move backward
{
Serial.println("");
Serial.println(“Moving backward”);
Serial.println("");
Serial.println(“Moving forward”);
// turn on motor A
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
// set speed to 200 out of possible range 0~255
analogWrite(enA, 200);
// turn on motor B
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
// set speed to 200 out of possible range 0~255
analogWrite(enB, 200);
delay(2000);
//digitalWrite(LeftMotorForward, LOW);
//digitalWrite(LeftMotorBackward, HIGH);
//digitalWrite(RightMotorForward, LOW);
//digitalWrite(RightMotorBackward, HIGH);
}
void Left() //This function tells the robot to turn left
{
Serial.println("");
Serial.println(“Moving left”);
//digitalWrite(LeftMotorForward, LOW);
//digitalWrite(LeftMotorBackward, HIGH);
//digitalWrite(RightMotorForward, HIGH);
//digitalWrite(RightMotorBackward, LOW);
}
void Right() //This function tells the robot to turn right
{
Serial.println("");
Serial.println(“Moving right”);
//digitalWrite(LeftMotorForward, HIGH);
//digitalWrite(LeftMotorBackward, LOW);
//digitalWrite(RightMotorForward, LOW);
//digitalWrite(RightMotorBackward, HIGH);
}
void Stop() //This function tells the robot to stop moving
{
Serial.println("");
Serial.println(“Stopping”);
// now turn off motors
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
//digitalWrite(LeftMotorBackward, LOW);
//digitalWrite(LeftMotorForward, LOW);
//digitalWrite(RightMotorForward, LOW);
//digitalWrite(RightMotorBackward, LOW);
}
void scan() //This function tells the robot to scan for obstacles and set the variable distance
{
Serial.println("");
Serial.println(“Scanning”);
//int duration, distance; //Removed as these varoiables are set as global variable at the start of the program
digitalWrite(trigPin, HIGH);
delayMicroseconds(10); //Scan for 10 milliseconds?
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) /29.1;
Serial.print(distance);
Serial.println(" cm");
delay(500);
//return distance; //Removed as it was causing a problem
}
void Select_Path() //This procedure is called if there is an object directly ahead, it moves the servo left and right, calls the scan procedure and selects a safe path
{
Serial.println(“There’s an obstacle!”); //Obstacle deteced by the initial forward scan
myservo.write(120); //Move the servo to the left
delay(1000); //Wait half a second for the servo to get there, may need adjusting
scan(); //Call the scan procedure
LeftDistance = distance; //Set the variable LeftDistance to the distance returned from the scan
Serial.println("Left distance = ");
Serial.print(distance);
myservo.write(60); //Move the servo to the right
delay(1000); //Wait half a second for the servo to get there, may need adjusting
scan(); //Call the scan procedure
RightDistance = distance; //Set the variable RightDistance to the distance returned from the scan
Serial.println("Right distance = ");
Serial.print(distance);
if(abs(RightDistance - LeftDistance) < 5) //Some sort of safety routine
{
Backward(); //Go to the moveBackward function
delay(200); //Pause the program for 200 milliseconds to let the robot reverse
Right(); //Go to the moveRight function
delay(100); //Pause the program for 200 milliseconds to let the robot turn right
}
else if(RightDistance < LeftDistance) //If the distance to an obstacle on the right is less than that on the left then…
{
Left(); //Go to the moveLeft function
delay(100); //Pause the program for half a second to let the robot turn
}
else if(LeftDistance < RightDistance) //Else if the distance on the left is less than that on the right then…
{
Right(); //Go to the moveRight function
delay(100); //Pause the program for half a second to let the robot turn
}
}
LOL 