Go Down

Topic: 1st semi intelligent robot, suggestions please! (Read 2 times) previous topic - next topic

pesce


pesce

#6
Feb 26, 2012, 08:51 am Last Edit: Feb 26, 2012, 04:03 pm by pesce Reason: 1
Here is the code! please be gentle!  hope this helps anyone else!  good luck! and please respond if you have any questions or suggestions

Code: [Select]
#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
   }
 }
}

pesce

#7
Feb 26, 2012, 09:35 am Last Edit: Feb 26, 2012, 09:44 am by pesce Reason: 1
Working code for 2 servo drive motors and one sonar servo motor

I'm drunk. So please forgive lame posts.


pesce

Well I've made the switch... I'm now powering everything directly from the battery. Even though I get more speed from my servos. I still feel like I may break something. 7.2V straight to the servos?   And also 7.2V straight to the HC-SR04 ultrasonic sensor?   I feel like something is wrong.

Please help!

Go Up