API tools faq
paste
Guest User

Untitled

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