Pastebin launched a little side project called VERYVIRAL.com, check it out ;-) Want more features on Pastebin? Sign Up, it's FREE!
Guest

robot code

By: a guest on Jan 10th, 2014  |  syntax: None  |  size: 5.38 KB  |  views: 268  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  1. //this is a modified C++ for Arduino
  2.  
  3. //two slashes mean that what i am writing is a note, so it does not get executed, the notes are used
  4. //to tell you what I am doing
  5. //setting inputs on the arduino to their respective roles
  6. #define echoPin 3
  7. #define trigPin 2
  8. #define motor1Pin1 8
  9. #define motor1Pin2 9
  10. #define motor2Pin1 10
  11. #define motor2Pin2 11
  12. #define redLed 4
  13. #define greenLed 5
  14. #define blueLed 6
  15.  
  16.  
  17. void setup()//what the Arduino does before it runs its loop
  18. {
  19.   //setting pins to either inputs or outputs
  20.   pinMode(trigPin, OUTPUT);
  21.   pinMode(echoPin, INPUT); //this is an input because  the sensor is telling the Arduino how long the echo took, as
  22.   //you will see below
  23.   pinMode(motor1Pin1, OUTPUT);   //this is an output because the Arduino is telling the motor to spin
  24.   pinMode(motor1Pin2, OUTPUT);  
  25.   pinMode(motor2Pin1, OUTPUT);  
  26.   pinMode(motor2Pin2, OUTPUT);  
  27.   pinMode(redLed, OUTPUT);
  28.   pinMode(greenLed, OUTPUT);  
  29.   pinMode(blueLed, OUTPUT);  
  30.   //set serial port to output data, so I can make sure the distance data is correct
  31.   Serial.begin (9600);
  32. }
  33.  
  34. void loop() //begin loop, this is what the Arduino keeps doing indefinitely
  35. {
  36.   //this gets a distance from the sensor
  37.   //NOTE: I take two different readings of distance because sometimes intereference was causing abnormal distance
  38.   //readings (e.g. -687 instead of 90), which messed things up.
  39.   //I then test to make sure the two readings are not to different from each other, and then I average the two
  40.   //readings together and use that as a final distance.
  41.   int duration1, distance1, duration2, distance2, predistance1, predistance2;  //create an variable that is an integer for duration and distance
  42.   digitalWrite(trigPin, HIGH); //output a HIGH (full 5v) signal from trigpin
  43.   delayMicroseconds(1000); //wait 1000 microseconds
  44.   digitalWrite(trigPin, LOW);// stop outputting signal from trigpin
  45.   duration1 = pulseIn(echoPin, HIGH); // set the variable duration to how long it took for the echoPin to receive 5v
  46.   predistance1 = (duration1/2) / 29.1; //to find distance, divide duration by 2 and then by 29.1, to get centimeters
  47.   distance1 = abs(predistance1);//take the abosolute value of the distance (to make sure I dont get negative numbers)
  48.   Serial.print("distance1:  "); //print the word distance1 in the serial port
  49.   Serial.print(distance1); //print valua for distance1 in the serial port
  50.   Serial.println("  cm"); //print cm next to it, then add new line
  51.   delay(1); //delay for 1 millisecond
  52.   digitalWrite(trigPin, HIGH);
  53.   delayMicroseconds(1000);
  54.   digitalWrite(trigPin, LOW);
  55.   duration2 = pulseIn(echoPin, HIGH);
  56.   predistance2 = (duration2/2) / 29.1;
  57.   distance2 = abs(predistance2);
  58.   Serial.print("distance2:  ");
  59.   Serial.print(distance2);
  60.   Serial.println("  cm");
  61.  
  62.  
  63.   //This tests to make sure the two different distance values are not more different than 20
  64.   if ((distance2 < distance1 && distance1 - distance2 > 20) || (distance2 >= distance1 && distance2 - distance1 > 20))
  65.   {
  66.     //if they are a bigger difference than 20, make them both 30 (the robot will continue forward, since this is
  67.     // greater than the 20 required to turn around)
  68.       distance1 = 30;
  69.       distance2 = 30;
  70.    }
  71.    //this makes a new variable distance which is the average of the two previous distance variables
  72.  int distance = (distance1+distance2)/2;
  73.  
  74.  //  Serial.print("distance:  "); //print distance in the serial port
  75.  //  Serial.print(distance); //print distance in the serial port
  76.  // Serial.println("  cm"); //print cm next to it, then add new line
  77.  delay(1);
  78.   //this is what makes the robot autonomous
  79.   if ((distance > 20)) //if the distance from the sensor to an object is greater than 20cm, do the following
  80.     {
  81.       GoForward(); // execute GoForward function (which is below)
  82.       digitalWrite(redLed, LOW); // turn red LED off
  83.       digitalWrite(greenLed, LOW); // turn green LED off
  84.       digitalWrite(blueLed, HIGH); // turn blue LEd on
  85.       //note: I can make any color by using various combinations of red, blue, and green
  86.     }
  87.   else
  88.     {
  89.      
  90.       GoBack(); // go back (
  91.       digitalWrite(redLed, HIGH); // turn on red LED
  92.       digitalWrite(greenLed, LOW);
  93.       digitalWrite(blueLed, LOW);
  94.       delay(750); //continue GoBack() for 1.5 seconds
  95.      
  96.       TurnRight(); //then, turn right
  97.       digitalWrite(redLed, LOW);
  98.       digitalWrite(greenLed, HIGH); // turn on green LED
  99.       digitalWrite(blueLed, LOW);
  100.       delay(1250);
  101.      
  102.     }
  103. }
  104.  
  105. void GoForward() // this is the GoForward function, void means that it doesnt get executed unless another function
  106.                  // calls it
  107. {
  108.   //by changing which pins signal is outputed, the integrated circuit (this one is a L293D H-Bridge) changes which direction and which motor is spinning
  109.   digitalWrite(motor1Pin1, HIGH); //output signal on motor1pin1
  110.   digitalWrite(motor1Pin2, LOW); //do not output signal on motor1pin2
  111.   digitalWrite(motor2Pin1, LOW); //do not output signal on motor2pin1
  112.   digitalWrite(motor2Pin2, HIGH); //output signal on motor2pin2
  113.  
  114. }
  115.  
  116. void GoBack()
  117. {
  118.   digitalWrite(motor1Pin1, LOW);
  119.   digitalWrite(motor1Pin2, HIGH);
  120.   digitalWrite(motor2Pin1, HIGH);
  121.   digitalWrite(motor2Pin2, LOW);
  122. }
  123.  
  124. void TurnRight()
  125. {
  126.   digitalWrite(motor1Pin1, HIGH);
  127.   digitalWrite(motor1Pin2, LOW);
  128.   digitalWrite(motor2Pin1, HIGH);
  129.   digitalWrite(motor2Pin2, LOW);
  130. }