//http://www.akeric.com/blog/?

p=1590 (Link Utama)

/*
2011-10-03
robot4wd_06.pde
Eric Pavey - www.akeric.com
This software is released to the wild, free to be used and modified. If you us
e it,
I just ask for some credit ;)
Program to drive the 4wd robot chasis using an arduino paired up with a ping)))
sensor
and a servo for collision detection.
Hardware:
Arduino Duemilanove (but I'm sure any others will work)
http://www.arduino.cc/en/Main/ArduinoBoardDuemilanove
Arduino is powered off a 5xAA producing 7.5v, sitting in the belly of the chass
is.
Main chassis is the "4wd mobile platform" from DF Robot
http://www.dfrobot.com/index.php?route=product/product&path=37&product_id=97
http://www.makershed.com/ProductDetails.asp?ProductCode=MKSEEED8
http://www.rugcommunity.org/page/dfrobot-4wd-arduino-mobile
Motor control using the Adafruit motorshield:
http://www.ladyada.net/make/mshield/index.html
There doesn't seem to be API docs, but there is this header file:
https://github.com/adafruit/Adafruit-Motor-Shield-library/blob/master/AFMotor.h
Motor shield is powered off 4xC batteries (6v).
Paralax ping))) :
http://www.parallax.com/tabid/768/ProductID/92/Default.aspx
http://www.arduino.cc/en/Tutorial/Ping
Notes: I had to rewire the ribon connecting the ping to the motorshield: The
ping's ribon is ground\+5v\signal, but the motorshield's input pins order are +
5v\ground\signal.
It works from 2cm to 3m.
Paralax (Fubata) "standard servo":
This is found in the "PING))) Mounting Bracket Kit":
http://www.parallax.com/Store/Robots/RoboticAccessories/tabid/145/CategoryID/22
/List/0/SortField/0/Level/a/ProductID/248/Default.aspx
This is plugged into the 'servo2' port on the motorshield.
It uses the servo library:
http://arduino.cc/en/Reference/Servo
Much of the ping code at the bottom is taken straight from the Arduino examples
.
*/
//-------------------------------------------------------------------------// Library include stuff:
#include <AFMotor.h> // needed for the motorshield.
#include <Servo.h> // motorshield needs this for running servos.
//-------------------------------------------------------------------------// globals:
// ping))) stuff:

int g_pingPin = 19; // this is actually analog pin 5, pretending to be digital,

on the motorshield.
long g_collideDist = 50; // in cm, how far before collision mode kicks in?
long g_cm = 300; // the current ping reading, just ana initial default.
boolean g_checkLeft = false; // used for collision detection
boolean g_checkRight = false; // used for collision detection
boolean g_checkCenter = false; // used for collision detection
int g_leftDist = 0; // used for collision detection
int g_rightDist = 0; // used for collision detection
// motor stuff:
AF_DCMotor g_motor1(1, MOTOR12_8KHZ); //
AF_DCMotor g_motor2(2, MOTOR12_8KHZ); //
AF_DCMotor g_motor3(3, MOTOR12_8KHZ); //
AF_DCMotor g_motor4(4, MOTOR12_8KHZ); //
byte g_motorSpeed = 255; // max is 255

back left
back right
front right
front left

// Mode (state) stuff:

