Dear forumreaders,
Since autonomic are awesome, Iwant to make a Rover V2 Autonomous.
http://www.robotshop.com/en/dfrobotshop-rover-v2-autonomous.html
For this product is no samplecode.
Therefore, I am trying to make it run, starting with only the Ultrasonic sensor, panning on a servo. This I am doing to learn to program this thing step by step.
After some lete hours I came up with program included below for this function and it seemed to work at first. But at second glance I noticed the robot starting to sputter and/or making victorylaps instead of keeping to do it's job.
I know the code isn't nice, but since I had no other idea on how to make the motors run, pan the servo and scan all at the same time, I did it like this. I there is another/better way (I doubt there isn't...), I'd like to know!
Can anyone take a look at my program and point out where I went wrong?
Any reaction is welcome.
With kind regards,
Jan
const int pingPin = 4;
int E1 = 6; //M1 Speed Control
int E2 = 5; //M2 Speed Control
int M1 = 8; //M1 Direction Control
int M2 = 7; //M2 Direction Control
#include <Servo.h>
Servo myservo1; // create servo object to control a servo
int pos1 = 90; // variable to store the servo position
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void setup()
{
int i;
for(i=5;i<=8;i++)
pinMode(i, OUTPUT);
Serial.begin(9600);
myservo1.attach(3); // attaches the panservo (right-left) on digital pin 3
}
void loop()
{
ride();
scanleft();
Ultrasonic();
ride();
scanmid();
Ultrasonic();
ride();
scanright();
Ultrasonic();
ride();
scanmid();
Ultrasonic();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void ride(){
int leftspeed = 255; //255 is maximum speed
int rightspeed = 255;
analogWrite (E1,255);
digitalWrite(M1,LOW);
analogWrite (E2,255);
digitalWrite(M2,LOW);
delay(100);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void avoid(){
int leftspeed = 255; //255 is maximum speed
int rightspeed = 255;
analogWrite (E1,200);
digitalWrite(M1,HIGH);
analogWrite (E2,175);
digitalWrite(M2,LOW);
delay(1500);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void scanleft(){
for(pos1 = 90; pos1 > 0; pos1 -= 5) // goes from 90 degrees to 180 degrees
{ // in steps of 5 degree
myservo1.write(pos1); // tell servo to go to position in variable 'pos'
delay(20); // waits 20ms for the servo to reach the position
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void scanmid(){
if (pos1 = 0)
{
for(pos1 = 0; pos1 < 90; pos1 += 5) // goes from 0 degrees to 90 degrees
{ // in steps of 5 degree
myservo1.write(pos1); // tell servo to go to position in variable 'pos'
delay(20);
}
}
else
for(pos1 = 180; pos1 > 90; pos1 -= 5) // goes from 180 degrees to 90 degrees
{ // in steps of 5 degree
myservo1.write(pos1); // tell servo to go to position in variable 'pos'
delay(20);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void scanright(){
for(pos1 = 90; pos1 < 180; pos1 += 5) // goes from 90 degrees to 180 degrees
{ // in steps of 1 degree
myservo1.write(pos1); // tell servo to go to position in variable 'pos'
delay(20);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void Ultrasonic()
{
// 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(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(15);
digitalWrite(pingPin, LOW);
delayMicroseconds(20);
// The same pin is used to read the signal from the PING))): a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
// convert the time into a distance
cm = microsecondsToCentimeters(duration);
Serial.print(cm);
Serial.print("cm");
Serial.println();
delay(100);
if (cm <= 25) {
avoid();
}
}
long microsecondsToCentimeters(long microseconds)
{
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
return microseconds / 29 / 2;
}