Facebook
From aaaa, 1 Year ago, written in C++.
Embed
Download Paste or View Raw
Hits: 811
  1. /* Sample code for the TI-Basispraktium final line-following and barcode-reading parcours
  2.  *  
  3.  * author: Lukas Kaul
  4.  * date: June 2016
  5.  *
  6.  * This code relies on the Asurino library with modified methods
  7.  * setMotorSpeed()
  8.  * setBackLED()
  9.  *
  10.  * https://github.com/LuSeKa/Asurino-Library.git
  11.  *
  12.  */
  13.  
  14. #include <Asuro.h>
  15.  
  16. // Instantiate Asuro
  17. Asuro asuro = Asuro();
  18.  
  19. // type defs
  20. enum MAIN_STATE {followLine, searchLine, scanBarcode, findLine, blinkNTimes, stop};
  21. enum ENCODER_STATE {black, white};
  22. enum SEARCH_LINE_STATE {left, backFromLeft, right, backFromRight, failure};
  23. enum BARCODE_STATE {bright, dark, end};
  24. enum BLINK_STATE {on, off, done};
  25.  
  26. // globals
  27.  
  28. // states
  29. enum MAIN_STATE mainState;
  30. enum ENCODER_STATE encoderState[2] {black, black};
  31. enum BARCODE_STATE barcodeState;
  32. enum SEARCH_LINE_STATE searchLineState;
  33. enum BLINK_STATE blinkState;
  34.  
  35. // intertask variables
  36. int lineSum = 0; /* WRITE: taskReadLineSensor, READ: others */
  37. int lineDiff = 0; /* WRITE: taskReadLineSensor, READ: others */
  38. int targetBlinkCount = 0; /* WRITE: taskScanBarcode, READ: taskBlinkNTimes */
  39. int statusLedColor = GREEN; /* WRITE: many, READ: taskStatusLED */
  40.  
  41. // task-local variables
  42. int currentBlinkCount = 0; /* taskBlinkNTimes */
  43. int ticksSinceLastDark = 0; /* taskScanBarcode */
  44. int barcodeCount = 0; /* taskScanBarcode */
  45. unsigned long encoderValues[2] {0, 0};
  46.  
  47. // timeouts
  48. unsigned long taskStatusLED_timeout;
  49. unsigned long taskScanBarcode_timeout;
  50. unsigned long taskEncoder_timeout;
  51. unsigned long taskFollowLine_timeout;
  52. unsigned long taskSearchLine_timeout;
  53. unsigned long taskReadLineSensor_timeout;
  54. unsigned long taskFindLine_timeout;
  55. unsigned long taskBlinkNTimes_timeout;
  56. unsigned long taskStop_timeout;
  57.  
  58. // periods
  59. unsigned long taskStatusLED_period = 250;
  60. unsigned long taskEncoder_period = 3;
  61. unsigned long taskFollowLine_period = 20;
  62. unsigned long taskSearchLine_period = 20;
  63. unsigned long taskScanBarcode_period = 50;
  64. unsigned long taskReadLineSensor_period = 10;
  65. unsigned long taskFindLine_period = 10;
  66. unsigned long taskBlinkNTimes_period = 500;
  67. unsigned long taskStop_period = 500;
  68.  
  69. // controller parameters
  70.  
  71. // distances
  72. int turnCount = 100;
  73. int maxBarDistance = 70;
  74.  
  75. // brightness values
  76. int whiteThreshold = 750;
  77. int minLineContrast = 110;
  78. int odoThrehshold = 120;
  79.  
  80. // PWMs
  81. int lineBasePWM = 60;
  82. int findLinePWM = 65;
  83. int turnPWM = 70;
  84. int barcodePWM = 70;
  85. int kp_line = 1;
  86.  
  87.  
  88.  
  89.  
  90.  
  91. /************************************************/
  92.  
  93. /*********** taskReadLineSensor ***********/
  94. void taskReadLineSensor() {
  95.   if (taskReadLineSensor_timeout > millis()) return;
  96.   taskReadLineSensor_timeout += taskReadLineSensor_period;
  97.  
  98.   int lineData[2];
  99.   asuro.readLinesensor(lineData);
  100.   lineDiff = lineData[RIGHT] - lineData[LEFT];
  101.   lineSum = lineData[RIGHT] + lineData[LEFT];
  102. }
  103. /************************************************/
  104.  
  105. /*********** taskEncoder ***********/
  106. void taskEncoder() {
  107.  
  108.   if (taskEncoder_timeout > millis()) return;
  109.   taskEncoder_timeout += taskEncoder_period;
  110.   int odoData[2];
  111.   asuro.readOdometry(odoData);
  112.  
  113.   int foundTick = 0;
  114.   foundTick += findTick(RIGHT, odoData);
  115.   foundTick +=  findTick(LEFT, odoData);
  116.  
  117.   if (foundTick != 0) {
  118.     Serial.print(encoderValues[LEFT]);
  119.     Serial.print('\t');
  120.     Serial.println(encoderValues[RIGHT]);
  121.   }
  122. }
  123.  
  124. int findTick(int side, int* odoData) {
  125.   // high raw values mean dark, low raw values mean bright
  126.     static int lastVal[2] {512, 512};
  127.   switch (encoderState[side]) {
  128.     case white: // values are low
  129.       if (odoData[side] < lastVal[side]) {
  130.         lastVal[side] = odoData[side]; // new lowest value
  131.       }
  132.       else if (odoData[side] > (lastVal[side] + odoThrehshold)) { // transition from bright to dark
  133.         lastVal[side] = odoData[side];
  134.         encoderState[side] = black;
  135.         // tick
  136.         encoderValues[side] ++;
  137.         return 1;
  138.       }
  139.       break;
  140.     case black: // values are high
  141.       if (odoData[side] > lastVal[side]) {
  142.         lastVal[side] = odoData[side]; // new highest value
  143.       }
  144.       else if (odoData[side] < (lastVal[side] - odoThrehshold)) { // transition from dark to bright
  145.         lastVal[side] = odoData[side];
  146.         encoderState[side] = white;
  147.         // tick
  148.         encoderValues[side] ++;
  149.         return 1;
  150.       }
  151.       break;
  152.   }
  153.   return 0;
  154. }
  155. /************************************************/
  156.  
  157. int phototransistorMWOffset(int dez, int n) {
  158.   int sum = 0;
  159.   /*Berechne den Mittelwert des Grundoffsets*/
  160.   for (int i = 0; i < dez; i++) {
  161.     asuro.readLinesensor(lineData);
  162.     sum += (lineData[0] - lineData[1]);   //  Summe der Offsets
  163.   }
  164.   return sum >> n;   /* Mittelwert des Offsets*/
  165.  
  166. }
  167.  
  168. /*********** taskFollowLine ***********/
  169. void taskFollowLine() {
  170.   if (taskFollowLine_timeout > millis()) return;
  171.   taskFollowLine_timeout += taskFollowLine_period;
  172.   asuro.setMotorDirection(FWD,FWD);
  173.  
  174.   don = round(phototransistorMWOffset(2, 1));
  175.   x = don - doff;                  // Regelabweichung
  176.   isum += x;
  177.   if (isum > 16000) isum = 16000;        //Begrenzung um ├ťberlauf zu vermeiden
  178.   if (isum < -16000) isum = -16000;
  179.   yi = isum / 625 * ki;             //I-Anteil berechnen
  180.   // yi= ki * isum;
  181.   yd = (x - xalt) * kd;             // D-Anteil berechnen und mit
  182.   yd += drest;                     // nicht ber├╝cksichtigtem Rest addieren
  183.   if (yd > 255) drest = yd - 255;      // merke Rest
  184.   else if (yd < -255) drest = yd + 255;
  185.   else drest = 0;
  186.   yp = x * kp;                      // P-Anteil berechnen
  187.   //yp = kp * x;
  188.   y = yp + yi + yd;                  // Gesamtkorrektur
  189.   y2 = y >> 1;                     // Aufteilung auf beide Motoren
  190.   xalt = x;                        // x merken
  191.   speedLeft = speedRight = speed1;
  192.   asuro.setMotorDirection(FWD, FWD);
  193.   if ( y > 5) {                     // nach rechts
  194.     speedLeft = speed1 + y2;         // links beschleunigen
  195.     if (speedLeft > 255) {
  196.       speedLeft = 255;            // falls Begrenzung
  197.       y2 = speedLeft - speed1;      // dann Rest rechts ber├╝cksichtigen
  198.     }
  199.     y = y - y2;
  200.     speedRight = speed1 - y;         // rechts abbremsen
  201.     if (speedRight < 0) {
  202.       speedRight = 0;
  203.     }
  204.   }
  205.   if ( y < -5) {                     // nach links
  206.     speedRight = speed1 - y2;         // rechts beschleunigen !!!was speed - y2!!
  207.     if (speedRight > 255) {
  208.       speedRight = 255;            // falls Begrenzung
  209.       y2 = speed1 - speedRight;      // dann Rest links ber├╝cksichtigen !!was speed - speedRight!!
  210.     }
  211.     y = y - y2;
  212.     speedLeft = speed1 + y;            // links abbremsen  !! was speed + y!!!
  213.     if (speedLeft < 0) {
  214.       speedLeft = 0;
  215.     }
  216.   }
  217.   leftDir = rightDir = FWD;
  218.  
  219.  
  220.   asuro.setMotorDirection(leftDir, rightDir);
  221.   asuro.setMotorSpeed(abs(speedLeft), abs(speedRight));
  222. }
  223. }
  224. void transitionToFollowLine() {
  225.   Serial.println("Transitioning to FollowLine");
  226.  
  227.   /* @@@ TODO: set mainState; reset sub-state; reset task timeouts; reset task variables; set status led; set motor speeds? */
  228. }
  229. /************************************************/
  230.  
  231.  
  232.  
  233.  
  234. /*********** taskSearchLine ***********/
  235. void taskSearchLine() {
  236.   // make sure encoders are reset before entering this task
  237.         if (taskSearchLine_timeout > millis()) return;
  238.         taskSearchLine_timeout += taskSearchLine_period;
  239.        
  240.         switch(searchLineState) {
  241.         case left:
  242.                 panAndSearch(RIGHT, backFromLeft); // turn right motor to search left
  243.                 break;
  244.         case right:
  245.                 panAndSearch(LEFT, backFromRight); // turn left motor to search right
  246.                 break;
  247.         case backFromLeft:
  248.                 backToCenter(RIGHT, right); // go back to center position
  249.                 break;
  250.         case backFromRight:
  251.                 backToCenter(LEFT, failure); // go back to center position
  252.                 break;
  253.         case failure: // the line does not continue
  254.                 asuro.setMotorSpeed(0, 0);
  255.                 Serial.println("Reached end of line");
  256.                 transitionToScanBarcode();
  257.                 break;
  258.         }
  259.   /* @@@ TODO: implement sub-statemachine on searchLineState, call panAndSearch, backToCenter or transition */
  260. }
  261.  
  262. void panAndSearch(int side, SEARCH_LINE_STATE nextState) {
  263.   /* @@@ TODO: control motors; if line is found transition main-statemachine, if encoderValues[side] > turnCount transition sub-statemachine(searchLineState) */
  264.   if
  265. }
  266.  
  267. void backToCenter(int side, SEARCH_LINE_STATE nextState) {
  268.   /* @@@ TODO: control motors; if encoderValues[side] > turnCount transition sub-statemachine(searchLineState) */
  269. }
  270.  
  271. void transitionToSearchLine() {
  272.   Serial.println("Transitioning to SearchLine");
  273.   /* @@@ TODO: set mainState; reset sub-state; reset task timeouts; reset task variables; set status led; set motor speeds? */
  274.  
  275. }
  276. /************************************************/
  277.  
  278. /*********** taskScanBarcode ***********/
  279. void taskScanBarcode() {
  280.   if (taskScanBarcode_timeout > millis()) return;
  281.   taskScanBarcode_timeout += taskScanBarcode_period;
  282.  
  283.   void taskScanBarcode() {
  284. /*
  285. insert the usual pattern here
  286. */
  287.         switch (barcodeState) { //bright, dark or end
  288.                 case bright:
  289.                         ticksSinceLastDark = encoderValues[RIGHT] + encoderValues[LEFT]; // keep track of distance
  290.                         if (ticksSinceLastDark > maxBarDistance) { // reached the end of the barcode
  291.                                 barcodeState = end;
  292.                         break;
  293.                         }
  294.                         if (!noLine()) { // bar detected
  295.                                 barcodeState = dark;
  296.                         break;
  297.                         }
  298.                         break;
  299.                 case dark:
  300.                         if (noLine()) { // transition from black to white
  301.                                 barcodeCount++; // this transition counts as bar
  302.                                 barcodeState = bright;
  303.                         }
  304.                         resetEncoders(); // start measuring distance from here
  305.                         break;
  306.                 case end:
  307.                         asuro.setMotorSpeed(0, 0);
  308.                         if (barcodeCount == 1) {
  309.                                 transitionToStop();
  310.                         }
  311.                         else {
  312.                                 statusBlinkCount = barcodeCount;
  313.                                 transitionToBlinkNTimes(); // go to blink task
  314.                         }
  315.                         break;
  316.                         }
  317.  
  318. }
  319.  
  320. void transitionToScanBarcode() {
  321.   Serial.println("Transitioning to ScanBarcode");
  322.   /* @@@ TODO: set mainState; reset sub-state; reset task timeouts; reset task variables; set status led; set motor speeds? */
  323.  
  324. }
  325. /************************************************/
  326.  
  327. /*********** taskBlinkNTimes ***********/
  328. void taskBlinkNTimes() {
  329.   if (taskBlinkNTimes_timeout > millis()) return;
  330.   taskBlinkNTimes_timeout += taskBlinkNTimes_period;
  331.  
  332.   /* @@@ TODO: implement sub-statemachine on blinkState */
  333. }
  334.  
  335. void transitionToBlinkNTimes() {
  336.   Serial.println("Transitioning to signal blinking");
  337.   /* @@@ TODO: set mainState; reset sub-state; reset task timeouts; reset task variables; set status led; set motor speeds? */
  338.  
  339. }
  340. /************************************************/
  341.  
  342. /*********** taskFindLine ***********/
  343. void taskFindLine() {
  344.   if (taskFindLine_timeout > millis()) return;
  345.   taskFindLine_timeout += taskFindLine_period;
  346.  
  347.   /* @@@ TODO: transition when line is found */
  348. }
  349.  
  350. void transitionToFindLine() {
  351.   Serial.println("Transitioning to findLine");
  352.   /* @@@ TODO: set mainState; reset sub-state; reset task timeouts; reset task variables; set status led; set motor speeds? */
  353.  
  354. }
  355. /************************************************/
  356.  
  357. /*********** taskStop ***********/
  358. void taskStop() {
  359.   if (taskStop_timeout > millis()) return;
  360.   taskStop_timeout += taskStop_period;
  361.  
  362.   Serial.println("Parcours finished. Hurray :)");
  363. }
  364.  
  365. void transitionToStop() {
  366.   Serial.println("Transitioning to Stop");
  367.   /* @@@ TODO: set mainState; reset sub-state; reset task timeouts; reset task variables; set status led */
  368.  
  369. }
  370. /************************************************/
  371.  
  372.  
  373. /*********** helper functions ***********/
  374. void resetEncoders() {
  375.   encoderValues[RIGHT] = 0;
  376.   encoderValues[LEFT] = 0;
  377. }
  378.  
  379. bool noLine() {
  380.   return (abs(lineDiff) < minLineContrast) && (lineSum > whiteThreshold);
  381. }
  382. /************************************************/
  383. /************* findTickLine ****************/
  384. int findTickLine(int side, int* lineData) {
  385.   // high raw values mean dark, low raw values mean bright
  386.   static int lastVal[2] {512, 512};
  387.   switch (line_state[side]) {
  388.     case white: // values are low
  389.       if (lineData[side] < lastVal[side]) {
  390.         lastVal[side] = lineData[side]; // new lowest value
  391.       }
  392.       else if (lineData[side] > (lastVal[side] + odoThrehshold)) { // transition from bright to dark
  393.         lastVal[side] = lineData[side];
  394.         line_state[side] = black;
  395.         // tick
  396.         lineValues[side] ++;
  397.         return 1;
  398.       }
  399.       break;
  400.     case black: // values are high
  401.       if (lineData[side] > lastVal[side]) {
  402.         lastVal[side] = lineData[side]; // new highest value
  403.       }
  404.       else if (lineData[side] < (lastVal[side] - odoThrehshold)) { // transition from dark to bright
  405.         lastVal[side] = lineData[side];
  406.         line_state[side] = white;
  407.         // tick
  408.         lineValues[side] ++;
  409.         return 1;
  410.       }
  411.       break;
  412.   }
  413.   return 0;
  414. }
  415.  
  416. /********* taskLine ***********/
  417. void taskLine() {
  418.   if (taskEncoder_timeout > millis()) return;
  419.   taskEncoder_timeout += taskEncoder_period;
  420.   int lineData[2];
  421.   asuro.readLinesensor(lineData);
  422.  
  423.   int foundTick = 0;
  424.   foundTick += findTickLine(RIGHT, lineData);
  425.   foundTick +=  findTickLine(LEFT, lineData);
  426.  
  427.   }
  428. }
  429. /*********** taskStatusLED ***********/
  430. void taskStatusLED() {
  431.   // blink status led green
  432.   if (taskStatusLED_timeout > millis()) return;
  433.   taskStatusLED_timeout += taskStatusLED_period;
  434.  
  435.   taskLine();
  436.   if(line_state[0] == white && line_state[1] == white)  asuro.setStatusLED(GREEN);
  437.   else if ((line_state[0] == white && line_state[1] == black) || ((line_state[0] == black && line_state[1] == white))) asuro.setStatusLED(RED);
  438.   else asuro.setStatusLED(OFF);
  439. }
  440.  
  441. /*********** Init ***********/
  442. void hardwareInit() {
  443.   // delays for robustness
  444.   // using delays *before* the main loop is OK
  445.   asuro.Init();
  446.   delay(100);
  447.   Serial.begin(2400);
  448.   delay(100);
  449.   asuro.setFrontLED(ON);
  450.   delay(100);
  451.   Serial.println("Hardware ready");
  452.   delay(100);
  453. }
  454. /************************************************/
  455.  
  456. /*********** main ***********/
  457. void setup() {
  458.   hardwareInit();
  459.   transitionToFollowLine();
  460. }
  461. void loop() {
  462.   // main state machine
  463.   switch (mainState) {
  464.    
  465.     case followLine:
  466.       taskStatusLED();
  467.       taskReadLineSensor();
  468.       taskFollowLine();
  469.       break;
  470.      
  471.     case searchLine:
  472.       taskStatusLED();
  473.       taskReadLineSensor();
  474.       taskEncoder();
  475.       taskSearchLine();
  476.       break;
  477.      
  478.     case scanBarcode:
  479.       taskStatusLED();
  480.       taskReadLineSensor();
  481.       taskEncoder();
  482.       taskScanBarcode();
  483.       break;
  484.      
  485.     case blinkNTimes:
  486.       taskBlinkNTimes();
  487.       break;
  488.      
  489.     case findLine:
  490.       taskStatusLED();
  491.       taskReadLineSensor();
  492.       taskFindLine();
  493.       break;
  494.      
  495.     case stop:
  496.       taskStatusLED();
  497.       taskStop();
  498.     }
  499. }
  500. /************************************************/
  501.