API tools faq
paste
Guest User

FreescaleCupMain

a guest
May 18th, 2015
32
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 45.80 KB | None | 0 0
raw download clone embed print report
  1. #include "mbed.h"
  2. #include "TFC.h"
  3. #define  DEBUG_ENABLED 0
  4. #define  DERIVATIVE_DEBUG_ENABLED 0
  5. #define  TAOS_CLK_LENGTH_DEBUG 0
  6.  
  7. // ############################################################################################################################
  8. // Turning parameters
  9. // ############################################################################################################################
  10. #define  SERVO_FACTOR                1.000                 // overall sensitivity of servo                               -- high value -> maximum servo
  11. #define  SERVO_DIFFERENCE            0.000                 // turn when  desired value differs more than this            -- low value=update more often
  12. #define  SERVO_TICKER_COUNT          0.000                 // wait time between adjusting the servo (10.0 = 20mS = minimum
  13. #define  DIFFERENTIAL_TURN_FACTOR    0.600                 // 0.0 - 1.0 -- how much slower the inside wheel should go?   -- high value=faster
  14. #define  MOTOR_REDUCTION_FACTOR      0.100                 // 0.0 - 1.0 -- how much slower should hard turns go?         -- high value=faster
  15. #define  MAX_TURN_SPEED              0.650                 // all speeds will turn at same rate unless coded differently -- high value=faster
  16. #define  MAX_SERVO                   0.70                  // maximum value
  17. #define  MAX_SERVO_NEGATIVE         -0.70                  // negative maximum value
  18. // ############################################################################################################################
  19. // Image parsing parameters
  20. // ############################################################################################################################
  21. #define  LIGHTING_CALIBRATION_TIME   200
  22. #define  TICKER_UPDATE_COUNT         200                   // was 2000 - 2mS
  23. #define  TOTAL_PIXELS               128                    // total pixels in image
  24. #define  PIXELS_TO_MASK               4                    // # pixels to disregard at each end of the image (at least 2!)
  25. #define  CENTER_LINE_WIDTH_MINIMUM    5                    // minimum pixel width for center line
  26. #define  CENTER_LINE_WIDTH_MAXIMUM    15                   // maximum pixel width for center line
  27. #define  FINISH_LINE_WIDTH_MINIMUM    30                   // minimum pixel width for finish line
  28. #define  FINISH_LINE_WIDTH_MAXIMUM    40                   // maximum pixel width for finish line
  29. #define  HYSTERESIS_START_FACTOR      6.00                 // 6x average of all derivatives - start counting as a line when >
  30. #define  HYSTERESIS_STOP_FACTOR       1.25                 // 1.5x average of all derivatives - stop counting as a line when <
  31. // ############################################################################################################################
  32. #define  MIDDLE_OF_TRACK             63.5                  // pixel count for the center of the track
  33. // ############################################################################################################################
  34. // for accelerometer code
  35. // ############################################################################################################################
  36. #define REG_WHO_AM_I         0x0D
  37. #define REG_CTRL_REG_1       0x2A
  38. #define REG_OUT_X_MSB        0x01
  39. #define REG_OUT_Y_MSB        0x03
  40. #define REG_OUT_Z_MSB        0x05
  41. #define UINT14_MAX           16383
  42. #define MMA8451_I2C_ADDRESS (0x1d<<1)
  43. #include "MMA8451Q.h"
  44. #include "MMA8451Q.hpp"
  45. // ############################################################################################################################
  46.  
  47. //This macro is to maintain compatibility with Codewarrior version of the sample.   This version uses the MBED libraries for serial port access
  48. Serial PC(USBTX,USBRX);
  49.  
  50. #define TERMINAL_PRINTF     PC.printf
  51.  
  52. //This ticker code is used to maintain compability with the Codewarrior version of the sample.   This code uses an MBED Ticker for background timing.
  53. #define NUM_TFC_TICKERS 4
  54.  
  55. Ticker TFC_TickerObj;
  56.  
  57. volatile uint32_t TFC_Ticker[NUM_TFC_TICKERS];
  58.  
  59. //accelerometer
  60. MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
  61.  
  62. bool                  findDerivative();
  63.  
  64. volatile float        centerLine=-1;
  65. volatile int          transitionCount=-1;
  66. volatile int          stopMarkerCount=-1;
  67. volatile int          dipSwitchValue=0;
  68. volatile float        straitawaySpeed=0.0;
  69. volatile float        turnSpeed=0.0;
  70. volatile float        speedMultiplier=0.0;
  71. volatile float        leftTurnMultiplier=0.0;
  72. volatile float        rightTurnMultiplier=0.0;
  73. volatile float        currentServoValue=0.0;
  74. volatile float        appliedServoValue=0.0;
  75. volatile bool         hbridgeEnabled=false;
  76. volatile bool         lineIdentified=false;
  77. volatile bool         finishMarkerIdentfied=false;
  78. volatile bool         lineFound=false;
  79. volatile uint16_t *   myCurrentImage;
  80. volatile float        tickerScale;
  81. volatile float        relativeOffsetFromCenter;
  82. volatile float        differentialSpeedAdjustment;
  83. volatile float        turnSpeedAdjustment;
  84. volatile bool         button_A_Pressed=false;
  85. volatile bool         button_B_Pressed=false;
  86. volatile float        maxThreshold;
  87. volatile float        minThreshold;
  88. volatile uint16_t     averageOfDerivatives = 0;
  89. extern uint8_t        LineScanWorkingBuffer;
  90. volatile uint8_t      TAOS_Pulse_Width=50;  //original value was 50
  91. volatile uint32_t     lightingCalibrationTime=0;
  92. volatile float        servoDifferenceFactor;
  93. volatile float        servoMultiplier;
  94.  
  95. //volatile float      accelerometerX;
  96. //volatile float      accelerometerY;
  97. //volatile float      accelerometerZ;
  98. //volatile int        flat=0; //flat=0, uphill=1, downhill=2
  99.  
  100. void TFC_TickerUpdate()
  101. {
  102.     int i;
  103.  
  104.     for(i=0; i<NUM_TFC_TICKERS; i++)
  105.      {
  106.         if(TFC_Ticker[i]<0xFFFFFFFF)
  107.         {
  108.             TFC_Ticker[i]++;
  109.         }
  110.     }
  111. }
  112.  
  113. int main()
  114. {
  115.     uint32_t i,j,t = 0;
  116.  
  117.     PC.baud(115200);
  118.    
  119.     tickerScale = 2000.0 / (float)TICKER_UPDATE_COUNT;
  120.    
  121.     TFC_TickerObj.attach_us(&TFC_TickerUpdate, TICKER_UPDATE_COUNT);
  122.    
  123.     TFC_Init();
  124.        
  125.     //all lights on to show calibration of lighting in progress
  126.     TFC_BAT_LED0_ON;
  127.     TFC_BAT_LED1_ON;
  128.     TFC_BAT_LED2_ON;
  129.     TFC_BAT_LED3_ON;
  130.  
  131. #if  DERIVATIVE_DEBUG_ENABLED
  132.    lightingCalibrationTime = LIGHTING_CALIBRATION_TIME;
  133. #endif
  134.     while (lightingCalibrationTime < LIGHTING_CALIBRATION_TIME) {
  135.        if (findDerivative()) {
  136.           lightingCalibrationTime++;
  137.           TFC_BAT_LED2_ON;
  138.        }
  139.     }
  140. #if  TAOS_CLK_LENGTH_DEBUG
  141.    TERMINAL_PRINTF("TAOS_Pulse_Width=%d\r\n", TAOS_Pulse_Width);
  142. #endif
  143.    
  144.     //all lights on to show calibration of lighting has completed
  145.     TFC_BAT_LED0_OFF;
  146.     TFC_BAT_LED1_OFF;
  147.     TFC_BAT_LED2_OFF;
  148.     TFC_BAT_LED3_OFF;
  149.  
  150.     while (!TFC_PUSH_BUTTON_0_PRESSED); //wait here button A is pressed.
  151.     while (TFC_PUSH_BUTTON_0_PRESSED);  //debounce button A.    
  152.    
  153.     // *******************************************
  154.     // * Set the speed multipliers if in race mode
  155.     // *******************************************
  156.     turnSpeed = MAX_TURN_SPEED;
  157.     switch(dipSwitchValue = TFC_GetDIP_Switch()) {
  158.        case 8:
  159.           straitawaySpeed = 0.700;
  160.           //turnSpeed = (MAX_TURN_SPEED > straitawaySpeed) ? straitawaySpeed :  MAX_TURN_SPEED;
  161.           if (hbridgeEnabled == false) {
  162.              TFC_SetMotorPWM(0,0); //Make sure motors are off
  163.              TFC_HBRIDGE_ENABLE;
  164.              hbridgeEnabled = true;
  165.           }
  166.           break;
  167.        case 9:
  168.           straitawaySpeed = 0.760;
  169.           //turnSpeed = (MAX_TURN_SPEED > straitawaySpeed) ? straitawaySpeed :  MAX_TURN_SPEED;
  170.           if (hbridgeEnabled == false) {
  171.              TFC_SetMotorPWM(0,0); //Make sure motors are off
  172.              TFC_HBRIDGE_ENABLE;
  173.              hbridgeEnabled = true;
  174.           }
  175.           break;
  176.        case 10:
  177.           straitawaySpeed = 0.800;
  178.           //turnSpeed = (MAX_TURN_SPEED > straitawaySpeed) ? straitawaySpeed :  MAX_TURN_SPEED;
  179.           if (hbridgeEnabled == false) {
  180.              TFC_SetMotorPWM(0,0); //Make sure motors are off
  181.              TFC_HBRIDGE_ENABLE;
  182.              hbridgeEnabled = true;
  183.           }
  184.           break;
  185.        case 11:
  186.           straitawaySpeed = 0.840;
  187.           //turnSpeed = (MAX_TURN_SPEED > straitawaySpeed) ? straitawaySpeed :  MAX_TURN_SPEED;
  188.           if (hbridgeEnabled == false) {
  189.              TFC_SetMotorPWM(0,0); //Make sure motors are off
  190.              TFC_HBRIDGE_ENABLE;
  191.              hbridgeEnabled = true;
  192.           }
  193.           break;
  194.        case 12:
  195.           straitawaySpeed = 0.880;
  196.           //turnSpeed = (MAX_TURN_SPEED > straitawaySpeed) ? straitawaySpeed :  MAX_TURN_SPEED;
  197.           if (hbridgeEnabled == false) {
  198.              TFC_SetMotorPWM(0,0); //Make sure motors are off
  199.              TFC_HBRIDGE_ENABLE;
  200.              hbridgeEnabled = true;
  201.           }
  202.           break;
  203.        case 13:
  204.           straitawaySpeed = 0.920;
  205.           //turnSpeed = (MAX_TURN_SPEED > straitawaySpeed) ? straitawaySpeed :  MAX_TURN_SPEED;
  206.           if (hbridgeEnabled == false) {
  207.              TFC_SetMotorPWM(0,0); //Make sure motors are off
  208.              TFC_HBRIDGE_ENABLE;
  209.              hbridgeEnabled = true;
  210.           }
  211.           break;
  212.       case 14:
  213.           straitawaySpeed = 0.960;
  214.           //turnSpeed = (MAX_TURN_SPEED > straitawaySpeed) ? straitawaySpeed :  MAX_TURN_SPEED;
  215.           if (hbridgeEnabled == false) {
  216.              TFC_SetMotorPWM(0,0); //Make sure motors are off
  217.              TFC_HBRIDGE_ENABLE;
  218.              hbridgeEnabled = true;
  219.           }
  220.           break;
  221.       case 15:
  222.           straitawaySpeed = 1.000;
  223.           //turnSpeed = (MAX_TURN_SPEED > straitawaySpeed) ? straitawaySpeed :  MAX_TURN_SPEED;
  224.           if (hbridgeEnabled == false) {
  225.              TFC_SetMotorPWM(0,0); //Make sure motors are off
  226.              TFC_HBRIDGE_ENABLE;
  227.              hbridgeEnabled = true;
  228.           }
  229.           break;
  230.       default:
  231.           straitawaySpeed = 0.0;
  232.           turnSpeed = 0.0;
  233.           leftTurnMultiplier = 0.0;
  234.           rightTurnMultiplier = 0.0;;
  235.           if (hbridgeEnabled == true) {
  236.              TFC_SetMotorPWM(0,0); //Make sure motors are off
  237.              TFC_HBRIDGE_DISABLE;
  238.              hbridgeEnabled = false;
  239.           }
  240.           break;
  241.     }
  242.  
  243.     while (1) {
  244.        if (TFC_PUSH_BUTTON_0_PRESSED) {  //use this button to start the state machine
  245.           button_A_Pressed = true;       //set the start flag
  246.           button_B_Pressed = false;      //clear the stop flag
  247.        }
  248.        if (TFC_PUSH_BUTTON_1_PRESSED) {  //use this button to reset the state machine
  249.           button_A_Pressed = false;      //clear the start flag
  250.           button_B_Pressed = true;       //set the stop flag
  251.        }
  252.  
  253.        if ((button_A_Pressed == false) || (button_B_Pressed == true)) {
  254.           if (TFC_Ticker[0] > (int)(SERVO_TICKER_COUNT * tickerScale)) { //20mS target when 10
  255.              TFC_SetServo(0.0);
  256.              TFC_Ticker[0] = 0;
  257.           }
  258.           TFC_SetMotorPWM(0,0); //Make sure motors are off
  259.           if (hbridgeEnabled == true) {
  260.              TFC_HBRIDGE_DISABLE;
  261.              hbridgeEnabled = false;
  262.           }
  263.        }
  264.        else {
  265.           //accelerometerX = abs(acc.getAccX());
  266.           //accelerometerY = abs(acc.getAccY());
  267.           //accelerometerZ = acc.getAccZ();
  268.           //if (accelerometerZ > 0.95) {
  269.           //   flat = 0;
  270.           //}
  271.           //else if ((flat < 2) && (accelerometerZ <= 0.95)) {
  272.           //   flat++;             //do nothing first 2 passes in case it is just bumps
  273.           //}
  274.           //else if ((flat >= 2) && (accelerometerZ <= 0.95)) {
  275.           //   //goose the motors here - going uphill
  276.           //   wait_ms(100);
  277.           //   //slow the motors here - going downhill
  278.           //   wait_ms(250);
  279.           //   flat = 0;  // expect to be going flat after going uphill and then downhill
  280.           //}
  281.           //else {
  282.           //   flat = 0;  // now going downhill
  283.           //}
  284. #if DEBUG_ENABLED
  285.           TERMINAL_PRINTF("%7.3f\r\n", accelerometerZ);
  286.           wait_ms(1000);
  287. #endif
  288.           if (hbridgeEnabled == false) {
  289.              TFC_HBRIDGE_ENABLE;
  290.              hbridgeEnabled = true;
  291.           }
  292.  
  293.           switch(dipSwitchValue) {
  294.             // **************************************************************
  295.             // * Demo mode 0 & unknown mode just tests the switches and LED's
  296.             // **************************************************************
  297.             default:
  298.             case 0:        
  299.                if (TFC_PUSH_BUTTON_0_PRESSED) {
  300.                   TFC_BAT_LED0_ON;
  301.                }
  302.                else {
  303.                   TFC_BAT_LED0_OFF;
  304.                }
  305.                
  306.                if (TFC_PUSH_BUTTON_1_PRESSED) {
  307.                   TFC_BAT_LED3_ON;
  308.                }
  309.                else {
  310.                   TFC_BAT_LED3_OFF;
  311.                }
  312.            
  313.                //turn these LEDs off for demo mode 0
  314.                TFC_BAT_LED1_OFF;
  315.                TFC_BAT_LED2_OFF;
  316.                break;
  317.  
  318.             // ************************************************************************
  319.             // * Demo mode 1 will just move the servos with the on-board potentiometers
  320.             // ************************************************************************  
  321.             case 1:
  322.                if (TFC_Ticker[0] > (int)(20.0 * tickerScale)) {
  323.                   TFC_Ticker[0] = 0; //reset the Ticker
  324.                
  325.                   //Every 20 mSeconds, update the Servos
  326.                   TFC_SetServo(TFC_ReadPot(0));
  327.                }
  328.                //Let's put a pattern on the LEDs
  329.                 if (TFC_Ticker[1] > (int)(125.0 * tickerScale)) {
  330.                    TFC_Ticker[1] = 0;
  331.                    t++;
  332.                    if (t>4) {
  333.                       t=0;
  334.                    }          
  335.                    TFC_SetBatteryLED_Level(t);
  336.                 }
  337.                 if (hbridgeEnabled == true) {
  338.                    TFC_SetMotorPWM(0,0); //Make sure motors are off
  339.                    TFC_HBRIDGE_DISABLE;
  340.                    hbridgeEnabled = false;
  341.                 }
  342.                 break;
  343.                
  344.             // *******************************************************
  345.             // * Demo Mode 2 will use the Pots to make the motors move
  346.             // *******************************************************  
  347.             case 2 :
  348.                if (hbridgeEnabled == false) {
  349.                   TFC_HBRIDGE_ENABLE;
  350.                   hbridgeEnabled = true;
  351.                }
  352.                TFC_SetMotorPWM(TFC_ReadPot(0),TFC_ReadPot(1));
  353.                //Let's put a pattern on the LEDs
  354.                 if (TFC_Ticker[1] > (int)(125.0 * tickerScale)) {
  355.                    TFC_Ticker[1] = 0;
  356.                    t++;
  357.                    if (t>4) {
  358.                       t=0;
  359.                    }          
  360.                    TFC_SetBatteryLED_Level(t);
  361.                 }
  362.                 break;
  363.            
  364.             // *************************************************************************************************************
  365.             // * Demo Mode 3 will be in Freescale Garage Mode.  It will beam data from the Camera to the Labview Application
  366.             // *************************************************************************************************************
  367.             case 3 :
  368.                if (TFC_LineScanImageReady) {          
  369.                
  370.                   TERMINAL_PRINTF("\r\n");
  371.                   TERMINAL_PRINTF("L:");
  372.                      
  373.                   //Let's put a pattern on the LEDs
  374.                   if (TFC_Ticker[1] > (int)(125.0 * tickerScale)) {
  375.                      TFC_Ticker[1] = 0;
  376.                      t++;
  377.                      if (t>4) {
  378.                         t=0;
  379.                      }          
  380.                   }
  381.                   // camera 1
  382.                   myCurrentImage = TFC_LineScanImage0;
  383.                   TFC_LineScanImageReady = 0; //only use images newer than the one I have now
  384.  
  385.                   for (i=0; i<8; i++) {
  386.                      for (j=0; j<16; j++) {
  387.                         TERMINAL_PRINTF("%04d",myCurrentImage[(i*16)+j]);
  388.                         if ((i==7) && (j==15)) {
  389.                            TERMINAL_PRINTF("\r\n");
  390.                         }
  391.                         else {
  392.                            TERMINAL_PRINTF(",");
  393.                         }
  394.                         wait_ms(10);
  395.                      }                            
  396.                   }
  397.                }
  398.                break;
  399.              
  400.             // *************************************************************************************************************
  401.             // * Demo Mode 4 will be in Freescale Garage Mode.  It counts the line transitions seen by the camera
  402.             // *************************************************************************************************************
  403.             case 4:
  404.                if (findDerivative()) {          
  405.                   // camera 1              
  406.                   switch (transitionCount) {
  407.                      case  0: TFC_BAT_LED1_OFF; TFC_BAT_LED0_OFF; break;
  408.                      case  1: TFC_BAT_LED1_OFF; TFC_BAT_LED0_ON;  break;
  409.                      case  2: TFC_BAT_LED1_ON;  TFC_BAT_LED0_OFF; break;
  410.                      case  3: TFC_BAT_LED1_ON;  TFC_BAT_LED0_ON;  break;
  411.                      default: TFC_BAT_LED1_OFF; TFC_BAT_LED0_OFF; break;
  412.                   }
  413.                }
  414.                break;
  415.              
  416.             // ***************************************************************************************
  417.             // * Race modes 8-15 will have the car naviagate at varioud speeds (8-slowest, 15-fastest)
  418.             // ***************************************************************************************          
  419.             case 8:
  420.             case 9:
  421.             case 10:
  422.             case 11:
  423.             case 12:
  424.             case 13:
  425.             case 14:
  426.             case 15:
  427.                if (findDerivative()) {  //Update the track position if a new image is available
  428. #if DEBUG_ENABLED    
  429.                   TERMINAL_PRINTF("centerLine=%4.1f, lineIdentified=%d, finishMarkerIdentfied=%d\n", centerLine, lineIdentified, finishMarkerIdentfied);
  430. #endif
  431.                   switch ((int)(centerLine + 0.5)) {
  432.                      case   0: case   1: case   2: case   3:  //hard right
  433.                      case   4: case   5: case   6: case   7:
  434.                      case   8: case   9: case  10: case  11:
  435.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR;
  436.                         leftTurnMultiplier  = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  437.                         rightTurnMultiplier = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  438.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  439.                         break;
  440.                      case  12: case  13: case  14: case  15:
  441.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR;
  442.                         leftTurnMultiplier  = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  443.                         rightTurnMultiplier = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  444.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  445.                         break;
  446.                      case  16: case  17: case  18: case  19:  // hard right
  447.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR;
  448.                         leftTurnMultiplier  = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  449.                         rightTurnMultiplier = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  450.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  451.                         break;
  452.                      case  20: case  21: case  22: case  23:
  453.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.95;
  454.                         leftTurnMultiplier  = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  455.                         rightTurnMultiplier = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  456.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  457.                         break;
  458.                      case  24: case  25: case  26: case  27:  // hard right
  459.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.90;
  460.                         leftTurnMultiplier  = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  461.                         rightTurnMultiplier = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  462.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  463.                         break;
  464.                      case  28: case  29: case  30: case  31:
  465.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.85;
  466.                         leftTurnMultiplier  = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  467.                         rightTurnMultiplier = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  468.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  469.                         break;
  470.                      case  32: case  33: case  34: case  35:  // hard right
  471.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.80;
  472.                         leftTurnMultiplier  = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  473.                         rightTurnMultiplier = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  474.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  475.                         break;
  476.                      case  36: case  37: case  38: case  39:  
  477.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.70;
  478.                         leftTurnMultiplier  = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  479.                         rightTurnMultiplier = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  480.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  481.                         break;
  482.                      case  40: case  41: case  42: case  43:  // medium right
  483.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.60;
  484.                         leftTurnMultiplier  = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  485.                         rightTurnMultiplier = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  486.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  487.                         break;
  488.                      case  44: case  45: case  46: case  47:
  489.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.50;
  490.                         leftTurnMultiplier  = (0.60 > (straitawaySpeed - 0.20)) ? 0.60 : (straitawaySpeed - 0.20);
  491.                         rightTurnMultiplier = (0.30 > (straitawaySpeed - 0.40)) ? 0.30 : (straitawaySpeed - 0.40);
  492.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  493.                         break;
  494.                      case  48: case  49: case  50: case  51:  // soft right
  495.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.40;
  496.                         leftTurnMultiplier  = (0.60 > (straitawaySpeed - 0.15)) ? 0.60 : (straitawaySpeed - 0.15);
  497.                         rightTurnMultiplier = (0.30 > (straitawaySpeed - 0.30)) ? 0.30 : (straitawaySpeed - 0.30);
  498.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  499.                         break;
  500.                      case  52: case  53: case  54: case  55:
  501.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.30;
  502.                         leftTurnMultiplier  = (0.60 > (straitawaySpeed - 0.10))? 0.60 : (straitawaySpeed - 0.010);
  503.                         rightTurnMultiplier = (0.30 > (straitawaySpeed - 0.20))? 0.30 : (straitawaySpeed - 0.20);
  504.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  505.                         break;
  506.                      case  56: case  57: case  58: case  59:  // very soft right
  507.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.20;
  508.                         leftTurnMultiplier  = (0.60 > (straitawaySpeed - 0.05))? 0.60 :(straitawaySpeed - 0.05);
  509.                         rightTurnMultiplier = (0.30 > (straitawaySpeed - 0.10))? 0.30 :(straitawaySpeed - 0.10);
  510.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  511.                         break;
  512.                      case  60: case  61: case  62:
  513.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.10;
  514.                         leftTurnMultiplier  = (0.60 > (straitawaySpeed - 0.025))? 0.60 : (straitawaySpeed - 0.025);
  515.                         rightTurnMultiplier = (0.30 > (straitawaySpeed - 0.05)) ? 0.30 : (straitawaySpeed - 0.05);
  516.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  517.                         break;
  518.                      default:
  519.                      case  63: case  64:
  520.                         currentServoValue = 0.0;
  521.                         rightTurnMultiplier = straitawaySpeed;
  522.                         leftTurnMultiplier  = straitawaySpeed;
  523.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  524.                         break;
  525.                      case  65: case  66:  case  67:
  526.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.10;
  527.                         leftTurnMultiplier  = (0.30 > (straitawaySpeed - 0.05)) ? 0.30 : (straitawaySpeed - 0.05);
  528.                         rightTurnMultiplier = (0.60 > (straitawaySpeed - 0.025))? 0.60 : (straitawaySpeed - 0.025);
  529.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  530.                         break;
  531.                      case  68: case  69: case  70: case  71:  // very soft left
  532.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.20;
  533.                         leftTurnMultiplier  = (0.30 > (straitawaySpeed - 0.10)) ? 0.30 : (straitawaySpeed - 0.10);
  534.                         rightTurnMultiplier = (0.60 > (straitawaySpeed - 0.05)) ? 0.60 : (straitawaySpeed - 0.05);
  535.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  536.                         break;
  537.                      case  72: case  73: case  74: case  75:
  538.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.30;
  539.                         leftTurnMultiplier  = (0.30 > (straitawaySpeed - 0.20)) ? 0.30 : (straitawaySpeed - 0.20);
  540.                         rightTurnMultiplier = (0.60 > (straitawaySpeed - 0.10)) ? 0.60 : (straitawaySpeed - 0.10);
  541.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  542.                         break;
  543.                      case  76: case  77: case  78: case  79:  // soft left
  544.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.40;
  545.                         leftTurnMultiplier  = (0.30 > (straitawaySpeed - 0.30)) ? 0.30 : (straitawaySpeed - 0.30);
  546.                         rightTurnMultiplier = (0.60 > (straitawaySpeed - 0.15)) ? 0.60 : (straitawaySpeed - 0.15);
  547.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  548.                         break;
  549.                      case  80: case  81: case  82: case  83:
  550.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.50;
  551.                         leftTurnMultiplier  = (0.30 > (straitawaySpeed - 0.40)) ? 0.30 : (straitawaySpeed - 0.40);
  552.                         rightTurnMultiplier = (0.60 > (straitawaySpeed - 0.20)) ? 0.60 : (straitawaySpeed - 0.20);
  553.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  554.                         break;
  555.                      case  84: case  85: case  86: case  87:  // medium left
  556.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.60;
  557.                         leftTurnMultiplier  = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  558.                         rightTurnMultiplier = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  559.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  560.                         break;
  561.                      case  88: case  89: case  90: case  91:
  562.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.70;
  563.                         leftTurnMultiplier  = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  564.                         rightTurnMultiplier = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  565.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  566.                         break;
  567.                      case  92: case  93: case  94: case  95:  // hard left
  568.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.80;
  569.                         leftTurnMultiplier  = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  570.                         rightTurnMultiplier = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  571.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  572.                         break;
  573.                      case  96: case  97: case  98: case  99:
  574.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.85;
  575.                         leftTurnMultiplier  = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  576.                         rightTurnMultiplier = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  577.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  578.                         break;
  579.                      case 100: case 101: case 102: case 103:  // hard left
  580.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.90;
  581.                         leftTurnMultiplier  = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  582.                         rightTurnMultiplier = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  583.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  584.                         break;
  585.                      case 104: case 105: case 106: case 107:
  586.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR * 0.95;
  587.                         leftTurnMultiplier  = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  588.                         rightTurnMultiplier = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  589.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  590.                         break;
  591.                      case 108: case 109: case 110: case 111:  // hard left
  592.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR;
  593.                         leftTurnMultiplier  = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  594.                         rightTurnMultiplier = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  595.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  596.                         break;
  597.                      case 112: case 113: case 114: case 115:
  598.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR;
  599.                         leftTurnMultiplier  = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  600.                         rightTurnMultiplier = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  601.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  602.                         break;
  603.                      case 116: case 117: case 118: case 119:  // hard left
  604.                      case 120: case 121: case 122: case 123:  // hard left
  605.                      case 124: case 125: case 126: case 127:
  606.                         currentServoValue = relativeOffsetFromCenter * MAX_SERVO * SERVO_FACTOR;
  607.                         leftTurnMultiplier  = (0.30 > (straitawaySpeed - 0.50)) ? 0.30 : (straitawaySpeed - 0.50);
  608.                         rightTurnMultiplier = (0.60 > (straitawaySpeed - 0.25)) ? 0.60 : (straitawaySpeed - 0.25);
  609.                         TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  610.                         break;
  611.                   }
  612. #if DEBUG_ENABLED    
  613.                   TERMINAL_PRINTF("currentServoValue=%f, speedMultiplier=%f, leftTurnMultiplier=%f, rightTurnMultiplier=%f\n", currentServoValue, speedMultiplier, leftTurnMultiplier, rightTurnMultiplier);
  614. #endif
  615.                }
  616.                
  617.                switch (transitionCount) {
  618.                   case  0: TFC_BAT_LED1_OFF; TFC_BAT_LED0_OFF; break;
  619.                   case  1: TFC_BAT_LED1_OFF; TFC_BAT_LED0_ON;  break;
  620.                   case  2: TFC_BAT_LED1_ON;  TFC_BAT_LED0_OFF; break;
  621.                   case  3: TFC_BAT_LED1_ON;  TFC_BAT_LED0_ON;  break;
  622.                   default: TFC_BAT_LED1_OFF; TFC_BAT_LED0_OFF; break;
  623.                }
  624.  
  625.                if ((lineIdentified == true) && (finishMarkerIdentfied == false) && TFC_ServoTicker) {
  626.                   appliedServoValue = currentServoValue;
  627.                   TFC_SetServo(appliedServoValue);
  628.                   TFC_ServoTicker = 0;
  629.                }
  630.                else if (finishMarkerIdentfied == true) {
  631.                   //shut everything down if multiple lines are found or if I'm lost
  632.                   TFC_SetServo(0.0);
  633.                   TFC_SetMotorPWM(0.0, 0.0); // crossing start/finish so stop
  634.                   TFC_HBRIDGE_DISABLE;
  635.                   hbridgeEnabled = false;
  636.                }
  637.                else if (lineIdentified == false) {
  638.                
  639.                   if (leftTurnMultiplier > 0.0001) {
  640.                      leftTurnMultiplier  -= 0.0001;
  641.                   }
  642.                   else if (leftTurnMultiplier < -0.0001) {
  643.                      leftTurnMultiplier  += 0.0001;
  644.                   }
  645.                   else {
  646.                      leftTurnMultiplier = 0.0;
  647.                   }
  648.                  
  649.                   if (rightTurnMultiplier > 0.0001) {
  650.                      rightTurnMultiplier  -= 0.0001;
  651.                   }
  652.                   else if (rightTurnMultiplier < -0.0001) {
  653.                      rightTurnMultiplier  += 0.001;
  654.                   }
  655.                   else {
  656.                      rightTurnMultiplier = 0.0;
  657.                   }
  658.                   TFC_SetMotorPWM(leftTurnMultiplier, (-1.0 * rightTurnMultiplier));
  659.                }
  660.                break;
  661.          }
  662.       }    
  663.    }
  664. }
  665.  
  666. bool findDerivative() { //use divided difference method to obtain approxmation of second derivative - accelaration of change
  667.    volatile uint8_t    i;
  668.    volatile uint8_t    transitionWidth;
  669.    volatile uint8_t    countOfDerivatives = 0;
  670.    volatile uint8_t    lineDetected[TOTAL_PIXELS];
  671.    volatile uint16_t   firstTransitionValue;
  672.    volatile uint16_t   secondTransitionValue;
  673.    volatile uint32_t   sumOfDerivatives = 0;
  674.    volatile uint16_t   startOfSecondTransition;
  675.    volatile uint16_t   derivativeValues[TOTAL_PIXELS];
  676.    volatile bool       firstTransitionFound;
  677.    volatile uint16_t * workingImagePtr;
  678.    volatile uint16_t   currentPixel;
  679.    volatile uint16_t * derivativeValuesPtr;
  680.    volatile uint8_t  * previousPreviousLineDetectedPtr;
  681.    volatile uint8_t  * previousLineDetectedPtr;
  682.    volatile uint8_t  * currentLineDetectedPtr;
  683.    volatile uint8_t  * nextLineDetectedPtr;
  684.    volatile uint8_t  * nextNextLineDetectedPtr;
  685.    volatile uint16_t * previousPixelPtr;
  686.    volatile uint16_t * currentPixelPtr;
  687.    volatile uint16_t * nextPixelPtr;
  688.    volatile uint16_t   maximumPixelValue;
  689.    volatile uint16_t   localImageBuffer[128];
  690.    volatile uint16_t * myLocalImage;
  691.    
  692.    if (!TFC_LineScanImageReady) {
  693.       lineFound = false;
  694.       transitionCount = stopMarkerCount = firstTransitionValue = secondTransitionValue = 0;
  695.       return(false);
  696.    }
  697.    
  698.    TFC_BAT_LED2_ON;
  699.    TFC_LineScanImageReady = 0;
  700.    
  701.    //make a copy of the image buffer before it is over-run
  702.    for (i=0, myCurrentImage=TFC_LineScanImage0, myLocalImage=localImageBuffer; i<128; i++, myLocalImage++, myCurrentImage++) {
  703.       *myLocalImage = *myCurrentImage;
  704.    }
  705.    myCurrentImage = localImageBuffer;
  706.    
  707.    transitionCount = stopMarkerCount = firstTransitionValue = secondTransitionValue = 0;
  708.    
  709.    //lower masked range now contains first valid pixel count
  710.    for (i=0, workingImagePtr=myCurrentImage, currentPixel=myCurrentImage[PIXELS_TO_MASK], derivativeValuesPtr=&(derivativeValues[0]), currentLineDetectedPtr=&(lineDetected[0]);
  711.         i<PIXELS_TO_MASK;
  712.         i++) {
  713.       *workingImagePtr++ = currentPixel; // tail values = first value of consequence
  714.       *derivativeValuesPtr++ = 0;
  715.       *currentLineDetectedPtr++ = 0;
  716.    }
  717.    
  718.    //upper masked range now contains last valid pixel count
  719.    for (i=(TOTAL_PIXELS-PIXELS_TO_MASK+1), workingImagePtr=&(myCurrentImage[TOTAL_PIXELS-PIXELS_TO_MASK+1]), currentPixel=myCurrentImage[TOTAL_PIXELS-PIXELS_TO_MASK], derivativeValuesPtr=&(derivativeValues[TOTAL_PIXELS-PIXELS_TO_MASK+1]), currentLineDetectedPtr=&(lineDetected[TOTAL_PIXELS-PIXELS_TO_MASK+1]);
  720.         i<PIXELS_TO_MASK;
  721.         i++) {
  722.       *workingImagePtr++ = currentPixel; // tail values = first value of consequence
  723.       *derivativeValuesPtr++ = 0;
  724.       *currentLineDetectedPtr++ = 0;
  725.    }
  726.  
  727.    if (lightingCalibrationTime < LIGHTING_CALIBRATION_TIME) {  //time to average the lighting
  728.       sumOfDerivatives = 0;
  729.       countOfDerivatives = 0;
  730.       maximumPixelValue = 0;
  731.       //look at i-1 & i+1 - first check will only determine thresholds
  732.       for (i=PIXELS_TO_MASK, lineFound=false, derivativeValuesPtr=&(derivativeValues[PIXELS_TO_MASK]), previousPixelPtr=&(myCurrentImage[PIXELS_TO_MASK-1]), currentPixelPtr=&(myCurrentImage[PIXELS_TO_MASK]), nextPixelPtr=&(myCurrentImage[PIXELS_TO_MASK+1]);
  733.            i<(TOTAL_PIXELS-PIXELS_TO_MASK);
  734.            i++, derivativeValuesPtr++, nextPixelPtr++, currentPixelPtr++, previousPixelPtr++) {
  735.          *derivativeValuesPtr = abs((int)((*nextPixelPtr + *currentPixelPtr)-(*previousPixelPtr << 1)));
  736.          sumOfDerivatives += *derivativeValuesPtr;
  737.          countOfDerivatives++;
  738.          if (*currentPixelPtr > maximumPixelValue) {
  739.             maximumPixelValue = *currentPixelPtr;    //find maximum pixel value to adjust camera exposure time
  740.          }
  741.       }
  742.       averageOfDerivatives = (sumOfDerivatives / countOfDerivatives);
  743.       maxThreshold = (HYSTERESIS_START_FACTOR * (float)averageOfDerivatives);
  744.       minThreshold = (HYSTERESIS_STOP_FACTOR * (float)averageOfDerivatives);
  745.      
  746.       if      ((maximumPixelValue > 4080) && (TAOS_Pulse_Width > 25)) {      //target an maximum pixel value of between 3500 & 4080 to set exposure time
  747.          TAOS_Pulse_Width--;
  748.       }
  749.       else if ((maximumPixelValue < 3500) && (TAOS_Pulse_Width < 250)) {     //target an maximum pixel value of between 3500 & 4080 to set exposure time
  750.          TAOS_Pulse_Width++;
  751.       }
  752.    }
  753.    
  754.    //look at i-1 & i+1 - subsequent checks will determine line placement
  755.    for (i=PIXELS_TO_MASK, lineFound=false, derivativeValuesPtr=&(derivativeValues[PIXELS_TO_MASK]), previousPixelPtr=&(myCurrentImage[PIXELS_TO_MASK-1]), currentPixelPtr=&(myCurrentImage[PIXELS_TO_MASK]), nextPixelPtr=&(myCurrentImage[PIXELS_TO_MASK+1]), currentLineDetectedPtr=&(lineDetected[PIXELS_TO_MASK]);
  756.         i<(TOTAL_PIXELS-PIXELS_TO_MASK);
  757.         i++, derivativeValuesPtr++, nextPixelPtr++, currentPixelPtr++, previousPixelPtr++, currentLineDetectedPtr++) {
  758.       *derivativeValuesPtr = abs((int)((*nextPixelPtr + *currentPixelPtr)-(*previousPixelPtr << 1)));
  759.       if (lineFound == false) {
  760.          if (*derivativeValuesPtr > maxThreshold) {
  761.             lineFound = true;
  762.             *currentLineDetectedPtr = 1;
  763.          }
  764.          else {
  765.             lineFound = false;
  766.             *currentLineDetectedPtr = 0;
  767.          }
  768.       }
  769.       else {
  770.          if (*derivativeValuesPtr > minThreshold) {
  771.             lineFound = true;
  772.             *currentLineDetectedPtr = 1;
  773.          }
  774.          else {
  775.             lineFound = false;
  776.             *currentLineDetectedPtr = 0;
  777.          }
  778.       }
  779.    }
  780.    
  781.    //join lines for glitch filtering when not in demo mode
  782.    for (i=PIXELS_TO_MASK,
  783.         previousPreviousLineDetectedPtr=&(lineDetected[PIXELS_TO_MASK-2]),
  784.         previousLineDetectedPtr=&(lineDetected[PIXELS_TO_MASK-1]),
  785.         currentLineDetectedPtr=&(lineDetected[PIXELS_TO_MASK]),
  786.         nextLineDetectedPtr=&(lineDetected[PIXELS_TO_MASK+1]),
  787.         nextNextLineDetectedPtr=&(lineDetected[PIXELS_TO_MASK+2]);
  788.         i<(TOTAL_PIXELS-PIXELS_TO_MASK);
  789.         i++,
  790.         previousPreviousLineDetectedPtr++,
  791.         previousLineDetectedPtr++,
  792.         currentLineDetectedPtr++,
  793.         nextLineDetectedPtr++,
  794.         nextNextLineDetectedPtr++) {
  795.       if (*previousLineDetectedPtr && *nextLineDetectedPtr) {
  796.          *currentLineDetectedPtr = 1;
  797.       }
  798.       if (*previousLineDetectedPtr && *nextNextLineDetectedPtr) {
  799.          *currentLineDetectedPtr = 1;
  800.       }
  801.       if (*previousPreviousLineDetectedPtr && *nextLineDetectedPtr) {
  802.          *currentLineDetectedPtr = 1;
  803.       }
  804.    }
  805.    
  806. #if DERIVATIVE_DEBUG_ENABLED
  807.    TERMINAL_PRINTF("%02d: ",LineScanWorkingBuffer);
  808.    for (i=0, currentLineDetectedPtr=&(lineDetected[0]); i<TOTAL_PIXELS; i++,currentLineDetectedPtr++) {
  809.       TERMINAL_PRINTF("%d",*currentLineDetectedPtr);
  810.    }
  811.    TERMINAL_PRINTF("\r\n");
  812.    wait_ms(100);
  813. #endif
  814.  
  815.    lineIdentified       = false;
  816.    firstTransitionFound = false;
  817.    //count the transitions found - should be at least 2
  818.    for (i=PIXELS_TO_MASK, currentLineDetectedPtr=&(lineDetected[PIXELS_TO_MASK]), previousLineDetectedPtr=&(lineDetected[PIXELS_TO_MASK-1]);
  819.         i<(TOTAL_PIXELS-PIXELS_TO_MASK);
  820.         i++, currentLineDetectedPtr++, previousLineDetectedPtr++) {
  821.       if (firstTransitionFound == false) {                                                  // still in the white area
  822.          if (!(*previousLineDetectedPtr) && *currentLineDetectedPtr){                       // transition starting
  823.             firstTransitionValue = i;
  824.             firstTransitionFound = true;                                                    // now look for falling edge
  825.          }
  826.       }
  827.       else {
  828.          if (!(*previousLineDetectedPtr) && *currentLineDetectedPtr){                       // transition starting
  829.             startOfSecondTransition = i;                                                    // start of the second tansition - save in case nothing
  830.             while ((i<TOTAL_PIXELS) && *currentLineDetectedPtr) {
  831.                i++;
  832.                currentLineDetectedPtr++;
  833.                previousLineDetectedPtr++;
  834.             }
  835.             secondTransitionValue = (i - 1);
  836.             transitionWidth = secondTransitionValue - firstTransitionValue;
  837.            
  838.             if ((transitionWidth >= CENTER_LINE_WIDTH_MINIMUM) &&                           // Am I looking at the line?
  839.                 (transitionWidth <= CENTER_LINE_WIDTH_MAXIMUM)) {
  840.                centerLine = (float)(firstTransitionValue + secondTransitionValue) / 2.0;
  841.                firstTransitionFound = false;
  842.                lineIdentified       = true;
  843.                transitionCount++;
  844.             }
  845.             else if ((transitionWidth >= FINISH_LINE_WIDTH_MINIMUM) &&                      // Am I looking at one of the start markers?
  846.                      (transitionWidth <= FINISH_LINE_WIDTH_MAXIMUM)) {
  847.                firstTransitionFound = false;
  848.                stopMarkerCount++;
  849.                transitionCount++;
  850.             }
  851.             else {                                                                          // Apparently looking at nothing important - shift the looking glass
  852.                firstTransitionValue = startOfSecondTransition;                                                
  853.             }
  854.          }
  855.       }
  856.    }
  857.    
  858.    if (stopMarkerCount == 2) {                            
  859.       finishMarkerIdentfied = true;                                                         // found the start/stop line
  860.    }
  861.    
  862.    if (lineIdentified) {
  863.       relativeOffsetFromCenter    = (MIDDLE_OF_TRACK - centerLine)/MIDDLE_OF_TRACK;
  864.       differentialSpeedAdjustment = abs(relativeOffsetFromCenter) * DIFFERENTIAL_TURN_FACTOR;
  865.       turnSpeedAdjustment         = abs(relativeOffsetFromCenter) * MOTOR_REDUCTION_FACTOR;
  866. #if DERIVATIVE_DEBUG_ENABLED
  867.       TERMINAL_PRINTF("relativeOffsetFromCenter=%7.3f, differentialSpeedAdjustment=%7.3f, turnSpeedAdjustment=%7.3f\r\n", relativeOffsetFromCenter, differentialSpeedAdjustment, turnSpeedAdjustment);
  868.       wait_ms(500);
  869. #endif
  870.    }
  871.    TFC_BAT_LED2_OFF;
  872.    return(true);
  873. }
Advertisement
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!