Im will create 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 // needed for the motorshield. #include // motorshield needs this for running servos. #include 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);}

astroboy9075 years ago
line breaks are great ;)
Yeah, I think they are "top!" sometimes.

L