byte g_mode = 1; // 1 = drive, 2 = proximity check, 3 = turning
byte g_driveDir = -1; // 0 = stop, 1 = straight, 2 = right, 3 = left
// servo stuff:
Servo g_servo2;
int g_servo2pin = 9; // servo2 uses digital pin 9, even though the docs say pin
10...
int g_servoCenter = 98; // in degrees. 98 is straight ahead based on how I moun
ted the servo (it must be
// a little inaccurate). full left is 180, full right is 0.
// Servo is flipped upside down, fyi
int g_servoRight = 135; // how far right? Max is 180, full right.
int g_servoLeft = 45; // how far left? Min is 0, full left.
// time stuff
unsigned long g_time; // global time tracker
unsigned long g_ptime; // global previous time tracker
unsigned long g_elapsed = 0; // helps keeping track of framerate.
unsigned long g_interval = 33; // in ms (about 30fps). This slows down the proc
essing allowing the
// multiple systems to play nicely together. Make this number smaller and i
t won't have enough
// time for each state to run properly: The servo won't do a full sweep whe
n scanning.
unsigned long g_turnlength = 750; // in ms, how long to turn for. Since we don'
t know how far
// it turns, we just guess on how long it should. The type of terrain turin
g on has a great
// effect on what this should be. This value works well for my hardwood flo
ors.
unsigned long g_turnUntil; // used to track how long to turn.
//-------------------------------------------------------------------------// setup \ loop :
void setup() {
// Attach & center the servo:
g_servo2.attach(g_servo2pin);
g_servo2.write(g_servoCenter); // center it by default
// Setup our framerate stuff
g_time = millis();
g_ptime = g_time;

}
void loop() {
// Main program entry point.
// Need to keep the aiming and the piging at a fixed rate, or the closer the o
bject gets
// to the ping the faster it will aim, and do bad stuff.
g_ptime = g_time;
g_time = millis();
g_elapsed += g_time - g_ptime;
// Wait for the proper amount of time to pass before we do anything:
if(g_elapsed < g_interval){
return;
}
else{
g_elapsed = 0;
}
// Do stuff, based on the current mode:
if(g_mode == 1){
g_cm = ping();
// sometimes the sensor returns a 0 value that can foul up things :(
if(g_cm > g_collideDist || g_cm == 0){
// If there is nothing in our way, drive!
if(g_driveDir != 1){
g_driveDir = 1;
move(g_driveDir);
}
}
else{
// if we're too close to something, stop the motors and switch to 'proximi
ty check' mode:
if(g_driveDir != 0){
g_driveDir = 0;
move(g_driveDir);
}
// initialize the proximity check stuff:
g_checkLeft = false;
g_checkRight = false;
g_checkCenter = false;
g_leftDist = 0;
g_rightDist = 0;
// switch to mode 2:
g_mode = 2;
}
}
else if(g_mode == 2){
// Do proximity check!
boolean checked = proximityCheck();
if(checked){
// Define which direction to turn.
// If the right distance is greater than the left distance, turn that dire
ction,
// otherwise go the other way:
if(g_rightDist >= g_leftDist){
g_driveDir = 2;
}
else{

g_driveDir = 3;
}
// now that we've set which direction to turn, start timing how long the t
urn
// has taken, and switch to mode 3.
g_turnUntil = millis() + g_turnlength;
g_mode = 3;
}
}
else if(g_mode == 3){
// We must be turning! The logic for switching to other modes is in that fu
nction.
turn();
}
}
//-------------------------------------------------------------------------// Helper functions:
void turn(){
// Executed when g_mode == 3
// Used to turn the robot.
if (millis() < g_turnUntil){
// keep'on turning...
move(g_driveDir);
}
else{
// If we're done turning, see if there is anything still in our way:
g_cm = ping();
// sometimes the sensor returns a 0 value that can foul up things :(
if(g_cm > g_collideDist || g_cm == 0){
// If there is nothing in our way go back to mode 1:
g_mode = 1;
}
else{
// If stuff is still in our way, turn more in the same direction until
// not blocked:
g_turnUntil = millis() + g_turnlength;
}
}
}
boolean proximityCheck(){
// Used when mode == 2: Swing the ping left and right taking readings to
// find which way is safe to go. Returns false when in the middle of the chec
k
// operation, returns true when entire check process is complete.
// Current angle of the servo:
float angle = g_servo2.read();
// check left first: ( full left is 0 deg)
if(g_checkLeft == false){
if(angle > g_servoLeft){
g_servo2.write(angle-10) ;
return false;
}
else{

g_checkLeft = true;
g_leftDist = ping();
return false;
}
}
// check right second: ( full right is 180 deg)
if(g_checkLeft == true && g_checkRight == false){
if(angle < g_servoRight){
g_servo2.write(angle+10) ;
return false;
}
else{
g_checkRight = true;
g_rightDist = ping();
return false;
}
}
// Go to center position third:
if(g_checkLeft == true && g_checkRight == true && g_checkCenter == false){
if(angle != g_servoCenter){
g_servo2.write(angle-10) ;
return false;
}
else{
g_checkCenter = true;
return false;
}
}
// Finally if we've checked left, right, and got back to center, return true
// to tell the later code that the full check is complete:
if(g_checkLeft == true && g_checkRight == true && g_checkCenter == true){
return true;
}
else{
return false;
}
}
void move(byte mode){
// This function moves the wheels. Can be called to when mode == 1 (driving)
or 3 (turning).
if(mode == 0){
// all stop!
g_motor1.setSpeed(0);
g_motor2.setSpeed(0);
g_motor3.setSpeed(0);
g_motor4.setSpeed(0);
}
else{
// full speed!
g_motor1.setSpeed(g_motorSpeed);
g_motor2.setSpeed(g_motorSpeed);
g_motor3.setSpeed(g_motorSpeed);
g_motor4.setSpeed(g_motorSpeed);
// full speed, but which direction?
if(mode == 1){

// go straight
g_motor1.run(FORWARD);
g_motor2.run(FORWARD);
g_motor3.run(FORWARD);
g_motor4.run(FORWARD);
}
else if(mode == 2){
// turn right
g_motor1.run(FORWARD);
g_motor2.run(BACKWARD);
g_motor3.run(BACKWARD);
g_motor4.run(FORWARD);
}
else if (mode == 3){
// turn left
g_motor1.run(BACKWARD);
g_motor2.run(FORWARD);
g_motor3.run(FORWARD);
g_motor4.run(BACKWARD);
}
}
}
float ping(){
// Returns the distance in cm from the ping sensor. Pulled from
// Arduino docs.
// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
pinMode(g_pingPin, OUTPUT);
digitalWrite(g_pingPin, LOW);
delayMicroseconds(2);
digitalWrite(g_pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(g_pingPin, LOW);
// The same pin is used to read the signal from the PING))): a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(g_pingPin, INPUT);
long duration = pulseIn(g_pingPin, HIGH);
float cm = microsecondsToCentimeters(duration);
return cm;
}
long microsecondsToCentimeters(long microseconds){
// Simple conversion setp, kept as a function simply to illustrate
// how it works. Pulled from Arduino docs.
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
//return microseconds / 29 / 2;
return microseconds / 58;
}