loading

i will the 4wdrobot with lcd that show the obstacle and the distance cm help please the code?

/*
 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 use 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 chassis.
 
 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.
#include <LiquidCrystal.h>
LiquidCrystal lcd(3, 4, 5, 6, 11, 12);

//--------------------------------------------------------------------------
// 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); // back left
AF_DCMotor g_motor2(2, MOTOR12_8KHZ); // back right
AF_DCMotor g_motor3(3, MOTOR12_8KHZ); // front right
AF_DCMotor g_motor4(4, MOTOR12_8KHZ); // front left
byte g_motorSpeed = 255; // max is 255

// 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 mounted 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 processing allowing the
    // multiple systems to play nicely together.  Make this number smaller and it won't have enough
    // time for each state to run properly:  The servo won't do a full sweep when 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 turing on has a great
    // effect on what this should be.  This value works well for my hardwood floors.
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 object 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 'proximity 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 direction,
      // 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 turn
      // 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 function.
    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 check
  // 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;

}

 

       // while(val == HIGH) {                  // Loop until pin reads a high value
//val = digitalRead(ultraSoundSignal);
//timecount = timecount +1;             // Count echo pulse time}
//ultrasoundValue = timecount;          // Append echo pulse time to ultrasoundValue
//lcd.clear();
//lcd.setCursor(0,0);
//lcd.print("OBSTAKEL  cm ");
//lcd.print(timecount/10);
//delay(200);
//if(timecount > 0){
//digitalWrite(ledPin, HIGH);}

 

 


rickharris4 years ago
As this isn't a question it's in the wrong place either put it in the forum or make an instructable out of it.