Go Down

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

S_Flex

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 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);
}

Go Up
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy