daily pastebin goal
20%
SHARE
TWEET

Untitled

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