Go Down

### Topic: Boe-bot for arduino roaming with ping on servo turret (Read 6717 times)previous topic - next topic

#### S_Flex

#15
##### Feb 13, 2013, 03:28 pm
as an alternative I been working on this sketch so i at least have some thing to work with.

The only lesson I have learned from this bot is to beware of what shield you buy for arduino. Unfortunately I do not own the boe-bot to return it and was helping a friend understand how it works and why it has this flaw.

Maybe time will tell if parallax is trying  to just make a buck or rely wants to support the products they sell.

I hope this code helps others.
Code: [Select]
`/* Current Version: 2.0 (2/13/2013) Not tested BOE Shield code from "Robotics with the BOE Bot" Roaming With Whiskers source, modified to use a PING))) sensor on the PING))) servo bracket. Using code from the Ping))) example sketch. http://thoughtfix.com/blog/2012/3/25/arduino-boe-shield-ping-and-a-servo.html Ping view angles, 125 or maybe 115 degree's, 45 can be wrong too maybe 35 will work                 centerDist  leftDist2=125      90       45=rightDist2              ^       ^       ^               \      |      / leftDist=160 <---------------> 20=rightDist Flex - changes (2/13/2013) v2.0 - added 45 degree turns - made the ping() function move the turret to the angle with delay then return the ping - Removed Look forward function since ping() does the work now - LookAround() has 2 new angles so 45 degree turns can be done and added the new ping method - Added 45 degree turn logic to loop() before the 90 degree turning logic so small turns can be done first - Fixed the code so it doesnt go forward if the safe distance is equal to the center distance. - Removed some variables that where not used - Removed microsecondsToInches() function and moved the math in ping. - Removed forward(),turnLeft(),turnRight() and backward() functions - Added function move_or_turn(time, task) this will do all the movement. - Added LengthConversion variable with notes to set length to inches or centimeters - Changed logic so the code does not favor one direction then another and turns to the true max distance Flex: Found code, did some small changes and am starting the version number at v1.0*/// Servo calibration values, Not used, can be if needed.const int max_LeftSpeed = 100;const int max_RightSpeed = 200;const int servoLeftPin = 13;                 // I/O Pin constantsconst int servoRightPin = 12;const int servoTurretPin = 11;const int pingPin = 10;const int piezoPin = 4;const int LengthConversion = 74;             // Length: 74 = Inches or 29 = Centimeters const int minSafeDist = 11;                  // Minimum safe distance of object #include <Servo.h>                           // Include servo libraryServo servoLeft;                             // Declare left, right and Ping))) servos Servo servoRight; Servo PingServo;// remember to look around after a 45 degree turnint DoLook = 0;// Define distance variablesint centerDist, leftDist, leftDist2, rightDist, rightDist2;// Define variables for Ping)))long duration, moveTime;                       void setup() {                               // Built-in initialization block      tone(piezoPin, 3000, 1000);                // Play tone for 1 second   delay(1000);                               // Delay to finish tone  servoLeft.attach(servoLeftPin);            // Attach left signal to pin 13    servoRight.attach(servoRightPin);          // Attach right signal to pin 12   PingServo.attach(servoTurretPin);          // Attach turret signal to pin 11}  void loop() {   if (DoLook == 0) {    // Look Ahead get center distance    centerDist = ping(90, 175);  }  else {    DoLook = 0; // set for forward    // Look around to see if last turn was enough    LookAround();    // set to 1 to skip moving forward so it can turn again    // To-Do: test if this logic works    if (rightDist2 < minSafeDist       || centerDist < minSafeDist       || leftDist2 < minSafeDist) DoLook = 1;    }    /* If the inches in front of an object is greater than    the minimum safe distance (11 inches), move forward */  if(centerDist > minSafeDist && DoLook == 0) {    move_or_turn(121, 1);    delay(110); // Wait 0.11 seconds   }  else {    // Stop moving    servoLeft.writeMicroseconds(1500);    servoRight.writeMicroseconds(1500);    // Check your surroundings for best route    if (DoLook == 0) LookAround(); // scan    else DoLook = 0; // scan was done    // Default move time    moveTime = 250;    // Check witch direction is the most so we dont faivor one direction then another    if (rightDist > minSafeDist && rightDist > leftDist && rightDist2 > leftDist2) {      // Right side has more distance       if (rightDist2 > centerDist && rightDist2 > minSafeDist) {        DoLook = 1; // remember to look around next loop        moveTime = 125;        move_or_turn(moveTime, 3); // do a 45 turn right      }      else {        // do a 90 Turn Right          move_or_turn(250, 3);       }   // maybe need to put backward here also and ask a question for 90 degree turns       }    else if (leftDist > minSafeDist && leftDist > rightDist && leftDist2 > rightDist2) {      // Left side has more distance      if (leftDist2 > centerDist && leftDist2 > minSafeDist) {              DoLook = 1; // remember to look around next loop         moveTime = 125;        move_or_turn(moveTime, 2); // do a 45 turn left      }      else {        // do a 90 Turn Right          move_or_turn(250, 3);       }     // maybe need to put backward here also and ask a question for 90 degree turns      }    else {      // Go Backward      move_or_turn(250, 4);     }     delay(moveTime);  }}/*Ping function moves turret to angle, pings then converts time to length and returns the number Distance_in_length = ping(angle, delayMS); */int ping(int angle, int delayMS) {  PingServo.write(angle);      // move turret to angle   delay(delayMS);              // wait time for turret  pinMode(pingPin, OUTPUT);    // Set pin to OUTPUT  digitalWrite(pingPin, LOW);  // Send a low pulse  delayMicroseconds(2);        // wait for two microseconds  digitalWrite(pingPin, HIGH); // Send a high pulse  delayMicroseconds(5);        // wait for 5 micro seconds  digitalWrite(pingPin, LOW);  // send a low pulse  pinMode(pingPin,INPUT);      // switch the Pingpin to input   //listen for echo with 25000 time out  duration = pulseIn(pingPin, HIGH, 25000);      // converts time to a distance then return the length  return duration / LengthConversion / 2;}/*Look around at 5 angles */void LookAround() {  //get the two right distances  rightDist = ping(20, 320);  rightDist2 = ping(45, 275); // TO-DO: check if 35 is better then 45  // get the two left distances  leftDist2 = ping(125, 320); // TO-DO: check if 115 is better then 125  leftDist = ping(160, 275);   // move turret back to center  centerDist = ping(90, 320); }/*move and turn function time - Time to wait for movement to complete task - move or turn command numberNumber   Task 1    =  Forward 2    =  Turn left 3    =  Turn right 4    =  Backwards */void move_or_turn(int mttime, int mttask) {  if (mttask == 1 || mttask == 3) servoLeft.writeMicroseconds(1700);  else if (mttask == 2 || mttask == 4) servoLeft.writeMicroseconds(1300);  if (mttask == 3 || mttask == 4) servoRight.writeMicroseconds(1700);  else if (mttask == 1 || mttask == 2) servoRight.writeMicroseconds(1300);  if (mttime <= 0) mttime = 1;   delay(mttime);}`

Go Up

Please enter a valid email to subscribe