API tools faq
paste
Guest User

code

a guest
Mar 21st, 2012
114
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 9.95 KB | None | 0 0
raw download clone embed print report
  1. #include <TimedAction.h>
  2. #include <PololuQTRSensors.h>
  3.  
  4. // PRESS PUSH BUTTON TO BEGIN LINE CALIBRATION
  5. // AFTER CALIBRATION 1 BEEP
  6. // IF PUSH BUTTON IS NEVER PRESSED, ROBOT WILL REMAIN AUTONOMOUS
  7.  
  8. // function prototypes
  9. void motorsf(); // motor forward
  10. void motorsl(); // motor left
  11. void motorsr(); // motor right
  12. void motorsb(); // motor reverse
  13. void readrange(); // range sensor
  14. void batteryCheck(); // check battery
  15.  
  16. //Interrupts
  17. TimedAction rangeAction = TimedAction(36,readrange); // range sensor
  18. TimedAction batteryAction = TimedAction(40,batteryCheck); // battery checking
  19. TimedAction lineSense(50,lineSensor); // was 500
  20.  
  21. #define NUM_SENSORS   6     // number of sensors used
  22. #define TIMEOUT       2500  // waits for 2500 us for sensor outputs to go low
  23.  
  24. //Global Variables
  25. int rangePin = A0; // pin for range sensor
  26. int motorPin1 = 4; // motor pins
  27. int motorPin2 = 5;
  28. int motorPin3 = 6;
  29. int motorPin4 = 7;
  30. int motorPin5 = 11;
  31. int motorPin6 = 10;
  32. int motorPin7 = 9;
  33. int motorPin8 = 8;
  34. bool state = false; // state of the push button
  35. bool linerun = false; // state of line sensor activation
  36. PololuQTRSensorsRC qtrrc((unsigned char[]) {22, 24, 26, 28, 30, 32}, // pins for line sensor
  37. NUM_SENSORS, TIMEOUT, QTR_NO_EMITTER_PIN);
  38. unsigned int sensorValues[NUM_SENSORS];
  39. const int startbutton = 12; // number of push button pin
  40. int startstate = LOW; // for reading status
  41. int SPKR = 36; // speaker for low bat warning
  42. float rangeValue; // values from IR range sensor in middle
  43.  
  44. void setup()
  45. {
  46.   Serial.begin(9600);
  47.   // declare inputs and outputs
  48.   pinMode(startbutton, INPUT); // push button
  49.   pinMode(rangePin, INPUT); // range sensor analog input 0
  50.  
  51.   pinMode(motorPin1, OUTPUT);
  52.   pinMode(motorPin2, OUTPUT);
  53.   pinMode(motorPin3, OUTPUT);
  54.   pinMode(motorPin4, OUTPUT);
  55.   pinMode(motorPin5, OUTPUT);
  56.   pinMode(motorPin6, OUTPUT);
  57.   pinMode(motorPin7, OUTPUT);
  58.   pinMode(motorPin8, OUTPUT);
  59.   pinMode(SPKR, OUTPUT); // speaker
  60. }
  61.  
  62. void readrange(){  // range sensor function
  63.  
  64.   int temp; // temporary variable for storing value
  65.   temp = analogRead(rangePin);
  66.   rangeValue = (6787.0 /((float)temp - 3.0)) - 4.0; // conversion to cm
  67.  // approximately 10 to 80 cm
  68.  
  69. if(rangeValue < 40 && rangeValue >= 10) {
  70.   for(int a = 0; a < 20; a++){
  71. motorsl();
  72.   }
  73. }
  74. else
  75. motorsf();
  76.  
  77. }
  78.  
  79. void motorsf(){
  80.   digitalWrite(motorPin1, HIGH);
  81.   digitalWrite(motorPin2, LOW);
  82.   digitalWrite(motorPin3, LOW);
  83.   digitalWrite(motorPin4, LOW);
  84.   delay(4);
  85.   digitalWrite(motorPin5, HIGH);
  86.   digitalWrite(motorPin6, LOW);
  87.   digitalWrite(motorPin7, LOW);
  88.   digitalWrite(motorPin8, LOW);
  89.  delay(4);
  90.   digitalWrite(motorPin1, LOW);
  91.   digitalWrite(motorPin2, HIGH);
  92.   digitalWrite(motorPin3, LOW);
  93.   digitalWrite(motorPin4, LOW);
  94.   delay(4);
  95.   digitalWrite(motorPin5, LOW);
  96.   digitalWrite(motorPin6, HIGH);
  97.   digitalWrite(motorPin7, LOW);
  98.   digitalWrite(motorPin8, LOW);
  99.   delay(4);
  100.   digitalWrite(motorPin1, LOW);
  101.   digitalWrite(motorPin2, LOW);
  102.   digitalWrite(motorPin3, HIGH);
  103.   digitalWrite(motorPin4, LOW);
  104.   delay(4);
  105.   digitalWrite(motorPin5, LOW);
  106.   digitalWrite(motorPin6, LOW);
  107.   digitalWrite(motorPin7, HIGH);
  108.   digitalWrite(motorPin8, LOW);
  109.   delay(4);
  110.   digitalWrite(motorPin1, LOW);
  111.   digitalWrite(motorPin2, LOW);
  112.   digitalWrite(motorPin3, LOW);
  113.   digitalWrite(motorPin4, HIGH);
  114.   delay(4);
  115.   digitalWrite(motorPin5, LOW);
  116.   digitalWrite(motorPin6, LOW);
  117.   digitalWrite(motorPin7, LOW);
  118.   digitalWrite(motorPin8, HIGH);
  119.   delay(4);
  120. }
  121.  
  122. void motorsb(){
  123.   digitalWrite(motorPin1, LOW);
  124.   digitalWrite(motorPin2, LOW);
  125.   digitalWrite(motorPin3, LOW);
  126.   digitalWrite(motorPin4, HIGH);
  127.   delay(5);
  128.   digitalWrite(motorPin5, LOW);
  129.   digitalWrite(motorPin6, LOW);
  130.   digitalWrite(motorPin7, LOW);
  131.   digitalWrite(motorPin8, HIGH);
  132.   delay(5);
  133.   digitalWrite(motorPin1, LOW);
  134.   digitalWrite(motorPin2, LOW);
  135.   digitalWrite(motorPin3, HIGH);
  136.   digitalWrite(motorPin4, LOW);
  137.   delay(5);
  138.   digitalWrite(motorPin5, LOW);
  139.   digitalWrite(motorPin6, LOW);
  140.   digitalWrite(motorPin7, HIGH);
  141.   digitalWrite(motorPin8, LOW);
  142.   delay(5);
  143.   digitalWrite(motorPin1, LOW);
  144.   digitalWrite(motorPin2, HIGH);
  145.   digitalWrite(motorPin3, LOW);
  146.   digitalWrite(motorPin4, LOW);
  147.   delay(5);
  148.   digitalWrite(motorPin5, LOW);
  149.   digitalWrite(motorPin6, HIGH);
  150.   digitalWrite(motorPin7, LOW);
  151.   digitalWrite(motorPin8, LOW);
  152.   delay(5);
  153.   digitalWrite(motorPin1, HIGH);
  154.   digitalWrite(motorPin2, LOW);
  155.   digitalWrite(motorPin3, LOW);
  156.   digitalWrite(motorPin4, LOW);
  157.   delay(5);
  158.   digitalWrite(motorPin5, HIGH);
  159.   digitalWrite(motorPin6, LOW);
  160.   digitalWrite(motorPin7, LOW);
  161.   digitalWrite(motorPin8, LOW);
  162.   delay(5);
  163.  
  164.  
  165. }
  166.  
  167. void motorsl(){
  168.  
  169.   digitalWrite(motorPin1, LOW);
  170.   digitalWrite(motorPin2, LOW);
  171.   digitalWrite(motorPin3, LOW);
  172.   digitalWrite(motorPin4, HIGH);
  173.   delay(5);
  174.   digitalWrite(motorPin5, HIGH);
  175.   digitalWrite(motorPin6, LOW);
  176.   digitalWrite(motorPin7, LOW);
  177.   digitalWrite(motorPin8, LOW);
  178.   delay(5);
  179.   digitalWrite(motorPin1, LOW);
  180.   digitalWrite(motorPin2, LOW);
  181.   digitalWrite(motorPin3, HIGH);
  182.   digitalWrite(motorPin4, LOW);
  183.   delay(5);
  184.   digitalWrite(motorPin5, LOW);
  185.   digitalWrite(motorPin6, HIGH);
  186.   digitalWrite(motorPin7, LOW);
  187.   digitalWrite(motorPin8, LOW);
  188.   delay(5);
  189.   digitalWrite(motorPin1, LOW);
  190.   digitalWrite(motorPin2, HIGH);
  191.   digitalWrite(motorPin3, LOW);
  192.   digitalWrite(motorPin4, LOW);
  193.   delay(5);
  194.   digitalWrite(motorPin5, LOW);
  195.   digitalWrite(motorPin6, LOW);
  196.   digitalWrite(motorPin7, HIGH);
  197.   digitalWrite(motorPin8, LOW);
  198.   delay(5);
  199.   digitalWrite(motorPin1, HIGH);
  200.   digitalWrite(motorPin2, LOW);
  201.   digitalWrite(motorPin3, LOW);
  202.   digitalWrite(motorPin4, LOW);
  203.   delay(5);
  204.   digitalWrite(motorPin5, LOW);
  205.   digitalWrite(motorPin6, LOW);
  206.   digitalWrite(motorPin7, LOW);
  207.   digitalWrite(motorPin8, HIGH);
  208.   delay(5);
  209.  
  210. }
  211.  
  212. void motorsr(){
  213.  
  214.   digitalWrite(motorPin1, HIGH);
  215.   digitalWrite(motorPin2, LOW);
  216.   digitalWrite(motorPin3, LOW);
  217.   digitalWrite(motorPin4, LOW);
  218.   delay(5);
  219.   digitalWrite(motorPin5, LOW);
  220.   digitalWrite(motorPin6, LOW);
  221.   digitalWrite(motorPin7, LOW);
  222.   digitalWrite(motorPin8, HIGH);
  223.   delay(5);
  224.   digitalWrite(motorPin1, LOW);
  225.   digitalWrite(motorPin2, HIGH);
  226.   digitalWrite(motorPin3, LOW);
  227.   digitalWrite(motorPin4, LOW);
  228.   delay(5);
  229.   digitalWrite(motorPin5, LOW);
  230.   digitalWrite(motorPin6, LOW);
  231.   digitalWrite(motorPin7, HIGH);
  232.   digitalWrite(motorPin8, LOW);
  233.   delay(5);
  234.   digitalWrite(motorPin1, LOW);
  235.   digitalWrite(motorPin2, LOW);
  236.   digitalWrite(motorPin3, HIGH);
  237.   digitalWrite(motorPin4, LOW);
  238.   delay(5);
  239.   digitalWrite(motorPin5, LOW);
  240.   digitalWrite(motorPin6, HIGH);
  241.   digitalWrite(motorPin7, LOW);
  242.   digitalWrite(motorPin8, LOW);
  243.   delay(5);
  244.   digitalWrite(motorPin1, LOW);
  245.   digitalWrite(motorPin2, LOW);
  246.   digitalWrite(motorPin3, LOW);
  247.   digitalWrite(motorPin4, HIGH);
  248.   delay(5);
  249.   digitalWrite(motorPin5, HIGH);
  250.   digitalWrite(motorPin6, LOW);
  251.   digitalWrite(motorPin7, LOW);
  252.   digitalWrite(motorPin8, LOW);
  253.   delay(5);
  254. }
  255.  
  256. void batteryCheck(){ // use the voltage sensor to determine if the battery goes below 8.43 Volts
  257. int sensorValue = analogRead(A7); // read analog pin 7
  258.   Serial.println(sensorValue, DEC);
  259.   if(sensorValue <= 70){ // if the value is below 70, 8.43V
  260.   for (int i=0; i<500; i++) {  // generate a 1KHz tone for 1/2 second
  261.   digitalWrite(SPKR, HIGH);
  262.   delayMicroseconds(500); // annoying beep
  263.   digitalWrite(SPKR, LOW);
  264.   delayMicroseconds(500);
  265.   }
  266.  
  267. }
  268. }
  269.  
  270. void lineSensor(){ // line sensor function
  271.   while(linerun == true){
  272.  
  273.   batteryAction.check(); // make sure we are still checking the battery
  274.   unsigned int position = qtrrc.readLine(sensorValues);
  275.  
  276.   unsigned char i;
  277.   for (i = 0; i < NUM_SENSORS; i++)
  278.   {
  279.     Serial.print(sensorValues[i] * 10 / 1001);
  280.     Serial.print(' ');
  281.   }
  282.   Serial.print("    ");
  283.   Serial.println(position);
  284.  
  285.   //0 1 2 3 4 5
  286.   //x x 0 0 x x
  287.   //go forward
  288.   //0 1 2 3 4 5
  289.   //x x x x 0 0
  290.   //go right
  291.   //0 1 2 3 4 5
  292.   //0 0 x x x x
  293.   //go left
  294.  
  295.   if((((sensorValues[2] * 10 / 1001) <= 2 )||((sensorValues[3] * 10 / 1001) <= 2))){
  296.   motorsf();
  297.   }
  298.   if((((sensorValues[4] * 10 / 1001) <= 2 )||((sensorValues[5] * 10 / 1001) <= 2))){
  299.    
  300.   motorsr();
  301.    
  302.   }
  303.   if((((sensorValues[0] * 10 / 1001) <= 2 )||((sensorValues[1] * 10 / 1001) <= 2))){
  304.  
  305.   motorsl();
  306.    
  307.   }
  308.  
  309.   }
  310. }
  311.  
  312. void loop(){
  313.  
  314.   startstate = digitalRead(startbutton); // constantly read to see if push button is pressed so autonomous mode can stop
  315.   rangeAction.check(); // start the IR range interrupt using the range sensor
  316.   batteryAction.check(); // start the battery checking interrupt using the voltage sensor
  317.  
  318.   if(startstate == HIGH){ // if push button is pressed make state true
  319.     state = true;
  320.   }
  321.   if(state == true){ // if state is true calibrate the robot
  322.     int y;
  323.   digitalWrite(34, HIGH);    // turn on LED to indicate we are in calibration mode
  324.    Serial.println("Calibration beginning");
  325.   for (y = 0; y < 400; y++)  // make the calibration take about 10 seconds was number 400
  326.   {
  327.     qtrrc.calibrate();       // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
  328.   }
  329.   digitalWrite(13, LOW);     // turn off LED to indicate we are through with calibration
  330.  
  331.   Serial.println("Calibration finished");
  332.  
  333.   // beep so we know that calibration is complete
  334.   for (int i=0; i<500; i++) {  // generate a 1KHz tone for 1/2 second
  335.   digitalWrite(SPKR, HIGH);
  336.   delayMicroseconds(500);
  337.   digitalWrite(SPKR, LOW);
  338.   delayMicroseconds(500);
  339.   }
  340.   int i;
  341.   for (i = 0; i < NUM_SENSORS; i++)
  342.   {
  343.     Serial.print(qtrrc.calibratedMinimumOn[i]);
  344.     Serial.print(' ');
  345.   }
  346.   Serial.println();
  347.  
  348.   for (i = 0; i < NUM_SENSORS; i++)
  349.   {
  350.     Serial.print(qtrrc.calibratedMaximumOn[i]);
  351.     Serial.print(' ');
  352.   }
  353.   Serial.println();
  354.   Serial.println();
  355.   delay(1000);
  356.  
  357.   linerun = true;
  358.   rangeAction.disable();
  359.   lineSense.check();
  360.  
  361.   }
  362.  
  363. }
Add Comment
Please, Sign In to add comment
Public Pastes
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
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!