Advertisement
Guest User

Untitled

a guest
Jun 10th, 2011
990
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 6.97 KB | None | 0 0
  1. /*
  2.  Autonomous roving robot
  3.  Parts:
  4.    -Arduino Uno
  5.    -Parallax PING))) ultrasonic sensor
  6.    -Adafruit motor shield http://www.ladyada.net/make/mshield/
  7.    -2WD Mobile Platform http://www.makershed.com/ProductDetails.asp?ProductCode=MKSEEED7
  8.    -Lever switch (for back bumper)
  9.    -Indicator LED
  10.    -Energizer XPAL XP8000 battery (custom cable and self-built regulator for motors)
  11.  Original motion functions and design from Robot Living 8/30/2010
  12.  Code borrowed from example sketches:
  13.    -Ping
  14.  Original code created June 2011
  15.  by Daniel Gentleman
  16.  http://thoughtfix.com
  17.  
  18. Additional credits:
  19.  Victor Brilon
  20.  Mark Balliet
  21.  luckylarry.co.uk for the math on the Sharp sensor
  22.  Advice from arduino.cc forum
  23.  Advice from from #arduino on irc.freenode.net
  24.  Advice from adafruit forums
  25.  Further inspiration
  26.       -Adafruit Industries
  27.       -Make Magazine
  28.  Adafruit Motor shield library
  29.  
  30.  Wiring (remember, analog 0 is also digital 14)
  31.  - Left motor to Motor 1
  32.  - Right motor to Motor 2
  33.  - LEDs on digital 15 and 16
  34.  - PING))) on digital 17
  35.  - Sharp IR sensors on digital 18 and 19
  36.  - Lever switch (bumper attached) on digital 14
  37.  - USB power (from a USB battery) to the Arduino
  38.  - Regulated power (I used an Energizer XPAL XP8000 regulated to +5v) for servos
  39. */
  40.  
  41.  
  42. #include <AFMotor.h>
  43.  
  44. AF_DCMotor motorL(1, MOTOR12_1KHZ);
  45. AF_DCMotor motorR(2, MOTOR12_1KHZ);
  46.  
  47.  
  48. int BSensor = 14; //Back sensor
  49. int FLEDPin = 15; //Forward LED
  50. int BLEDPin = 16; //Backup LED
  51. int pingPin = 17; //Front sensor
  52. int FSensorR = 18; //Front right sensor
  53. int FSensorL = 19;  //Front left sensor
  54. int count = 0;
  55. int b=0;
  56. int bs=0;
  57. int sensorValue;
  58. int forwardSensor;
  59. int sensorValueB=1;
  60. int leftSensor=0;
  61. int rightSensor=0;
  62. int obstacle=0;
  63.  
  64.  
  65. void setup() {
  66.   Serial.begin(9600);
  67.   randomSeed(analogRead(0));
  68.   pinMode(BLEDPin, OUTPUT);
  69.   pinMode(FLEDPin, OUTPUT);
  70.   pinMode(BSensor, INPUT);
  71.   pinMode(pingPin, INPUT);
  72.   pinMode(FSensorR, INPUT);
  73.   pinMode(FSensorL, INPUT);
  74.   motorL.setSpeed(200);
  75.   motorR.setSpeed(200);
  76.   motorL.run(RELEASE);
  77.   motorR.run(RELEASE);
  78. }
  79.  
  80. void loop()
  81. {
  82.   uint8_t i;
  83.   Forward();
  84. }
  85.  
  86. void Forward () // Charge forth!
  87. {
  88.   digitalWrite(FLEDPin, HIGH);                
  89.   digitalWrite(BLEDPin, LOW);
  90.   ForwardSensor ();
  91.   if (obstacle == 0){ //unless something is in the way
  92.   Serial.println("Going forward.");
  93.   motorL.run(FORWARD);
  94.   motorR.run(FORWARD);
  95.   }
  96.  
  97. }
  98.  
  99. void ForwardSensor ()
  100. {
  101.   leftSensor = (ForwardSensorLeft());
  102.   rightSensor =(ForwardSensorRight());
  103.   forwardSensor = (getDistance());
  104.   delay(30);
  105.   //  Serial.print(leftSensor);
  106.   //  Serial.println(" left");
  107.   //  Serial.print(rightSensor);
  108.   //  Serial.println(" right");
  109.   //  Serial.print(forwardSensor);
  110.   //  Serial.println(" front");
  111.   if (forwardSensor <=30 || leftSensor <= 30 || rightSensor <= 30 )
  112.   { // I gave 10cm of "wiggle room" so it doesn't turn endlessly.
  113.     obstacle=1;
  114.     if (ForwardSensorRight() > (ForwardSensorLeft()-10)){
  115.       RightBackward();
  116.     }
  117.     else if (ForwardSensorRight() < (ForwardSensorLeft()-10)){
  118.       LeftBackward();
  119.     }
  120.     else{
  121.       Backward();
  122.     }
  123.   }
  124.   else {
  125.     obstacle = 0;
  126.   }
  127.   sensorValue=0;
  128.   rightSensor=0;
  129.   leftSensor=0;
  130. }
  131.  
  132. int ForwardSensorRight ()
  133. { // This function is here so you don't have to re-write code
  134.   // if you use a different sensor.
  135.   sensorValue = irDistance(FSensorR);
  136.   return sensorValue;
  137. }
  138.  
  139. int ForwardSensorLeft ()
  140. { // This function is here so you don't have to re-write code
  141.   // if you use a different sensor.
  142.   sensorValue = irDistance(FSensorL);
  143.   return sensorValue;
  144. }
  145.  
  146. void BackwardSensor ()
  147. {
  148.   sensorValueB = digitalRead(BSensor);
  149.   if (sensorValueB == 1)
  150.   {
  151.     Serial.println("Object detected while going backwards.");
  152.     motorL.run(RELEASE);
  153.     motorR.run(RELEASE);
  154.     for (bs=0; bs <= 3; bs++) // Add blinky lights for personality on detecting object when backing up.
  155.     {
  156.       digitalWrite(FLEDPin, HIGH);                
  157.       digitalWrite(BLEDPin, HIGH);
  158.       delay(5);
  159.       digitalWrite(FLEDPin, LOW);                
  160.       digitalWrite(BLEDPin, LOW);
  161.       delay (5);
  162.     }
  163.     b=31;
  164.     bs=0;
  165.     Forward();
  166.  
  167.   }
  168.   sensorValueB = 0;
  169. }
  170.  
  171. void Backward ()
  172. {
  173.   motorL.run(RELEASE);
  174.   motorR.run(RELEASE);
  175.   digitalWrite(FLEDPin, LOW);                
  176.   digitalWrite(BLEDPin, HIGH);
  177.   Serial.println("Going backwards.");
  178.  
  179.   do
  180.   {
  181.     b++;
  182.     motorL.run(BACKWARD);
  183.     motorR.run(BACKWARD);
  184.     delay(20);
  185.     BackwardSensor ();
  186.   }
  187.   while (b < 20);
  188.  
  189.   b=0;
  190. }
  191.  
  192.  
  193. void LeftBackward ()
  194. {
  195.   motorL.run(RELEASE);
  196.   motorR.run(RELEASE);
  197.   digitalWrite(FLEDPin, LOW);                
  198.   digitalWrite(BLEDPin, HIGH);
  199.   Serial.println("Going left backwards.");
  200.  
  201.   do
  202.   {
  203.     b++;
  204.     //    Serial.print("b = ");
  205.     //    Serial.print(b);
  206.     motorL.run(BACKWARD);
  207.     motorR.run(RELEASE);
  208.     BackwardSensor ();
  209.     delay (20);
  210.   }
  211.   while (b < 20);
  212.  
  213.   b=0;
  214. }
  215.  
  216. void RightBackward ()
  217. {
  218.   motorL.run(RELEASE);
  219.   motorR.run(RELEASE);
  220.   digitalWrite(FLEDPin, LOW);                
  221.   digitalWrite(BLEDPin, HIGH);
  222.   Serial.println("Going right backwards.");
  223.  
  224.   do
  225.   {
  226.     b++;
  227.     //    Serial.print("b = ");
  228.     //    Serial.print(b);
  229.     motorL.run(RELEASE);
  230.     motorR.run(BACKWARD);
  231.     BackwardSensor ();
  232.     delay (20);
  233.   }
  234.   while (b < 20);
  235.  
  236.   b=0;
  237. }
  238.  
  239. int getDistance()
  240. {
  241.   // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  242.   // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  243. reread:  // takes another reading if cm=0
  244.   long duration, cm;
  245.   pinMode(pingPin, OUTPUT);
  246.   digitalWrite(pingPin, LOW);
  247.   delayMicroseconds(2);
  248.   digitalWrite(pingPin, HIGH);
  249.   delayMicroseconds(5);
  250.   digitalWrite(pingPin, LOW);
  251.   // The same pin is used to read the signal from the PING))): a HIGH
  252.   // pulse whose duration is the time (in microseconds) from the sending
  253.   // of the ping to the reception of its echo off of an object.
  254.   pinMode(pingPin, INPUT);
  255.   duration = pulseIn(pingPin, HIGH);
  256.  
  257.   // convert the time into a distance
  258.   cm = microsecondsToCentimeters(duration);
  259.   //  Serial.print(cm);
  260.   //  Serial.print("cm");
  261.   //  Serial.println();
  262.   delay(100);
  263.   //  if (cm == 0) {
  264.   //    goto reread;
  265.   //  }
  266.   return cm;
  267. }
  268.  
  269. long microsecondsToCentimeters(long microseconds)
  270. {
  271.   // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  272.   // The ping travels out and back, so to find the distance of the
  273.   // object we take half of the distance travelled.
  274.   return (microseconds/58);
  275. }
  276.  
  277. int irDistance(int irPin) {
  278.   float volts = analogRead(irPin)*0.0048828125;   // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
  279.   int distance = 32.5*pow(volts, -1.10);          // theretical distance 32.5/ (1/Volts)S
  280.   //  Serial.print(distance);
  281.   //  Serial.print(" from ");
  282.   //  Serial.println(irPin);  
  283.   delay (10);
  284.   return distance;        // print the raw analog value to serial port
  285. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement