Kacper Lukowiak / Mbed 2 deprecated 2018_NXP_car_Kacper

NXP Car Kacper

Dependencies:   FRDM-TFC-KACPER mbed

Fork of 2017_NXP_car by Zhengguo Sheng

main.cpp@1:5d2d5520f01f, 2018-04-30 (annotated)

Committer:
evenix
Date:
Mon Apr 30 18:35:46 2018 +0000
Revision:
1:5d2d5520f01f
Parent:
0:0de05459de46 
NXP car 2018

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MaxEllery 0:0de05459de46 1 #include "mbed.h"
MaxEllery 0:0de05459de46 2 #include "TFC.h"
MaxEllery 0:0de05459de46 3
evenix 1:5d2d5520f01f 4 #define MidPointT 63
evenix 1:5d2d5520f01f 5
evenix 1:5d2d5520f01f 6 float ServoTurn;
evenix 1:5d2d5520f01f 7 int LineThreshold1;
evenix 1:5d2d5520f01f 8 int CA[128];
evenix 1:5d2d5520f01f 9
evenix 1:5d2d5520f01f 10 //Button Debouncer Engine
evenix 1:5d2d5520f01f 11 int ButtonPressed1 = 0;
evenix 1:5d2d5520f01f 12 int ButtonNotPressed1 = 0;
evenix 1:5d2d5520f01f 13 bool Button1 = false;
evenix 1:5d2d5520f01f 14 bool ButtonCounter1 = false;
evenix 1:5d2d5520f01f 15
evenix 1:5d2d5520f01f 16 //Engine ON/OFF
evenix 1:5d2d5520f01f 17 bool Engine = false;
evenix 1:5d2d5520f01f 18
evenix 1:5d2d5520f01f 19 //Button Debouncer Servo
evenix 1:5d2d5520f01f 20 int ButtonPressed0 = 0;
evenix 1:5d2d5520f01f 21 int ButtonNotPressed0 = 0;
evenix 1:5d2d5520f01f 22 bool Button0 = false;
evenix 1:5d2d5520f01f 23 bool ButtonCounter0 = false;
evenix 1:5d2d5520f01f 24
evenix 1:5d2d5520f01f 25 //Servo ON/OFF
evenix 1:5d2d5520f01f 26 bool Servo = false;
evenix 1:5d2d5520f01f 27
evenix 1:5d2d5520f01f 28 //USB connection
evenix 1:5d2d5520f01f 29 bool usb;
evenix 1:5d2d5520f01f 30
evenix 1:5d2d5520f01f 31 //Mid Point Memory
evenix 1:5d2d5520f01f 32 int MidPointMemory [50];
evenix 1:5d2d5520f01f 33 int MemoryIndex = 0;
MaxEllery 0:0de05459de46 34
evenix 1:5d2d5520f01f 35 int MidPointMemoryDifference;
evenix 1:5d2d5520f01f 36
evenix 1:5d2d5520f01f 37 //Tick Counter
evenix 1:5d2d5520f01f 38 int Tick;
evenix 1:5d2d5520f01f 39 int TickMemory;
evenix 1:5d2d5520f01f 40 bool ReadTick = true;
evenix 1:5d2d5520f01f 41 int TickDifference;
evenix 1:5d2d5520f01f 42
evenix 1:5d2d5520f01f 43 //Initial Case
evenix 1:5d2d5520f01f 44 int Switch_Position = 11;
evenix 1:5d2d5520f01f 45
evenix 1:5d2d5520f01f 46 //PD
evenix 1:5d2d5520f01f 47 int Derivative = 0;
evenix 1:5d2d5520f01f 48 int LastError;
evenix 1:5d2d5520f01f 49 float Kp = 0.9;
evenix 1:5d2d5520f01f 50 float Kd = 0.1;
evenix 1:5d2d5520f01f 51
evenix 1:5d2d5520f01f 52 //Automatic Threshold
evenix 1:5d2d5520f01f 53 bool AutomaticThreshold;
evenix 1:5d2d5520f01f 54 int HighValues;
evenix 1:5d2d5520f01f 55 int LowValues;
evenix 1:5d2d5520f01f 56 int Constant = -100;
evenix 1:5d2d5520f01f 57
evenix 1:5d2d5520f01f 58
evenix 1:5d2d5520f01f 59 //Functions
evenix 1:5d2d5520f01f 60 void SwitchToCase11()
evenix 1:5d2d5520f01f 61 {
evenix 1:5d2d5520f01f 62 ReadTick = true;
evenix 1:5d2d5520f01f 63 TickDifference = 0;
evenix 1:5d2d5520f01f 64 TFC_BAT_LED2_OFF;
evenix 1:5d2d5520f01f 65 Switch_Position = 11;
evenix 1:5d2d5520f01f 66 }
evenix 1:5d2d5520f01f 67
MaxEllery 0:0de05459de46 68 int main() {
MaxEllery 0:0de05459de46 69 TFC_Init();
evenix 1:5d2d5520f01f 70 char CameraArrayChar[128] = {' '};
evenix 1:5d2d5520f01f 71 int LeftBorder = 0;
evenix 1:5d2d5520f01f 72 int RightBorder = 0;
evenix 1:5d2d5520f01f 73 int MidPoint = 0;
MaxEllery 0:0de05459de46 74 uint32_t i = 0;
evenix 1:5d2d5520f01f 75 uint32_t j = 0;
evenix 1:5d2d5520f01f 76 float Error= 0;
evenix 1:5d2d5520f01f 77
evenix 1:5d2d5520f01f 78
MaxEllery 0:0de05459de46 79
evenix 1:5d2d5520f01f 80 for(;;)
evenix 1:5d2d5520f01f 81 {
evenix 1:5d2d5520f01f 82 //read values from the potentiometers
MaxEllery 0:0de05459de46 83 float ReadPot1 = TFC_ReadPot(1);
evenix 1:5d2d5520f01f 84 float ReadPotRange = TFC_ReadPotRange(0);
evenix 1:5d2d5520f01f 85
evenix 1:5d2d5520f01f 86 //Manual Read Threshold Value
evenix 1:5d2d5520f01f 87 if (TFC_DIP_SWITCH_1_ON)
evenix 1:5d2d5520f01f 88 {
evenix 1:5d2d5520f01f 89 AutomaticThreshold = false;
evenix 1:5d2d5520f01f 90 }
evenix 1:5d2d5520f01f 91 else //Automatic Dynamic Threshold Search
evenix 1:5d2d5520f01f 92 {
evenix 1:5d2d5520f01f 93 AutomaticThreshold = true;
evenix 1:5d2d5520f01f 94 }
evenix 1:5d2d5520f01f 95
evenix 1:5d2d5520f01f 96 //Button Debouncer Servo
evenix 1:5d2d5520f01f 97 if(TFC_PUSH_BUTTON_0_PRESSED & Button0 == false)
evenix 1:5d2d5520f01f 98 { //increment when button is pressed
evenix 1:5d2d5520f01f 99 ButtonPressed0++;
evenix 1:5d2d5520f01f 100 if (ButtonPressed0 == 4000)
evenix 1:5d2d5520f01f 101 {
evenix 1:5d2d5520f01f 102 Button0 = true; //flag that button is pressed
evenix 1:5d2d5520f01f 103 ButtonPressed0 = 0; //reset the counter
evenix 1:5d2d5520f01f 104 }
evenix 1:5d2d5520f01f 105 }
evenix 1:5d2d5520f01f 106
evenix 1:5d2d5520f01f 107 if (Button0 == true)
evenix 1:5d2d5520f01f 108 {
evenix 1:5d2d5520f01f 109 ButtonNotPressed0++; //increment when button is not pressed
evenix 1:5d2d5520f01f 110 if (ButtonNotPressed0 == 4000)
evenix 1:5d2d5520f01f 111 {
evenix 1:5d2d5520f01f 112 //ButtonCounter0 diffrentiates if the servo should be turned on or off
evenix 1:5d2d5520f01f 113 if (ButtonCounter0 == false)
evenix 1:5d2d5520f01f 114 {
evenix 1:5d2d5520f01f 115 TFC_BAT_LED3_ON; //turn the LED on
evenix 1:5d2d5520f01f 116 Servo = true; //turn the servo on
evenix 1:5d2d5520f01f 117 ButtonCounter0 = true; //reset the flag (servo is on)
evenix 1:5d2d5520f01f 118 }
evenix 1:5d2d5520f01f 119 else
evenix 1:5d2d5520f01f 120 {
evenix 1:5d2d5520f01f 121 TFC_BAT_LED3_OFF; //turn the LED off
evenix 1:5d2d5520f01f 122 Servo = false; //turn the servo off
evenix 1:5d2d5520f01f 123 ButtonCounter0 = false; //reset the flag (servo is off)
evenix 1:5d2d5520f01f 124 }
evenix 1:5d2d5520f01f 125 Button0 = false; //resets the button press flag
evenix 1:5d2d5520f01f 126 ButtonNotPressed0 = 0; //reset the counter (button not pressed)
evenix 1:5d2d5520f01f 127 }
evenix 1:5d2d5520f01f 128 }
evenix 1:5d2d5520f01f 129
evenix 1:5d2d5520f01f 130 ///Button Debouncer Engine
evenix 1:5d2d5520f01f 131 if(TFC_PUSH_BUTTON_1_PRESSED & Button1 == false)
evenix 1:5d2d5520f01f 132 {
evenix 1:5d2d5520f01f 133 ButtonPressed1++;
evenix 1:5d2d5520f01f 134 if (ButtonPressed1 == 4000)
evenix 1:5d2d5520f01f 135 {
evenix 1:5d2d5520f01f 136 Button1 = true;
evenix 1:5d2d5520f01f 137 ButtonPressed1 = 0;
evenix 1:5d2d5520f01f 138 }
evenix 1:5d2d5520f01f 139 }
evenix 1:5d2d5520f01f 140
evenix 1:5d2d5520f01f 141 if (Button1 == true)
evenix 1:5d2d5520f01f 142 {
evenix 1:5d2d5520f01f 143 ButtonNotPressed1++;
evenix 1:5d2d5520f01f 144 if (ButtonNotPressed1 == 4000)
evenix 1:5d2d5520f01f 145 {
evenix 1:5d2d5520f01f 146 if (ButtonCounter1 == false)
evenix 1:5d2d5520f01f 147 {
evenix 1:5d2d5520f01f 148 TFC_BAT_LED0_ON;
evenix 1:5d2d5520f01f 149 TFC_HBRIDGE_ENABLE; //Enable H-Bridge
evenix 1:5d2d5520f01f 150 Engine = true;
evenix 1:5d2d5520f01f 151 ButtonCounter1 = true;
evenix 1:5d2d5520f01f 152 }
evenix 1:5d2d5520f01f 153 else
evenix 1:5d2d5520f01f 154 {
evenix 1:5d2d5520f01f 155 TFC_BAT_LED0_OFF;
evenix 1:5d2d5520f01f 156 TFC_HBRIDGE_DISABLE; //Disable H-Bridge
evenix 1:5d2d5520f01f 157 Engine = false;
evenix 1:5d2d5520f01f 158 ButtonCounter1 = false;
evenix 1:5d2d5520f01f 159 }
evenix 1:5d2d5520f01f 160 Button1 = false;
evenix 1:5d2d5520f01f 161 ButtonNotPressed1 = 0;
evenix 1:5d2d5520f01f 162 }
evenix 1:5d2d5520f01f 163 }
evenix 1:5d2d5520f01f 164
evenix 1:5d2d5520f01f 165 //USB Connection
evenix 1:5d2d5520f01f 166 if (TFC_DIP_SWITCH_0_ON)
evenix 1:5d2d5520f01f 167 {
evenix 1:5d2d5520f01f 168 usb = true;
evenix 1:5d2d5520f01f 169 }
evenix 1:5d2d5520f01f 170 else
evenix 1:5d2d5520f01f 171 {
evenix 1:5d2d5520f01f 172 usb = false;
evenix 1:5d2d5520f01f 173 }
evenix 1:5d2d5520f01f 174
evenix 1:5d2d5520f01f 175 if (usb == true)
evenix 1:5d2d5520f01f 176 {
evenix 1:5d2d5520f01f 177 Serial pc(USBTX, USBRX);
evenix 1:5d2d5520f01f 178 }
evenix 1:5d2d5520f01f 179
MaxEllery 0:0de05459de46 180
MaxEllery 0:0de05459de46 181 switch(Switch_Position)
MaxEllery 0:0de05459de46 182 {
MaxEllery 0:0de05459de46 183 default:
evenix 1:5d2d5520f01f 184
evenix 1:5d2d5520f01f 185
evenix 1:5d2d5520f01f 186 case 11:
evenix 1:5d2d5520f01f 187
evenix 1:5d2d5520f01f 188 //Set the speed of the motors using a potentiometer
evenix 1:5d2d5520f01f 189 TFC_SetMotorPWM(ReadPot1,ReadPot1);
MaxEllery 0:0de05459de46 190
evenix 1:5d2d5520f01f 191 if (TFC_LineScanImageReady > 0) //check if flag is non-zero
evenix 1:5d2d5520f01f 192 {
evenix 1:5d2d5520f01f 193 TFC_LineScanImageReady = 0; //reset the flag back to zero
evenix 1:5d2d5520f01f 194 //pc.printf("Threshold = %d.\r\n", LineThreshold1);
evenix 1:5d2d5520f01f 195
evenix 1:5d2d5520f01f 196 if (AutomaticThreshold == true) //enable adaptive thresholding
evenix 1:5d2d5520f01f 197 {
MaxEllery 0:0de05459de46 198 for(i = 0; i < 128; i++)
MaxEllery 0:0de05459de46 199 {
evenix 1:5d2d5520f01f 200 //store the output values in the array CA => CameraArray
evenix 1:5d2d5520f01f 201 CA[i] = ((int)TFC_LineScanImage0[i]);
MaxEllery 0:0de05459de46 202 }
evenix 1:5d2d5520f01f 203
evenix 1:5d2d5520f01f 204 //Segregate the array in the ascending order
evenix 1:5d2d5520f01f 205 for (i = 0; i < 128; i++)
MaxEllery 0:0de05459de46 206 {
evenix 1:5d2d5520f01f 207 for (j = i + 1; j < 128; j++)
evenix 1:5d2d5520f01f 208 {
evenix 1:5d2d5520f01f 209 if (CA[i] > CA[j])
evenix 1:5d2d5520f01f 210 {
evenix 1:5d2d5520f01f 211 int a = CA[i];
evenix 1:5d2d5520f01f 212 CA[i] = CA[j];
evenix 1:5d2d5520f01f 213 CA[j] = a;
evenix 1:5d2d5520f01f 214 }
MaxEllery 0:0de05459de46 215 }
MaxEllery 0:0de05459de46 216 }
evenix 1:5d2d5520f01f 217 //Calculate threshold using smallest and largest numbers (ignoring the extreme ones)
evenix 1:5d2d5520f01f 218 LowValues = ((CA[7] + CA[8] + CA[9] + CA[10] + CA[11])/5);
evenix 1:5d2d5520f01f 219 HighValues = ((CA[120] + CA[119] + CA[118] + CA[117] + CA[116])/5);
evenix 1:5d2d5520f01f 220 LineThreshold1 = ((LowValues + HighValues)/2) + Constant;
evenix 1:5d2d5520f01f 221 }
evenix 1:5d2d5520f01f 222 else //use manual thresholding
evenix 1:5d2d5520f01f 223 {
evenix 1:5d2d5520f01f 224 LineThreshold1 = (ReadPotRange);
evenix 1:5d2d5520f01f 225 }
evenix 1:5d2d5520f01f 226
evenix 1:5d2d5520f01f 227 //BORDER SEARCH
evenix 1:5d2d5520f01f 228 for(i = 0; i < 128; i++)
evenix 1:5d2d5520f01f 229 {
evenix 1:5d2d5520f01f 230 //check if the ADC values from the camera are higher than the threshold
evenix 1:5d2d5520f01f 231 if ((int)TFC_LineScanImage0[i] >= LineThreshold1)
evenix 1:5d2d5520f01f 232 {
evenix 1:5d2d5520f01f 233 //if the value is higher, put the empty space in the string
evenix 1:5d2d5520f01f 234 CameraArrayChar[i] = 219; //white
evenix 1:5d2d5520f01f 235 }
evenix 1:5d2d5520f01f 236 else
evenix 1:5d2d5520f01f 237 //otherwise put a black square
evenix 1:5d2d5520f01f 238 {
evenix 1:5d2d5520f01f 239 CameraArrayChar[i] = ' '; //black
evenix 1:5d2d5520f01f 240 }
evenix 1:5d2d5520f01f 241
evenix 1:5d2d5520f01f 242
evenix 1:5d2d5520f01f 243
evenix 1:5d2d5520f01f 244 if (usb == true)
evenix 1:5d2d5520f01f 245 {
evenix 1:5d2d5520f01f 246 printf("%c", CameraArrayChar[i]);
evenix 1:5d2d5520f01f 247 }
evenix 1:5d2d5520f01f 248 }
evenix 1:5d2d5520f01f 249 }
evenix 1:5d2d5520f01f 250
evenix 1:5d2d5520f01f 251
evenix 1:5d2d5520f01f 252 for(i = 63; i > 0; i--) //check from middle to the left
evenix 1:5d2d5520f01f 253 {
evenix 1:5d2d5520f01f 254 if (CameraArrayChar[i] == ' ') //if there's a black square (line)
evenix 1:5d2d5520f01f 255 {
evenix 1:5d2d5520f01f 256 LeftBorder = i; //save it as a left border
evenix 1:5d2d5520f01f 257 break;
evenix 1:5d2d5520f01f 258 }
evenix 1:5d2d5520f01f 259 else LeftBorder = 0; //if black square is not found, left border is equal to 0
evenix 1:5d2d5520f01f 260 }
evenix 1:5d2d5520f01f 261
evenix 1:5d2d5520f01f 262 for(i = 65; i < 128; i++) //check from middle to the right
evenix 1:5d2d5520f01f 263 {
evenix 1:5d2d5520f01f 264 if (CameraArrayChar[i] == ' ') //if there's a black square (line)
evenix 1:5d2d5520f01f 265 {
evenix 1:5d2d5520f01f 266 RightBorder = i; //save it as right border
evenix 1:5d2d5520f01f 267 break;
evenix 1:5d2d5520f01f 268 }
evenix 1:5d2d5520f01f 269 else RightBorder = 127; //if there's no black, save it as 127
evenix 1:5d2d5520f01f 270 }
evenix 1:5d2d5520f01f 271 //calculate mid point
evenix 1:5d2d5520f01f 272 MidPoint = (int)(LeftBorder + (RightBorder - LeftBorder)/2 );
evenix 1:5d2d5520f01f 273
evenix 1:5d2d5520f01f 274 //Mid Point Memory
evenix 1:5d2d5520f01f 275 if (MemoryIndex <= 49) //after start-up begin by putting the midpoint values
evenix 1:5d2d5520f01f 276 //into an empty array [MidPointMemory]
evenix 1:5d2d5520f01f 277 {
evenix 1:5d2d5520f01f 278 MidPointMemory[MemoryIndex] = MidPoint;
evenix 1:5d2d5520f01f 279 MemoryIndex++;
evenix 1:5d2d5520f01f 280 }
evenix 1:5d2d5520f01f 281
evenix 1:5d2d5520f01f 282 if (MemoryIndex == 50) //when the array is full, get rid of the oldest value,
evenix 1:5d2d5520f01f 283 { // move all of the new ones by one address
evenix 1:5d2d5520f01f 284 for (int k = 49; k >= 0; k--)
evenix 1:5d2d5520f01f 285 {
evenix 1:5d2d5520f01f 286 MidPointMemory[k]=MidPointMemory[k-1];
evenix 1:5d2d5520f01f 287 }
evenix 1:5d2d5520f01f 288 //and put a new one in the "empty" address
evenix 1:5d2d5520f01f 289 MidPointMemory[0] = MidPoint;
evenix 1:5d2d5520f01f 290 }
evenix 1:5d2d5520f01f 291
evenix 1:5d2d5520f01f 292
evenix 1:5d2d5520f01f 293 if (CameraArrayChar[63] == ' ' & Servo == true)
evenix 1:5d2d5520f01f 294 {
evenix 1:5d2d5520f01f 295 Switch_Position = 12;
evenix 1:5d2d5520f01f 296 }
evenix 1:5d2d5520f01f 297
evenix 1:5d2d5520f01f 298 //PD / P - enable/disable
evenix 1:5d2d5520f01f 299 if (TFC_DIP_SWITCH_2_ON)
evenix 1:5d2d5520f01f 300 {
evenix 1:5d2d5520f01f 301 TFC_BAT_LED1_ON;
evenix 1:5d2d5520f01f 302 LastError = Error;
evenix 1:5d2d5520f01f 303 Error = (MidPointT-MidPoint);
evenix 1:5d2d5520f01f 304
evenix 1:5d2d5520f01f 305 if (Engine == true)
evenix 1:5d2d5520f01f 306 {
evenix 1:5d2d5520f01f 307 Derivative = Error - LastError;
evenix 1:5d2d5520f01f 308 }
evenix 1:5d2d5520f01f 309
evenix 1:5d2d5520f01f 310 ServoTurn = ((Kp * Error) + (Kd * Derivative))/-20;
evenix 1:5d2d5520f01f 311
evenix 1:5d2d5520f01f 312 }
evenix 1:5d2d5520f01f 313 else
evenix 1:5d2d5520f01f 314 {
evenix 1:5d2d5520f01f 315 TFC_BAT_LED1_OFF;
evenix 1:5d2d5520f01f 316 Error = (MidPointT-MidPoint);
evenix 1:5d2d5520f01f 317 ServoTurn = (Error/-20);
evenix 1:5d2d5520f01f 318 }
evenix 1:5d2d5520f01f 319
evenix 1:5d2d5520f01f 320 if (ServoTurn > 1)
evenix 1:5d2d5520f01f 321 {
evenix 1:5d2d5520f01f 322 ServoTurn = 1;
evenix 1:5d2d5520f01f 323 }
evenix 1:5d2d5520f01f 324 else if (ServoTurn < -1)
evenix 1:5d2d5520f01f 325 {
evenix 1:5d2d5520f01f 326 ServoTurn = -1;
evenix 1:5d2d5520f01f 327 }
evenix 1:5d2d5520f01f 328
evenix 1:5d2d5520f01f 329 if (usb == true)
evenix 1:5d2d5520f01f 330 {
evenix 1:5d2d5520f01f 331
evenix 1:5d2d5520f01f 332 printf("Left Border: %d ", LeftBorder);
evenix 1:5d2d5520f01f 333 printf("Right Border: %d ", RightBorder);
evenix 1:5d2d5520f01f 334 printf("Mid Point: %d ", MidPoint);
evenix 1:5d2d5520f01f 335 printf("Error: %f ", Error);
evenix 1:5d2d5520f01f 336 printf("Derivative: %d ", Derivative);
evenix 1:5d2d5520f01f 337 printf("ServoTurn: %f ", ServoTurn);
evenix 1:5d2d5520f01f 338 printf("Threshold = %d.\r\n", LineThreshold1);
evenix 1:5d2d5520f01f 339 /*
evenix 1:5d2d5520f01f 340 for(int i = 0; i < 50; i++)
evenix 1:5d2d5520f01f 341 {
evenix 1:5d2d5520f01f 342 printf("%d ", MidPointMemory[i]);
evenix 1:5d2d5520f01f 343 }
evenix 1:5d2d5520f01f 344 */
evenix 1:5d2d5520f01f 345 }
evenix 1:5d2d5520f01f 346 //Servo Enable
evenix 1:5d2d5520f01f 347 if (Servo == true)
evenix 1:5d2d5520f01f 348 {
evenix 1:5d2d5520f01f 349 TFC_SetServo(0,ServoTurn);
evenix 1:5d2d5520f01f 350 }
evenix 1:5d2d5520f01f 351 else
evenix 1:5d2d5520f01f 352 {
evenix 1:5d2d5520f01f 353 TFC_SetServo(0,0);
evenix 1:5d2d5520f01f 354 }
evenix 1:5d2d5520f01f 355
MaxEllery 0:0de05459de46 356
evenix 1:5d2d5520f01f 357 Tick++;
evenix 1:5d2d5520f01f 358
MaxEllery 0:0de05459de46 359 break;
MaxEllery 0:0de05459de46 360
evenix 1:5d2d5520f01f 361
evenix 1:5d2d5520f01f 362 case 12:
evenix 1:5d2d5520f01f 363 if (usb == true)
evenix 1:5d2d5520f01f 364 {
evenix 1:5d2d5520f01f 365 printf("State 12 ");
evenix 1:5d2d5520f01f 366 printf("ServoTurn: %f.\r\n", ServoTurn);
evenix 1:5d2d5520f01f 367 }
MaxEllery 0:0de05459de46 368
evenix 1:5d2d5520f01f 369 TFC_BAT_LED2_ON; //turn LED2 on
evenix 1:5d2d5520f01f 370 if (ReadTick == true) //start measuring time
evenix 1:5d2d5520f01f 371 {
evenix 1:5d2d5520f01f 372 TickMemory = Tick;
evenix 1:5d2d5520f01f 373 ReadTick = false;
evenix 1:5d2d5520f01f 374 }
evenix 1:5d2d5520f01f 375
evenix 1:5d2d5520f01f 376 //slow down and turn left
evenix 1:5d2d5520f01f 377 if (MidPointMemory[15] <= 63) //read MidPoint from memory
evenix 1:5d2d5520f01f 378 //depending on the value of the remembered value, decide to turn left or right
evenix 1:5d2d5520f01f 379 {
evenix 1:5d2d5520f01f 380 TFC_SetMotorPWM(ReadPot1-0.1,ReadPot1-0.1); //slow down
evenix 1:5d2d5520f01f 381 ServoTurn = (-0.9); //turn left
evenix 1:5d2d5520f01f 382 }
evenix 1:5d2d5520f01f 383 //otherwise slow down and turn right
evenix 1:5d2d5520f01f 384 else if (MidPointMemory[15] > 63) //read MidPoint from memory
evenix 1:5d2d5520f01f 385 {
evenix 1:5d2d5520f01f 386 TFC_SetMotorPWM(ReadPot1-0.1,ReadPot1-0.1); //slow down
evenix 1:5d2d5520f01f 387 ServoTurn = (0.9); //turn right
evenix 1:5d2d5520f01f 388 }
evenix 1:5d2d5520f01f 389 TFC_SetServo(0,ServoTurn);
evenix 1:5d2d5520f01f 390 Tick++; //increment time measuring Tick
evenix 1:5d2d5520f01f 391 TickDifference = Tick - TickMemory; //calculate the time since the emergency algorithm
evenix 1:5d2d5520f01f 392 //start up
evenix 1:5d2d5520f01f 393 //Stop when time runs out
evenix 1:5d2d5520f01f 394 if (TickDifference == 10000)
evenix 1:5d2d5520f01f 395 {
evenix 1:5d2d5520f01f 396 SwitchToCase11();
evenix 1:5d2d5520f01f 397 }
evenix 1:5d2d5520f01f 398 break;
evenix 1:5d2d5520f01f 399
evenix 1:5d2d5520f01f 400 case 13:
evenix 1:5d2d5520f01f 401
evenix 1:5d2d5520f01f 402 break;
evenix 1:5d2d5520f01f 403
evenix 1:5d2d5520f01f 404 case 14:
evenix 1:5d2d5520f01f 405
evenix 1:5d2d5520f01f 406 break;
evenix 1:5d2d5520f01f 407
evenix 1:5d2d5520f01f 408 case 15:
evenix 1:5d2d5520f01f 409
evenix 1:5d2d5520f01f 410
MaxEllery 0:0de05459de46 411 break;
MaxEllery 0:0de05459de46 412 }
MaxEllery 0:0de05459de46 413 }
MaxEllery 0:0de05459de46 414 }