Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed MODSERIAL telemetry-master
Fork of coolcarsuperfast by
main.cpp@19:d9746cc2ec80, 2015-04-28 (annotated)
- Committer:
- mawk2311
- Date:
- Tue Apr 28 17:13:33 2015 +0000
- Revision:
- 19:d9746cc2ec80
- Parent:
- 18:7941524e0d28
- Child:
- 22:21ec3ac3b8ea
- Child:
- 23:bf38d7d2255a
Added some of the values that need to be tuned due to varying lighting conditions as variables towards the top of the file. Also got rid of all of the code that we weren't using (old linetracking function, velocity control, encoder stuff, etc.)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mawk2311 | 0:ad375c052b4c | 1 | #include "mbed.h" |
mawk2311 | 0:ad375c052b4c | 2 | #include "stdlib.h" |
ericoneill | 7:6d5ddcf12cf3 | 3 | #include <vector> |
mawk2311 | 0:ad375c052b4c | 4 | //Outputs |
mawk2311 | 0:ad375c052b4c | 5 | DigitalOut led1(LED1); |
mawk2311 | 0:ad375c052b4c | 6 | DigitalOut clk(PTD5); |
mawk2311 | 0:ad375c052b4c | 7 | DigitalOut si(PTD4); |
mawk2311 | 5:20223464f7aa | 8 | PwmOut motor1(PTA12); |
mawk2311 | 5:20223464f7aa | 9 | PwmOut motor2(PTA4); |
mawk2311 | 5:20223464f7aa | 10 | DigitalOut break1(PTC7); |
mawk2311 | 5:20223464f7aa | 11 | DigitalOut break2(PTC0); |
mawk2311 | 0:ad375c052b4c | 12 | PwmOut servo(PTA5); |
mawk2311 | 0:ad375c052b4c | 13 | |
mawk2311 | 5:20223464f7aa | 14 | Serial pc(USBTX, USBRX); // tx, rx |
mawk2311 | 5:20223464f7aa | 15 | |
mawk2311 | 0:ad375c052b4c | 16 | //Inputs |
mawk2311 | 0:ad375c052b4c | 17 | AnalogIn camData(PTC2); |
mawk2311 | 0:ad375c052b4c | 18 | |
mawk2311 | 0:ad375c052b4c | 19 | //Encoder setup and variables |
mawk2311 | 0:ad375c052b4c | 20 | InterruptIn interrupt(PTA13); |
mawk2311 | 0:ad375c052b4c | 21 | |
mawk2311 | 0:ad375c052b4c | 22 | //Line Tracking Variables -------------------------------- |
mawk2311 | 0:ad375c052b4c | 23 | float ADCdata [128]; |
mawk2311 | 5:20223464f7aa | 24 | float maxAccum; |
mawk2311 | 5:20223464f7aa | 25 | float maxCount; |
mawk2311 | 0:ad375c052b4c | 26 | float approxPos; |
mawk2311 | 12:48b76450c4b4 | 27 | float prevApproxPos; |
mawk2311 | 15:55e9fffc653a | 28 | int trackWindow = 30; |
mawk2311 | 14:888495814f3c | 29 | int startWindow; |
mawk2311 | 14:888495814f3c | 30 | int endWindow; |
mawk2311 | 5:20223464f7aa | 31 | float maxVal; |
mawk2311 | 5:20223464f7aa | 32 | int maxLoc; |
mawk2311 | 12:48b76450c4b4 | 33 | |
mawk2311 | 12:48b76450c4b4 | 34 | bool firstTime; |
mawk2311 | 11:b59ec039a712 | 35 | |
mawk2311 | 10:e40ad924e935 | 36 | //Data Collection |
mawk2311 | 10:e40ad924e935 | 37 | bool dataCol = false; |
mawk2311 | 11:b59ec039a712 | 38 | int loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime; |
mawk2311 | 10:e40ad924e935 | 39 | |
mawk2311 | 5:20223464f7aa | 40 | //Line Crossing variables |
mawk2311 | 5:20223464f7aa | 41 | int prevTrackLoc; |
mawk2311 | 18:7941524e0d28 | 42 | int spaceThresh = 3; |
mawk2311 | 18:7941524e0d28 | 43 | int widthThresh = 10; |
mawk2311 | 5:20223464f7aa | 44 | bool space; |
mawk2311 | 0:ad375c052b4c | 45 | |
mawk2311 | 0:ad375c052b4c | 46 | //Servo turning parameters |
mawk2311 | 0:ad375c052b4c | 47 | float straight = 0.00155f; |
mawk2311 | 16:79106efd7a57 | 48 | float hardLeft = 0.0012f; |
mawk2311 | 16:79106efd7a57 | 49 | float hardRight = 0.0020f; |
mawk2311 | 12:48b76450c4b4 | 50 | //float hardLeft = 0.0010f; |
mawk2311 | 12:48b76450c4b4 | 51 | //float hardRight = 0.00195f; |
mawk2311 | 0:ad375c052b4c | 52 | |
mawk2311 | 5:20223464f7aa | 53 | //Servo Directions |
mawk2311 | 5:20223464f7aa | 54 | float currDir; |
mawk2311 | 5:20223464f7aa | 55 | float prevDir; |
mawk2311 | 5:20223464f7aa | 56 | |
ericoneill | 7:6d5ddcf12cf3 | 57 | // All linescan data for the period the car is running on. To be printed after a set amount of time |
ericoneill | 7:6d5ddcf12cf3 | 58 | //std::vector<std::vector<int> > frames; |
ericoneill | 7:6d5ddcf12cf3 | 59 | const int numData = 1000; |
ericoneill | 7:6d5ddcf12cf3 | 60 | int lineCenters [numData] = {0}; |
ericoneill | 7:6d5ddcf12cf3 | 61 | int times [numData] = {0}; |
ericoneill | 7:6d5ddcf12cf3 | 62 | int loopCtr = 0; |
ericoneill | 7:6d5ddcf12cf3 | 63 | |
mawk2311 | 0:ad375c052b4c | 64 | //End of Line Tracking Variables ------------------------- |
mawk2311 | 0:ad375c052b4c | 65 | |
mawk2311 | 0:ad375c052b4c | 66 | //Encoder and Motor Driver Variables --------------------- |
mawk2311 | 0:ad375c052b4c | 67 | |
mawk2311 | 0:ad375c052b4c | 68 | //Intervals used during encoder data collection to measure velocity |
mawk2311 | 0:ad375c052b4c | 69 | int interval1=0; |
mawk2311 | 0:ad375c052b4c | 70 | int interval2=0; |
mawk2311 | 0:ad375c052b4c | 71 | int interval3=0; |
mawk2311 | 0:ad375c052b4c | 72 | int avg_interval=0; |
mawk2311 | 0:ad375c052b4c | 73 | int lastchange1 = 0; |
mawk2311 | 0:ad375c052b4c | 74 | int lastchange2 = 0; |
mawk2311 | 0:ad375c052b4c | 75 | int lastchange3 = 0; |
mawk2311 | 0:ad375c052b4c | 76 | int lastchange4 = 0; |
mawk2311 | 0:ad375c052b4c | 77 | |
mawk2311 | 0:ad375c052b4c | 78 | //Variables used to for velocity control |
mawk2311 | 0:ad375c052b4c | 79 | float avg_speed = 0; |
mawk2311 | 5:20223464f7aa | 80 | float last_speed = 0; |
mawk2311 | 5:20223464f7aa | 81 | |
mawk2311 | 0:ad375c052b4c | 82 | float stall_check = 0; |
mawk2311 | 0:ad375c052b4c | 83 | float tuning_val = 1; |
mawk2311 | 0:ad375c052b4c | 84 | |
mawk2311 | 6:f1d948d2d6c1 | 85 | int numInterrupts = 0; |
mawk2311 | 5:20223464f7aa | 86 | |
mawk2311 | 19:d9746cc2ec80 | 87 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Tuning Values that can make it or break it ~~~~~~~~~~~~~~~~~~~~~~~~ |
mawk2311 | 19:d9746cc2ec80 | 88 | float pulsewidth = 0.14f; |
mawk2311 | 19:d9746cc2ec80 | 89 | int intTimMod = 0; |
mawk2311 | 19:d9746cc2ec80 | 90 | int maxValThresh = .1; //~.1 for earlier in the day, reduce it (maybe something like .05 - .01 or something) as it gets darker |
mawk2311 | 19:d9746cc2ec80 | 91 | bool turnSpeedControl = false; //have increased PWMs when turning when true. |
mawk2311 | 14:888495814f3c | 92 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
mawk2311 | 16:79106efd7a57 | 93 | |
mawk2311 | 5:20223464f7aa | 94 | // Hardware periods |
mawk2311 | 5:20223464f7aa | 95 | float motorPeriod = .0025; |
mawk2311 | 5:20223464f7aa | 96 | float servoPeriod = .0025; |
mawk2311 | 5:20223464f7aa | 97 | |
mawk2311 | 0:ad375c052b4c | 98 | Timer t; |
mawk2311 | 0:ad375c052b4c | 99 | Timer servoTimer; |
ericoneill | 7:6d5ddcf12cf3 | 100 | Timer printTimer; //after printTimer reaches a certain value the main loop will terminate and print the frames |
mawk2311 | 0:ad375c052b4c | 101 | |
mawk2311 | 0:ad375c052b4c | 102 | //End of Encoder and Motor Driver Variables ---------------------- |
mawk2311 | 0:ad375c052b4c | 103 | |
mawk2311 | 6:f1d948d2d6c1 | 104 | //Function for speeding up KL25Z ADC |
mawk2311 | 6:f1d948d2d6c1 | 105 | void initADC(void){ |
mawk2311 | 6:f1d948d2d6c1 | 106 | |
mawk2311 | 6:f1d948d2d6c1 | 107 | ADC0->CFG1 = ADC0->CFG1 & ( |
mawk2311 | 6:f1d948d2d6c1 | 108 | ~( |
mawk2311 | 6:f1d948d2d6c1 | 109 | 0x80 // LDLPC = 0 ; no low-power mode |
mawk2311 | 6:f1d948d2d6c1 | 110 | | 0x60 // ADIV = 1 |
mawk2311 | 6:f1d948d2d6c1 | 111 | | 0x10 // Sample time short |
mawk2311 | 6:f1d948d2d6c1 | 112 | | 0x03 // input clock = BUS CLK |
mawk2311 | 6:f1d948d2d6c1 | 113 | ) |
mawk2311 | 6:f1d948d2d6c1 | 114 | ) ; // clkdiv <= 1 |
mawk2311 | 6:f1d948d2d6c1 | 115 | ADC0->CFG2 = ADC0->CFG2 |
mawk2311 | 6:f1d948d2d6c1 | 116 | | 0x03 ; // Logsample Time 11 = 2 extra ADCK |
mawk2311 | 6:f1d948d2d6c1 | 117 | ADC0->SC3 = ADC0->SC3 |
mawk2311 | 6:f1d948d2d6c1 | 118 | & (~(0x03)) ; // hardware avarage off |
mawk2311 | 6:f1d948d2d6c1 | 119 | } |
mawk2311 | 6:f1d948d2d6c1 | 120 | |
mawk2311 | 0:ad375c052b4c | 121 | int main() { |
mawk2311 | 6:f1d948d2d6c1 | 122 | |
mawk2311 | 6:f1d948d2d6c1 | 123 | //Alter reg values to speed up KL25Z |
mawk2311 | 6:f1d948d2d6c1 | 124 | initADC(); |
mawk2311 | 6:f1d948d2d6c1 | 125 | |
mawk2311 | 0:ad375c052b4c | 126 | //Line Tracker Initializations |
mawk2311 | 0:ad375c052b4c | 127 | int integrationCounter = 0; |
mawk2311 | 0:ad375c052b4c | 128 | |
mawk2311 | 6:f1d948d2d6c1 | 129 | //Initial values for directions |
mawk2311 | 5:20223464f7aa | 130 | currDir = 0; |
mawk2311 | 5:20223464f7aa | 131 | prevDir = 0; |
mawk2311 | 5:20223464f7aa | 132 | |
mawk2311 | 0:ad375c052b4c | 133 | // Motor Driver Initializations |
mawk2311 | 5:20223464f7aa | 134 | motor1.period(motorPeriod); |
mawk2311 | 5:20223464f7aa | 135 | motor2.period(motorPeriod); |
mawk2311 | 0:ad375c052b4c | 136 | |
mawk2311 | 5:20223464f7aa | 137 | // Servo Initialization |
mawk2311 | 5:20223464f7aa | 138 | servo.period(servoPeriod); |
mawk2311 | 15:55e9fffc653a | 139 | servo.pulsewidth(hardRight); |
mawk2311 | 5:20223464f7aa | 140 | wait(3); |
mawk2311 | 0:ad375c052b4c | 141 | |
mawk2311 | 12:48b76450c4b4 | 142 | motor1.pulsewidth(motorPeriod*pulsewidth); |
mawk2311 | 12:48b76450c4b4 | 143 | motor2.pulsewidth(motorPeriod*pulsewidth); |
mawk2311 | 0:ad375c052b4c | 144 | break1 = 0; |
mawk2311 | 0:ad375c052b4c | 145 | break2 = 0; |
mawk2311 | 0:ad375c052b4c | 146 | |
mawk2311 | 12:48b76450c4b4 | 147 | firstTime = true; |
mawk2311 | 12:48b76450c4b4 | 148 | |
mawk2311 | 10:e40ad924e935 | 149 | //t.start(); |
mawk2311 | 10:e40ad924e935 | 150 | |
mawk2311 | 10:e40ad924e935 | 151 | if(dataCol){ |
mawk2311 | 11:b59ec039a712 | 152 | loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime = 0; |
mawk2311 | 10:e40ad924e935 | 153 | printTimer.start(); |
mawk2311 | 10:e40ad924e935 | 154 | } |
ericoneill | 8:e126c900c89d | 155 | |
mawk2311 | 0:ad375c052b4c | 156 | while(1) { |
mawk2311 | 10:e40ad924e935 | 157 | if(dataCol){ |
mawk2311 | 10:e40ad924e935 | 158 | //break out of main loop if enough time has passed; |
mawk2311 | 10:e40ad924e935 | 159 | if(loopCtr >= numData && dataCol){ |
mawk2311 | 10:e40ad924e935 | 160 | break; |
mawk2311 | 10:e40ad924e935 | 161 | } |
ericoneill | 7:6d5ddcf12cf3 | 162 | } |
mawk2311 | 0:ad375c052b4c | 163 | if(integrationCounter % 151== 0){ |
mawk2311 | 10:e40ad924e935 | 164 | /* |
mawk2311 | 0:ad375c052b4c | 165 | //Disable interrupts |
mawk2311 | 5:20223464f7aa | 166 | interrupt.fall(NULL); |
mawk2311 | 5:20223464f7aa | 167 | interrupt.rise(NULL); |
ericoneill | 7:6d5ddcf12cf3 | 168 | */ |
mawk2311 | 10:e40ad924e935 | 169 | |
mawk2311 | 0:ad375c052b4c | 170 | //Send start of integration signal |
mawk2311 | 0:ad375c052b4c | 171 | si = 1; |
mawk2311 | 0:ad375c052b4c | 172 | clk = 1; |
mawk2311 | 0:ad375c052b4c | 173 | |
mawk2311 | 0:ad375c052b4c | 174 | si = 0; |
mawk2311 | 0:ad375c052b4c | 175 | clk = 0; |
mawk2311 | 0:ad375c052b4c | 176 | |
mawk2311 | 0:ad375c052b4c | 177 | //Reset timing counter for integration |
mawk2311 | 0:ad375c052b4c | 178 | integrationCounter = 0; |
mawk2311 | 0:ad375c052b4c | 179 | |
mawk2311 | 0:ad375c052b4c | 180 | //Reset line tracking variables |
mawk2311 | 5:20223464f7aa | 181 | maxAccum = 0; |
mawk2311 | 5:20223464f7aa | 182 | maxCount = 0; |
mawk2311 | 0:ad375c052b4c | 183 | approxPos = 0; |
mawk2311 | 0:ad375c052b4c | 184 | |
mawk2311 | 5:20223464f7aa | 185 | space = false; |
mawk2311 | 6:f1d948d2d6c1 | 186 | |
mawk2311 | 0:ad375c052b4c | 187 | } |
mawk2311 | 0:ad375c052b4c | 188 | else if (integrationCounter > 129){ |
mawk2311 | 5:20223464f7aa | 189 | //Start Timer |
mawk2311 | 6:f1d948d2d6c1 | 190 | //t.start(); |
mawk2311 | 0:ad375c052b4c | 191 | |
mawk2311 | 5:20223464f7aa | 192 | //Enable interrupts |
mawk2311 | 6:f1d948d2d6c1 | 193 | //interrupt.fall(&fallInterrupt); |
mawk2311 | 6:f1d948d2d6c1 | 194 | //interrupt.rise(&riseInterrupt); |
mawk2311 | 5:20223464f7aa | 195 | |
mawk2311 | 12:48b76450c4b4 | 196 | if (firstTime){ |
mawk2311 | 12:48b76450c4b4 | 197 | |
mawk2311 | 12:48b76450c4b4 | 198 | maxVal = ADCdata[10]; |
mawk2311 | 12:48b76450c4b4 | 199 | for (int c = 11; c < 118; c++) { |
mawk2311 | 12:48b76450c4b4 | 200 | if (ADCdata[c] > maxVal){ |
mawk2311 | 12:48b76450c4b4 | 201 | maxVal = ADCdata[c]; |
mawk2311 | 12:48b76450c4b4 | 202 | maxLoc = c; |
mawk2311 | 12:48b76450c4b4 | 203 | } |
mawk2311 | 0:ad375c052b4c | 204 | } |
mawk2311 | 12:48b76450c4b4 | 205 | |
mawk2311 | 12:48b76450c4b4 | 206 | for (int c = 10; c < 118; c++) { |
mawk2311 | 19:d9746cc2ec80 | 207 | if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < maxValThresh){ |
mawk2311 | 12:48b76450c4b4 | 208 | maxAccum += c; |
mawk2311 | 12:48b76450c4b4 | 209 | maxCount++; |
mawk2311 | 15:55e9fffc653a | 210 | if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){ |
mawk2311 | 12:48b76450c4b4 | 211 | space = true; |
mawk2311 | 12:48b76450c4b4 | 212 | } |
mawk2311 | 12:48b76450c4b4 | 213 | prevTrackLoc = c; |
mawk2311 | 12:48b76450c4b4 | 214 | } |
mawk2311 | 12:48b76450c4b4 | 215 | } |
mawk2311 | 12:48b76450c4b4 | 216 | |
mawk2311 | 16:79106efd7a57 | 217 | //firstTime = false; |
mawk2311 | 12:48b76450c4b4 | 218 | } else { |
mawk2311 | 12:48b76450c4b4 | 219 | |
mawk2311 | 14:888495814f3c | 220 | startWindow = prevApproxPos - trackWindow; |
mawk2311 | 14:888495814f3c | 221 | endWindow = prevApproxPos + trackWindow; |
mawk2311 | 14:888495814f3c | 222 | if (startWindow < 0){ |
mawk2311 | 14:888495814f3c | 223 | startWindow = 0; |
mawk2311 | 14:888495814f3c | 224 | } |
mawk2311 | 14:888495814f3c | 225 | if (endWindow > 118){ |
mawk2311 | 14:888495814f3c | 226 | endWindow = 118; |
mawk2311 | 14:888495814f3c | 227 | } |
mawk2311 | 12:48b76450c4b4 | 228 | maxVal = ADCdata[10]; |
mawk2311 | 14:888495814f3c | 229 | for (int c = startWindow; c < endWindow; c++) { |
mawk2311 | 12:48b76450c4b4 | 230 | if (ADCdata[c] > maxVal){ |
mawk2311 | 12:48b76450c4b4 | 231 | maxVal = ADCdata[c]; |
mawk2311 | 12:48b76450c4b4 | 232 | maxLoc = c; |
mawk2311 | 12:48b76450c4b4 | 233 | } |
mawk2311 | 12:48b76450c4b4 | 234 | } |
mawk2311 | 12:48b76450c4b4 | 235 | |
mawk2311 | 14:888495814f3c | 236 | for (int c = startWindow; c < endWindow; c++) { |
mawk2311 | 19:d9746cc2ec80 | 237 | if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < maxValThresh){ |
mawk2311 | 11:b59ec039a712 | 238 | maxAccum += c; |
mawk2311 | 11:b59ec039a712 | 239 | maxCount++; |
mawk2311 | 15:55e9fffc653a | 240 | if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){ |
mawk2311 | 11:b59ec039a712 | 241 | space = true; |
mawk2311 | 11:b59ec039a712 | 242 | } |
mawk2311 | 11:b59ec039a712 | 243 | prevTrackLoc = c; |
mawk2311 | 5:20223464f7aa | 244 | } |
mawk2311 | 12:48b76450c4b4 | 245 | } |
mawk2311 | 11:b59ec039a712 | 246 | } |
mawk2311 | 12:48b76450c4b4 | 247 | /* |
mawk2311 | 11:b59ec039a712 | 248 | //Check if we need to alter integration time due to brightness |
mawk2311 | 11:b59ec039a712 | 249 | if (maxVal < 0.15f){ |
mawk2311 | 11:b59ec039a712 | 250 | intTimMod += 10; |
mawk2311 | 11:b59ec039a712 | 251 | } else if (maxVal >= 1) { |
mawk2311 | 11:b59ec039a712 | 252 | if (intTimMod > 0) { |
mawk2311 | 11:b59ec039a712 | 253 | intTimMod -= 10; |
mawk2311 | 0:ad375c052b4c | 254 | } |
mawk2311 | 0:ad375c052b4c | 255 | } |
mawk2311 | 12:48b76450c4b4 | 256 | */ |
mawk2311 | 11:b59ec039a712 | 257 | |
mawk2311 | 6:f1d948d2d6c1 | 258 | //Line Crossing Checks |
mawk2311 | 10:e40ad924e935 | 259 | if (space) { |
mawk2311 | 5:20223464f7aa | 260 | currDir = prevDir; |
mawk2311 | 15:55e9fffc653a | 261 | firstTime = true; |
mawk2311 | 5:20223464f7aa | 262 | } else { |
mawk2311 | 15:55e9fffc653a | 263 | approxPos = (float)maxAccum/(float)maxCount; |
ericoneill | 8:e126c900c89d | 264 | |
mawk2311 | 10:e40ad924e935 | 265 | if(dataCol){ |
mawk2311 | 10:e40ad924e935 | 266 | if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%3==0){ |
mawk2311 | 10:e40ad924e935 | 267 | lineCenters[loopCtr] = approxPos; |
mawk2311 | 10:e40ad924e935 | 268 | times[loopCtr] = printTimer.read_ms(); |
mawk2311 | 10:e40ad924e935 | 269 | loopCtr++; |
mawk2311 | 10:e40ad924e935 | 270 | } |
ericoneill | 9:ad08181ad1cc | 271 | } |
mawk2311 | 10:e40ad924e935 | 272 | |
mawk2311 | 16:79106efd7a57 | 273 | currDir = hardLeft + (approxPos)/((float) 118)*(hardRight-hardLeft); |
mawk2311 | 12:48b76450c4b4 | 274 | prevApproxPos = approxPos; |
ericoneill | 9:ad08181ad1cc | 275 | |
mawk2311 | 5:20223464f7aa | 276 | } |
mawk2311 | 16:79106efd7a57 | 277 | |
mawk2311 | 16:79106efd7a57 | 278 | if (turnSpeedControl){ |
mawk2311 | 16:79106efd7a57 | 279 | //Change speed when turning at different angles |
mawk2311 | 16:79106efd7a57 | 280 | if(approxPos > 0 && approxPos <= 45){ |
mawk2311 | 19:d9746cc2ec80 | 281 | motor1.pulsewidth(motorPeriod*(pulsewidth*2.0f)); |
mawk2311 | 19:d9746cc2ec80 | 282 | motor2.pulsewidth(motorPeriod*(pulsewidth*2.0f)); |
mawk2311 | 19:d9746cc2ec80 | 283 | } else if (approxPos > 45 && approxPos <= 55){ |
mawk2311 | 19:d9746cc2ec80 | 284 | motor1.pulsewidth(motorPeriod*pulsewidth*1.5f); |
mawk2311 | 19:d9746cc2ec80 | 285 | motor2.pulsewidth(motorPeriod*pulsewidth*1.5f); |
mawk2311 | 19:d9746cc2ec80 | 286 | } else if (approxPos > 55 && approxPos <= 85){ |
mawk2311 | 16:79106efd7a57 | 287 | motor1.pulsewidth(motorPeriod*pulsewidth); |
mawk2311 | 16:79106efd7a57 | 288 | motor2.pulsewidth(motorPeriod*pulsewidth); |
mawk2311 | 19:d9746cc2ec80 | 289 | } else if (approxPos > 85 && approxPos <= 95){ |
mawk2311 | 19:d9746cc2ec80 | 290 | motor1.pulsewidth(motorPeriod*pulsewidth*1.5f); |
mawk2311 | 19:d9746cc2ec80 | 291 | motor2.pulsewidth(motorPeriod*pulsewidth*1.5f); |
mawk2311 | 16:79106efd7a57 | 292 | } else { |
mawk2311 | 19:d9746cc2ec80 | 293 | motor1.pulsewidth(motorPeriod*(pulsewidth*2.0f)); |
mawk2311 | 19:d9746cc2ec80 | 294 | motor2.pulsewidth(motorPeriod*(pulsewidth*2.0f)); |
mawk2311 | 16:79106efd7a57 | 295 | } |
mawk2311 | 16:79106efd7a57 | 296 | } |
mawk2311 | 16:79106efd7a57 | 297 | |
mawk2311 | 5:20223464f7aa | 298 | servo.pulsewidth(currDir); |
mawk2311 | 5:20223464f7aa | 299 | |
mawk2311 | 6:f1d948d2d6c1 | 300 | //Start Velocity control after requisite number of encoder signals have been collected |
mawk2311 | 6:f1d948d2d6c1 | 301 | //if(numInterrupts >= 4){ |
mawk2311 | 6:f1d948d2d6c1 | 302 | //velocity_control(0.1f, TUNING_CONSTANT_10); |
mawk2311 | 6:f1d948d2d6c1 | 303 | //} |
mawk2311 | 4:09c68df71785 | 304 | |
mawk2311 | 6:f1d948d2d6c1 | 305 | //Save current direction as previous direction |
mawk2311 | 5:20223464f7aa | 306 | prevDir = currDir; |
mawk2311 | 0:ad375c052b4c | 307 | |
mawk2311 | 6:f1d948d2d6c1 | 308 | //Prepare to start collecting more data |
mawk2311 | 0:ad375c052b4c | 309 | integrationCounter = 150; |
mawk2311 | 6:f1d948d2d6c1 | 310 | |
mawk2311 | 6:f1d948d2d6c1 | 311 | //Disable interrupts |
mawk2311 | 6:f1d948d2d6c1 | 312 | //interrupt.fall(NULL); |
mawk2311 | 6:f1d948d2d6c1 | 313 | //interrupt.rise(NULL); |
mawk2311 | 6:f1d948d2d6c1 | 314 | |
mawk2311 | 6:f1d948d2d6c1 | 315 | //Stop timer |
mawk2311 | 6:f1d948d2d6c1 | 316 | //t.stop(); |
mawk2311 | 0:ad375c052b4c | 317 | } |
mawk2311 | 0:ad375c052b4c | 318 | else{ |
mawk2311 | 0:ad375c052b4c | 319 | clk = 1; |
mawk2311 | 14:888495814f3c | 320 | wait_us(intTimMod); |
mawk2311 | 3:e867c4e984df | 321 | ADCdata[integrationCounter - 1] = camData; |
mawk2311 | 0:ad375c052b4c | 322 | clk = 0; |
mawk2311 | 0:ad375c052b4c | 323 | } |
mawk2311 | 0:ad375c052b4c | 324 | |
mawk2311 | 0:ad375c052b4c | 325 | //clk = 0; |
mawk2311 | 0:ad375c052b4c | 326 | integrationCounter++; |
mawk2311 | 10:e40ad924e935 | 327 | if(dataCol){ |
mawk2311 | 10:e40ad924e935 | 328 | loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime++; |
mawk2311 | 10:e40ad924e935 | 329 | } |
mawk2311 | 0:ad375c052b4c | 330 | //camData. |
mawk2311 | 0:ad375c052b4c | 331 | |
mawk2311 | 0:ad375c052b4c | 332 | } |
mawk2311 | 10:e40ad924e935 | 333 | if (dataCol){ |
mawk2311 | 10:e40ad924e935 | 334 | //print frame data |
mawk2311 | 10:e40ad924e935 | 335 | pc.printf("printing frame data\n\r"); |
mawk2311 | 10:e40ad924e935 | 336 | //int frameSize = frames.size(); |
mawk2311 | 10:e40ad924e935 | 337 | //pc.printf("%i",frameSize); |
mawk2311 | 10:e40ad924e935 | 338 | pc.printf("["); |
mawk2311 | 10:e40ad924e935 | 339 | for(int i=0; i<numData; i++){ |
mawk2311 | 10:e40ad924e935 | 340 | if(lineCenters > 0){ |
mawk2311 | 10:e40ad924e935 | 341 | pc.printf("%i %i,",lineCenters[i], times[i]); |
mawk2311 | 10:e40ad924e935 | 342 | } |
ericoneill | 7:6d5ddcf12cf3 | 343 | } |
mawk2311 | 10:e40ad924e935 | 344 | pc.printf("]\n\r"); |
ericoneill | 7:6d5ddcf12cf3 | 345 | } |
mawk2311 | 0:ad375c052b4c | 346 | } |
ericoneill | 9:ad08181ad1cc | 347 |