Pages: 1 [2]   Go Down
Author Topic: Boe-bot for arduino roaming with ping on servo turret  (Read 2680 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 1
Posts: 47
I'm glad I bought an Arduino Leonardo
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
/*
 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 constants
const 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 library
Servo servoLeft;                             // Declare left, right and Ping))) servos
Servo servoRight;
Servo PingServo;

// remember to look around after a 45 degree turn
int DoLook = 0;

// Define distance variables
int 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 number
Number   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);
}
Logged

Pages: 1 [2]   Go Up
Jump to: