Here is the code! please be gentle! hope this helps anyone else! good luck! and please respond if you have any questions or suggestions
#include <Servo.h> //thank you Michael Margolis et al.
#include <Ultrasonic.h> // thank you ITead studio. Alex, cnobile and Rowan Simms et al.
/*
robot_7 - for Arduino
Copyright (c) 2012 Richard Pesce. All right reserved.
This is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
Richard Pesce (please give me a little credit
PesceTech.com
*/
// -------------------------------------------------------
// 90 = stop |
// forward = right >90 & left < 90 |
// backward = right <90 & left > 90 |
// left = right >90 & left >90 |
// right = right <90 & left <90 |
// i belive +- 20 is the fastest my driving servos turn |
// -------------------------------------------------------
// #define TRIGGER_PIN 8 // no need for variable
// #define ECHO_PIN 12 // no need for variable
Ultrasonic ultrasonic(8, 12); // set pins for HC-SRO4 (sonar) *why cant this be in the "void setup"?
Servo rightservo; // create servo object to control a servo *why cant this be in the "void setup"?
Servo leftservo; // create servo object to control a servo *why cant this be in the "void setup"?
Servo sonarservo; // create servo object to control a servo *why cant this be in the "void setup"?
int stopped = 90; // these servos stop moving at 90 *why cant this be in the "void setup"?
int sonarcenter = 98; // the center point of my sonar servo *why cant this be in the "void setup"?
int sonarleftlimit = 137; // how far for sonar to turn left *why cant this be in the "void setup"? 128
int sonarrightlimit = 53; // how far for sonar to turn right *why cant this be in the "void setup"? 60
int sonardirectionfacing = 0; // where is sonar facing? *why cant this be in the "void setup"?
int sonarcurrentposition = sonarcenter; // variable to store the servo position *why cant this be in the "void setup"?
int sonorleftORright = 0; // 0=left 1=right *why cant this be in the "void setup"?
int StuckInACorner = 0;
void setup()
{
Serial.begin(9600); // to talk to serial
rightservo.attach(10); // attaches the servo on pin 8 to the servo object
leftservo.attach(9); // attaches the servo on pin 9 to the servo object
sonarservo.attach(11); // attaches the servo on pin 9 to the servo object
pinMode(13, OUTPUT); // LED
sonarservo.write(sonarcenter); // center position of my servo
delay(500);
}
void RotateSonar()
{
if (sonorleftORright == 0) {
sonarcurrentposition = sonarcurrentposition +10; // if sonar is heading left rotate sonar left 10 degrees
}
if (sonorleftORright == 1) {
sonarcurrentposition = sonarcurrentposition -10; // if sonar is heading right rotate sonar right 10 degrees
}
if (sonorleftORright == 0 && sonarcurrentposition >= sonarleftlimit) { // if sonar is heading left but hits the left limit, go right
sonorleftORright = 1;
}
if (sonorleftORright == 1 && sonarcurrentposition <= sonarrightlimit) { // if sonar is heading right but hits the right limit, go left
sonorleftORright = 0;
}
sonarservo.write(sonarcurrentposition); // move the sonar servo
delay(60); // so servo can catch up
}
int distance()
{
float inMsec; // some math with sonar
long microsec = ultrasonic.timing(); // some math with sonar
inMsec = ultrasonic.convert(microsec, Ultrasonic::IN); // convert to american (aka imperial)
Serial.print(inMsec); // print distance to serial
Serial.println(" inches away"); // print distance to serial
return inMsec; // return the distance to the object in inches
}
void MoveForward()
{
StuckInACorner = 0;
digitalWrite(13, LOW); // turn off led
rightservo.write(stopped +14); // move forward offset is due to servo performance
leftservo.write (stopped -20); // move forward
}
void EvasiveAction()
{
int objectlocation; // var to store the location of the object
digitalWrite(13, HIGH); // turn on the led
if ( sonarcurrentposition <= sonarcenter ) { // if looking right
Serial.println("Object detected on the right!");
objectlocation = 1; // then the object must be on the right (1 = right)
}
else {
Serial.println("Object detected on the left!");
objectlocation = 0; // otherwise the object must be on the left (0 = left)
}
if (objectlocation == 1) { // if object is on right
rightservo.write(stopped +15); // turn left until the object is not there anymore
leftservo.write (stopped +15); // see above
delay(500);
}
if (objectlocation == 0) { // if object is on left
rightservo.write(stopped -15); // turn right until the object is not there anymore
leftservo.write (stopped -15); // see above
delay(500);
}
}
void PlanAhead()
{
int objectlocation; // var to store the location of the object
digitalWrite(13, HIGH); // turn on the led
if ( sonarcurrentposition <= sonarcenter ) { // if looking right
Serial.println("Object detected on the right!");
objectlocation = 1; // then the object must be on the right (1 = right)
}
else {
Serial.println("Object detected on the left!");
objectlocation = 0; // otherwise the object must be on the left (0 = left)
}
if (objectlocation == 1){
StuckInACorner = StuckInACorner +1;
}
else{
StuckInACorner = StuckInACorner -1; // this is a buffer of sorts, kinda like counting cards, prevents sporadic movement
}
if (StuckInACorner >= 3 ) { // if object is on right
rightservo.write(stopped +14); // turn left until the object is not there anymore
leftservo.write (stopped); // see above
}
if (StuckInACorner <= -3 ) { // if object is on left
rightservo.write(stopped); // turn right until the object is not there anymore
leftservo.write (stopped -20); // see above
}
}
void loop()
{
if ( distance() < 5) // if an oject is less than 5" away
{
EvasiveAction(); // DANGER take evasive action
}
else{ // or
if ( distance() >= 5 and distance() < 14 ) { // if greater than 5" but less than 14"
PlanAhead(); // avoid the object
RotateSonar(); // and rotate sonar
}
else{ // otherwise
MoveForward(); // move forward
RotateSonar(); // and rotate sonar
}
}
}
