Hello all,
So here I have my pretty basic collision avoidance robot:
#include <Servo.h>
#include<AFMotor.h>
//Declare Servos
Servo scanservo; //Ping Sensor Servo
const int motor1 = 12;
const int motor2 = 9;
const int steer1 = 13;
const int steer2 = 8;
const int a = 800;
const int turntime = 500; //Number of milliseconds to turn when turning
const int pingPin = 4; //Pin that the Ping sensor is attached to.
const int echoPin = 5; //Pin that the Ping sensor is attached to.
const int scanservopin = 7; // Pin number for scan servo
const int distancelimit = 10; //If something gets this many inched from
// the robot it stops and looks for where to go.
//Setup function. Runs once when Arduino is turned on or restarted
void setup()
{
Serial.begin (9600);
scanservo.attach(scanservopin); // Attach the scan servo
pinMode(motor1, OUTPUT);
pinMode(motor2, OUTPUT);
pinMode(steer1, OUTPUT);
pinMode(steer2, OUTPUT);
//Variable inicialization
digitalWrite(motor1,LOW);
digitalWrite(motor2,LOW);
digitalWrite(steer1,LOW);
digitalWrite(steer2,LOW);
delay(2000); // wait two seconds
}
void loop(){
go(); // if nothing is wrong the go forward using go() function below.
int distance = ping(); // us the ping() function to see if anything is ahead.
if (distance < distancelimit){
stopmotors(); // If something is ahead, stop the motors.
char turndirection = scan(); //Decide which direction to turn.
switch (turndirection){
case 'l':
turnleft(turntime);
break;
case 'r':
turnright(turntime);
break;
case 'b':
backward(1500);
turnright(1500);
stopmotors();
break;
case 's':
forward(turntime);
break;
}
}
}
int ping(){
long distance, duration;
//Send Pulse
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
//Read Echo
pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);
distance=(duration/2)*0.0341;
Serial.print("Ping: ");
Serial.println(distance);
}
void go(){
digitalWrite(12, HIGH);
digitalWrite(9, LOW);
digitalWrite(3, HIGH);
delay(600);
}
void turnleft(int t){
digitalWrite (13, HIGH);
digitalWrite (8, LOW);
digitalWrite (11, HIGH);
delay(t);
}
void turnright(int t){
digitalWrite (13, LOW);
digitalWrite (8, LOW);
digitalWrite (11, HIGH);
delay(t);
}
void forward(int t){
digitalWrite(12, HIGH);
digitalWrite(9, LOW);
digitalWrite(3, HIGH);
delay(600);
}
void backward(int t){
digitalWrite (9, LOW);
digitalWrite (12, LOW);
digitalWrite(3, HIGH);
delay(t);
}
void stopmotors(){
digitalWrite (9, HIGH);
digitalWrite (8, HIGH);
digitalWrite (3, HIGH);
digitalWrite (11, HIGH);
delay(600);
}
char scan(){
int leftscanval, centerscanval, rightscanval;
char choice;
//Look left
scanservo.write(30);
delay(300);
leftscanval = ping();
//Look right
scanservo.write(150);
delay(1000);
rightscanval = ping();
//center scan servo
scanservo.write(88);
if (leftscanval>rightscanval && leftscanval>centerscanval){
choice = 'l';
}
else if (rightscanval>leftscanval && rightscanval>centerscanval){
choice = 'r';
}
else if (rightscanval<30 && leftscanval<30 && centerscanval<30){
choice = 'b';
}
else{
choice = 's';
}
Serial.print("Choice: ");
Serial.println(choice);
return choice;
}
Works alright....the problem is, it moves too fast...
So when I try and set the set using " analogWrite(3, 123); in place of digitalWrite(3,HIGH); for example it make beeping noises that do not sound very healthy .
I guess I'm not using PWM correctly. Can anyone see the error?
Please help my poor robot become the best that it can be 