E=MC
/
e=mc
.
main.cpp@13:f7ec3f026634, 2015-04-23 (annotated)
- Committer:
- ericoneill
- Date:
- Thu Apr 23 23:24:16 2015 +0000
- Revision:
- 13:f7ec3f026634
- Parent:
- 8:f3e0b4814888
UPDATE
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ericoneill | 0:d328ecb3fbb1 | 1 | #include "mbed.h" |
ericoneill | 13:f7ec3f026634 | 2 | #include "stdlib.h" |
ericoneill | 13:f7ec3f026634 | 3 | #include <vector> |
ericoneill | 13:f7ec3f026634 | 4 | //Outputs |
ericoneill | 13:f7ec3f026634 | 5 | DigitalOut led1(LED1); |
ericoneill | 13:f7ec3f026634 | 6 | DigitalOut clk(PTD5); |
ericoneill | 13:f7ec3f026634 | 7 | DigitalOut si(PTD4); |
ericoneill | 13:f7ec3f026634 | 8 | PwmOut motor1(PTA12); |
ericoneill | 13:f7ec3f026634 | 9 | PwmOut motor2(PTA4); |
ericoneill | 13:f7ec3f026634 | 10 | DigitalOut break1(PTC7); |
ericoneill | 13:f7ec3f026634 | 11 | DigitalOut break2(PTC0); |
ericoneill | 13:f7ec3f026634 | 12 | PwmOut servo(PTA5); |
ericoneill | 0:d328ecb3fbb1 | 13 | |
ericoneill | 0:d328ecb3fbb1 | 14 | Serial pc(USBTX, USBRX); // tx, rx |
ericoneill | 3:7eaf505f811e | 15 | |
ericoneill | 13:f7ec3f026634 | 16 | //Inputs |
ericoneill | 13:f7ec3f026634 | 17 | AnalogIn camData(PTC2); |
ericoneill | 13:f7ec3f026634 | 18 | |
ericoneill | 13:f7ec3f026634 | 19 | //Encoder setup and variables |
ericoneill | 3:7eaf505f811e | 20 | InterruptIn interrupt(PTA13); |
ericoneill | 4:263bddc51c0f | 21 | |
ericoneill | 13:f7ec3f026634 | 22 | //Line Tracking Variables -------------------------------- |
ericoneill | 13:f7ec3f026634 | 23 | float ADCdata [128]; |
ericoneill | 13:f7ec3f026634 | 24 | float maxAccum; |
ericoneill | 13:f7ec3f026634 | 25 | float maxCount; |
ericoneill | 13:f7ec3f026634 | 26 | float approxPos; |
ericoneill | 13:f7ec3f026634 | 27 | float prevApproxPos; |
ericoneill | 13:f7ec3f026634 | 28 | int trackWindow = 30; |
ericoneill | 13:f7ec3f026634 | 29 | int startWindow; |
ericoneill | 13:f7ec3f026634 | 30 | int endWindow; |
ericoneill | 13:f7ec3f026634 | 31 | float maxVal; |
ericoneill | 13:f7ec3f026634 | 32 | int maxLoc; |
ericoneill | 13:f7ec3f026634 | 33 | |
ericoneill | 13:f7ec3f026634 | 34 | bool firstTime; |
ericoneill | 13:f7ec3f026634 | 35 | |
ericoneill | 13:f7ec3f026634 | 36 | //Brightness accommodater |
ericoneill | 13:f7ec3f026634 | 37 | |
ericoneill | 13:f7ec3f026634 | 38 | |
ericoneill | 13:f7ec3f026634 | 39 | //Data Collection |
ericoneill | 13:f7ec3f026634 | 40 | bool dataCol = true; |
ericoneill | 13:f7ec3f026634 | 41 | int loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime; |
ericoneill | 13:f7ec3f026634 | 42 | |
ericoneill | 13:f7ec3f026634 | 43 | //Line Crossing variables |
ericoneill | 13:f7ec3f026634 | 44 | int prevTrackLoc; |
ericoneill | 13:f7ec3f026634 | 45 | int spaceThresh = 1; |
ericoneill | 13:f7ec3f026634 | 46 | int widthThresh = 10; |
ericoneill | 13:f7ec3f026634 | 47 | bool space; |
ericoneill | 13:f7ec3f026634 | 48 | |
ericoneill | 13:f7ec3f026634 | 49 | //Servo turning parameters |
ericoneill | 13:f7ec3f026634 | 50 | float straight = 0.00155f; |
ericoneill | 13:f7ec3f026634 | 51 | float hardLeft = 0.0011f; |
ericoneill | 13:f7ec3f026634 | 52 | float hardRight = 0.0021f; |
ericoneill | 13:f7ec3f026634 | 53 | //float hardLeft = 0.0010f; |
ericoneill | 13:f7ec3f026634 | 54 | //float hardRight = 0.00195f; |
ericoneill | 13:f7ec3f026634 | 55 | |
ericoneill | 13:f7ec3f026634 | 56 | //Servo Directions |
ericoneill | 13:f7ec3f026634 | 57 | float currDir; |
ericoneill | 13:f7ec3f026634 | 58 | float prevDir; |
ericoneill | 13:f7ec3f026634 | 59 | |
ericoneill | 13:f7ec3f026634 | 60 | // All linescan data for the period the car is running on. To be printed after a set amount of time |
ericoneill | 13:f7ec3f026634 | 61 | //std::vector<std::vector<int> > frames; |
ericoneill | 13:f7ec3f026634 | 62 | const int numData = 500; |
ericoneill | 13:f7ec3f026634 | 63 | int lineCenters [numData] = {0}; |
ericoneill | 13:f7ec3f026634 | 64 | int times [numData] = {0}; |
ericoneill | 13:f7ec3f026634 | 65 | float pSteering [numData] = {0.0f}; |
ericoneill | 13:f7ec3f026634 | 66 | float pdSteering [numData] = {0.0f}; |
ericoneill | 13:f7ec3f026634 | 67 | int loopCtr = 0; |
ericoneill | 13:f7ec3f026634 | 68 | |
ericoneill | 13:f7ec3f026634 | 69 | //End of Line Tracking Variables ------------------------- |
ericoneill | 13:f7ec3f026634 | 70 | |
ericoneill | 13:f7ec3f026634 | 71 | //Encoder and Motor Driver Variables --------------------- |
ericoneill | 13:f7ec3f026634 | 72 | |
mawk2311 | 7:ea395616348c | 73 | //Intervals used during encoder data collection to measure velocity |
ericoneill | 4:263bddc51c0f | 74 | int interval1=0; |
ericoneill | 4:263bddc51c0f | 75 | int interval2=0; |
ericoneill | 4:263bddc51c0f | 76 | int interval3=0; |
ericoneill | 4:263bddc51c0f | 77 | int avg_interval=0; |
ericoneill | 4:263bddc51c0f | 78 | int lastchange1 = 0; |
ericoneill | 4:263bddc51c0f | 79 | int lastchange2 = 0; |
ericoneill | 4:263bddc51c0f | 80 | int lastchange3 = 0; |
ericoneill | 4:263bddc51c0f | 81 | int lastchange4 = 0; |
mawk2311 | 5:61a0a21134f7 | 82 | |
mawk2311 | 7:ea395616348c | 83 | //Variables used to for velocity control |
mawk2311 | 7:ea395616348c | 84 | float avg_speed = 0; |
ericoneill | 13:f7ec3f026634 | 85 | float last_speed = 0; |
ericoneill | 13:f7ec3f026634 | 86 | |
mawk2311 | 7:ea395616348c | 87 | float stall_check = 0; |
mawk2311 | 7:ea395616348c | 88 | float tuning_val = 1; |
mawk2311 | 5:61a0a21134f7 | 89 | |
ericoneill | 13:f7ec3f026634 | 90 | int numInterrupts = 0; |
ericoneill | 13:f7ec3f026634 | 91 | |
ericoneill | 13:f7ec3f026634 | 92 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~PWM & Integration Time ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
ericoneill | 13:f7ec3f026634 | 93 | float pulsewidth = 0.14f; |
ericoneill | 13:f7ec3f026634 | 94 | int intTimMod = 0; |
ericoneill | 13:f7ec3f026634 | 95 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
ericoneill | 13:f7ec3f026634 | 96 | // Hardware periods |
ericoneill | 13:f7ec3f026634 | 97 | float motorPeriod = .0025; |
ericoneill | 13:f7ec3f026634 | 98 | float servoPeriod = .0025; |
ericoneill | 13:f7ec3f026634 | 99 | |
ericoneill | 1:8e5821dec0f7 | 100 | Timer t; |
ericoneill | 8:f3e0b4814888 | 101 | Timer servoTimer; |
ericoneill | 13:f7ec3f026634 | 102 | Timer printTimer; //after printTimer reaches a certain value the main loop will terminate and print the frames |
mawk2311 | 7:ea395616348c | 103 | |
mawk2311 | 7:ea395616348c | 104 | //Observed average speeds for each duty cycle |
ericoneill | 13:f7ec3f026634 | 105 | |
ericoneill | 4:263bddc51c0f | 106 | const float PI = 3.14159; |
ericoneill | 4:263bddc51c0f | 107 | const float WHEEL_CIRCUMFERENCE = .05*PI; |
ericoneill | 0:d328ecb3fbb1 | 108 | |
mawk2311 | 7:ea395616348c | 109 | //Velocity Control Tuning Constants |
mawk2311 | 7:ea395616348c | 110 | const float TUNE_THRESH = 0.5f; |
mawk2311 | 7:ea395616348c | 111 | const float TUNE_AMT = 0.1f; |
mawk2311 | 7:ea395616348c | 112 | |
mawk2311 | 7:ea395616348c | 113 | //Parameters specifying sample sizes and delays for small and large average speed samples |
ericoneill | 13:f7ec3f026634 | 114 | float num_samples_small = 3.0f; |
mawk2311 | 7:ea395616348c | 115 | float delay_small = 0.05f; |
mawk2311 | 7:ea395616348c | 116 | float num_samples_large = 100.0f; |
mawk2311 | 7:ea395616348c | 117 | float delay_large = 0.1f; |
mawk2311 | 7:ea395616348c | 118 | |
ericoneill | 13:f7ec3f026634 | 119 | //Large and small arrays used to get average velocity values |
mawk2311 | 7:ea395616348c | 120 | float large_avg_speed_list [100]; |
mawk2311 | 7:ea395616348c | 121 | float small_avg_speed_list [10]; |
mawk2311 | 7:ea395616348c | 122 | |
ericoneill | 13:f7ec3f026634 | 123 | //End of Encoder and Motor Driver Variables ---------------------- |
ericoneill | 13:f7ec3f026634 | 124 | |
ericoneill | 13:f7ec3f026634 | 125 | //Line Tracker Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
ericoneill | 13:f7ec3f026634 | 126 | |
ericoneill | 13:f7ec3f026634 | 127 | //PIDcontrol ----------------------------------------------------- |
ericoneill | 13:f7ec3f026634 | 128 | int centerSum = 0; |
ericoneill | 13:f7ec3f026634 | 129 | int prevLineCenter = 0; |
ericoneill | 13:f7ec3f026634 | 130 | int prevTime = 0; |
ericoneill | 13:f7ec3f026634 | 131 | int p,i,d = 1; |
ericoneill | 13:f7ec3f026634 | 132 | float timeDiff = 0.0f; |
ericoneill | 13:f7ec3f026634 | 133 | float lastTime = 0.0f; |
ericoneill | 13:f7ec3f026634 | 134 | |
ericoneill | 13:f7ec3f026634 | 135 | //Function for speeding up KL25Z ADC |
ericoneill | 13:f7ec3f026634 | 136 | void initADC(void){ |
ericoneill | 13:f7ec3f026634 | 137 | |
ericoneill | 13:f7ec3f026634 | 138 | ADC0->CFG1 = ADC0->CFG1 & ( |
ericoneill | 13:f7ec3f026634 | 139 | ~( |
ericoneill | 13:f7ec3f026634 | 140 | 0x80 // LDLPC = 0 ; no low-power mode |
ericoneill | 13:f7ec3f026634 | 141 | | 0x60 // ADIV = 1 |
ericoneill | 13:f7ec3f026634 | 142 | | 0x10 // Sample time short |
ericoneill | 13:f7ec3f026634 | 143 | | 0x03 // input clock = BUS CLK |
ericoneill | 13:f7ec3f026634 | 144 | ) |
ericoneill | 13:f7ec3f026634 | 145 | ) ; // clkdiv <= 1 |
ericoneill | 13:f7ec3f026634 | 146 | ADC0->CFG2 = ADC0->CFG2 |
ericoneill | 13:f7ec3f026634 | 147 | | 0x03 ; // Logsample Time 11 = 2 extra ADCK |
ericoneill | 13:f7ec3f026634 | 148 | ADC0->SC3 = ADC0->SC3 |
ericoneill | 13:f7ec3f026634 | 149 | & (~(0x03)) ; // hardware avarage off |
ericoneill | 13:f7ec3f026634 | 150 | } |
ericoneill | 13:f7ec3f026634 | 151 | |
ericoneill | 13:f7ec3f026634 | 152 | |
ericoneill | 13:f7ec3f026634 | 153 | // Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
ericoneill | 13:f7ec3f026634 | 154 | |
ericoneill | 13:f7ec3f026634 | 155 | float PIDcontrol(int lineCenter){ |
ericoneill | 13:f7ec3f026634 | 156 | //centerSum += lineCenter; |
ericoneill | 13:f7ec3f026634 | 157 | float newTime = t.read(); |
ericoneill | 13:f7ec3f026634 | 158 | timeDiff = newTime - lastTime; |
ericoneill | 13:f7ec3f026634 | 159 | lastTime = newTime; |
ericoneill | 13:f7ec3f026634 | 160 | float lineDif = ((float)prevLineCenter - (float)lineCenter)/(float) timeDiff; |
ericoneill | 13:f7ec3f026634 | 161 | return p * lineCenter + d * lineDif; |
ericoneill | 13:f7ec3f026634 | 162 | |
ericoneill | 4:263bddc51c0f | 163 | } |
mawk2311 | 5:61a0a21134f7 | 164 | |
ericoneill | 13:f7ec3f026634 | 165 | int main() { |
ericoneill | 13:f7ec3f026634 | 166 | |
ericoneill | 13:f7ec3f026634 | 167 | //Alter reg values to speed up KL25Z |
ericoneill | 13:f7ec3f026634 | 168 | initADC(); |
mawk2311 | 5:61a0a21134f7 | 169 | |
ericoneill | 13:f7ec3f026634 | 170 | //Line Tracker Initializations |
ericoneill | 13:f7ec3f026634 | 171 | int integrationCounter = 0; |
ericoneill | 13:f7ec3f026634 | 172 | |
ericoneill | 13:f7ec3f026634 | 173 | //Initial values for directions |
ericoneill | 13:f7ec3f026634 | 174 | currDir = 0; |
ericoneill | 13:f7ec3f026634 | 175 | prevDir = 0; |
mawk2311 | 5:61a0a21134f7 | 176 | |
ericoneill | 13:f7ec3f026634 | 177 | // Motor Driver Initializations |
ericoneill | 13:f7ec3f026634 | 178 | motor1.period(motorPeriod); |
ericoneill | 13:f7ec3f026634 | 179 | motor2.period(motorPeriod); |
ericoneill | 13:f7ec3f026634 | 180 | |
ericoneill | 13:f7ec3f026634 | 181 | // Servo Initialization |
ericoneill | 13:f7ec3f026634 | 182 | servo.period(servoPeriod); |
ericoneill | 13:f7ec3f026634 | 183 | servo.pulsewidth(hardRight); |
ericoneill | 13:f7ec3f026634 | 184 | wait(3); |
ericoneill | 13:f7ec3f026634 | 185 | |
ericoneill | 13:f7ec3f026634 | 186 | motor1.pulsewidth(motorPeriod*pulsewidth); |
ericoneill | 13:f7ec3f026634 | 187 | motor2.pulsewidth(motorPeriod*pulsewidth); |
ericoneill | 13:f7ec3f026634 | 188 | break1 = 0; |
ericoneill | 13:f7ec3f026634 | 189 | break2 = 0; |
ericoneill | 13:f7ec3f026634 | 190 | |
ericoneill | 13:f7ec3f026634 | 191 | firstTime = true; |
ericoneill | 13:f7ec3f026634 | 192 | |
ericoneill | 13:f7ec3f026634 | 193 | t.start(); |
ericoneill | 13:f7ec3f026634 | 194 | |
ericoneill | 13:f7ec3f026634 | 195 | if(dataCol){ |
ericoneill | 13:f7ec3f026634 | 196 | loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime = 0; |
ericoneill | 13:f7ec3f026634 | 197 | printTimer.start(); |
ericoneill | 13:f7ec3f026634 | 198 | } |
ericoneill | 13:f7ec3f026634 | 199 | |
ericoneill | 13:f7ec3f026634 | 200 | while(1) { |
ericoneill | 13:f7ec3f026634 | 201 | if(dataCol){ |
ericoneill | 13:f7ec3f026634 | 202 | //break out of main loop if enough time has passed; |
ericoneill | 13:f7ec3f026634 | 203 | //pc.printf("%i", loopCtr); |
ericoneill | 13:f7ec3f026634 | 204 | if(loopCtr >= numData){ |
ericoneill | 13:f7ec3f026634 | 205 | break; |
mawk2311 | 7:ea395616348c | 206 | } |
mawk2311 | 5:61a0a21134f7 | 207 | } |
ericoneill | 13:f7ec3f026634 | 208 | if(integrationCounter % 151== 0){ |
ericoneill | 13:f7ec3f026634 | 209 | /* |
ericoneill | 13:f7ec3f026634 | 210 | //Disable interrupts |
ericoneill | 13:f7ec3f026634 | 211 | interrupt.fall(NULL); |
ericoneill | 13:f7ec3f026634 | 212 | interrupt.rise(NULL); |
ericoneill | 13:f7ec3f026634 | 213 | */ |
ericoneill | 13:f7ec3f026634 | 214 | |
ericoneill | 13:f7ec3f026634 | 215 | //Send start of integration signal |
ericoneill | 13:f7ec3f026634 | 216 | si = 1; |
ericoneill | 13:f7ec3f026634 | 217 | clk = 1; |
ericoneill | 13:f7ec3f026634 | 218 | |
ericoneill | 13:f7ec3f026634 | 219 | si = 0; |
ericoneill | 13:f7ec3f026634 | 220 | clk = 0; |
ericoneill | 13:f7ec3f026634 | 221 | |
ericoneill | 13:f7ec3f026634 | 222 | //Reset timing counter for integration |
ericoneill | 13:f7ec3f026634 | 223 | integrationCounter = 0; |
ericoneill | 13:f7ec3f026634 | 224 | |
ericoneill | 13:f7ec3f026634 | 225 | //Reset line tracking variables |
ericoneill | 13:f7ec3f026634 | 226 | maxAccum = 0; |
ericoneill | 13:f7ec3f026634 | 227 | maxCount = 0; |
ericoneill | 13:f7ec3f026634 | 228 | approxPos = 0; |
ericoneill | 13:f7ec3f026634 | 229 | |
ericoneill | 13:f7ec3f026634 | 230 | space = false; |
mawk2311 | 5:61a0a21134f7 | 231 | |
ericoneill | 13:f7ec3f026634 | 232 | } |
ericoneill | 13:f7ec3f026634 | 233 | else if (integrationCounter > 129){ |
ericoneill | 13:f7ec3f026634 | 234 | //Start Timer |
ericoneill | 13:f7ec3f026634 | 235 | //t.start(); |
ericoneill | 13:f7ec3f026634 | 236 | |
ericoneill | 13:f7ec3f026634 | 237 | //Enable interrupts |
ericoneill | 13:f7ec3f026634 | 238 | //interrupt.fall(&fallInterrupt); |
ericoneill | 13:f7ec3f026634 | 239 | //interrupt.rise(&riseInterrupt); |
ericoneill | 13:f7ec3f026634 | 240 | |
ericoneill | 13:f7ec3f026634 | 241 | if (firstTime){ |
ericoneill | 13:f7ec3f026634 | 242 | |
ericoneill | 13:f7ec3f026634 | 243 | maxVal = ADCdata[10]; |
ericoneill | 13:f7ec3f026634 | 244 | for (int c = 11; c < 118; c++) { |
ericoneill | 13:f7ec3f026634 | 245 | if (ADCdata[c] > maxVal){ |
ericoneill | 13:f7ec3f026634 | 246 | maxVal = ADCdata[c]; |
ericoneill | 13:f7ec3f026634 | 247 | maxLoc = c; |
ericoneill | 13:f7ec3f026634 | 248 | } |
ericoneill | 13:f7ec3f026634 | 249 | } |
ericoneill | 13:f7ec3f026634 | 250 | |
ericoneill | 13:f7ec3f026634 | 251 | for (int c = 10; c < 118; c++) { |
ericoneill | 13:f7ec3f026634 | 252 | if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.007f){ |
ericoneill | 13:f7ec3f026634 | 253 | maxAccum += c; |
ericoneill | 13:f7ec3f026634 | 254 | maxCount++; |
ericoneill | 13:f7ec3f026634 | 255 | if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){ |
ericoneill | 13:f7ec3f026634 | 256 | space = true; |
ericoneill | 13:f7ec3f026634 | 257 | } |
ericoneill | 13:f7ec3f026634 | 258 | prevTrackLoc = c; |
ericoneill | 13:f7ec3f026634 | 259 | } |
ericoneill | 13:f7ec3f026634 | 260 | } |
ericoneill | 13:f7ec3f026634 | 261 | |
ericoneill | 13:f7ec3f026634 | 262 | firstTime = false; |
ericoneill | 13:f7ec3f026634 | 263 | } else { |
ericoneill | 13:f7ec3f026634 | 264 | |
ericoneill | 13:f7ec3f026634 | 265 | startWindow = prevApproxPos - trackWindow; |
ericoneill | 13:f7ec3f026634 | 266 | endWindow = prevApproxPos + trackWindow; |
ericoneill | 13:f7ec3f026634 | 267 | if (startWindow < 0){ |
ericoneill | 13:f7ec3f026634 | 268 | startWindow = 0; |
ericoneill | 13:f7ec3f026634 | 269 | } |
ericoneill | 13:f7ec3f026634 | 270 | if (endWindow > 118){ |
ericoneill | 13:f7ec3f026634 | 271 | endWindow = 118; |
ericoneill | 13:f7ec3f026634 | 272 | } |
ericoneill | 13:f7ec3f026634 | 273 | maxVal = ADCdata[10]; |
ericoneill | 13:f7ec3f026634 | 274 | for (int c = startWindow; c < endWindow; c++) { |
ericoneill | 13:f7ec3f026634 | 275 | if (ADCdata[c] > maxVal){ |
ericoneill | 13:f7ec3f026634 | 276 | maxVal = ADCdata[c]; |
ericoneill | 13:f7ec3f026634 | 277 | maxLoc = c; |
ericoneill | 13:f7ec3f026634 | 278 | } |
ericoneill | 13:f7ec3f026634 | 279 | } |
ericoneill | 13:f7ec3f026634 | 280 | |
ericoneill | 13:f7ec3f026634 | 281 | for (int c = startWindow; c < endWindow; c++) { |
ericoneill | 13:f7ec3f026634 | 282 | if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.007f){ |
ericoneill | 13:f7ec3f026634 | 283 | maxAccum += c; |
ericoneill | 13:f7ec3f026634 | 284 | maxCount++; |
ericoneill | 13:f7ec3f026634 | 285 | if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){ |
ericoneill | 13:f7ec3f026634 | 286 | space = true; |
ericoneill | 13:f7ec3f026634 | 287 | } |
ericoneill | 13:f7ec3f026634 | 288 | prevTrackLoc = c; |
ericoneill | 13:f7ec3f026634 | 289 | } |
ericoneill | 13:f7ec3f026634 | 290 | } |
ericoneill | 13:f7ec3f026634 | 291 | } |
ericoneill | 13:f7ec3f026634 | 292 | /* |
ericoneill | 13:f7ec3f026634 | 293 | //Check if we need to alter integration time due to brightness |
ericoneill | 13:f7ec3f026634 | 294 | if (maxVal < 0.15f){ |
ericoneill | 13:f7ec3f026634 | 295 | intTimMod += 10; |
ericoneill | 13:f7ec3f026634 | 296 | } else if (maxVal >= 1) { |
ericoneill | 13:f7ec3f026634 | 297 | if (intTimMod > 0) { |
ericoneill | 13:f7ec3f026634 | 298 | intTimMod -= 10; |
ericoneill | 13:f7ec3f026634 | 299 | } |
ericoneill | 13:f7ec3f026634 | 300 | } |
ericoneill | 13:f7ec3f026634 | 301 | */ |
ericoneill | 13:f7ec3f026634 | 302 | |
ericoneill | 13:f7ec3f026634 | 303 | //Line Crossing Checks |
ericoneill | 13:f7ec3f026634 | 304 | if (space) { |
ericoneill | 13:f7ec3f026634 | 305 | currDir = prevDir; |
ericoneill | 13:f7ec3f026634 | 306 | firstTime = true; |
ericoneill | 13:f7ec3f026634 | 307 | } else { |
ericoneill | 13:f7ec3f026634 | 308 | approxPos = (float)maxAccum/(float)maxCount; |
ericoneill | 13:f7ec3f026634 | 309 | |
ericoneill | 13:f7ec3f026634 | 310 | if(dataCol){ |
ericoneill | 13:f7ec3f026634 | 311 | if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%6==0){ |
ericoneill | 13:f7ec3f026634 | 312 | lineCenters[loopCtr] = approxPos; |
ericoneill | 13:f7ec3f026634 | 313 | times[loopCtr] = printTimer.read_ms(); |
ericoneill | 13:f7ec3f026634 | 314 | pSteering[loopCtr] = currDir; |
ericoneill | 13:f7ec3f026634 | 315 | pSteering[loopCtr] = PIDcontrol(approxPos); |
ericoneill | 13:f7ec3f026634 | 316 | loopCtr++; |
ericoneill | 13:f7ec3f026634 | 317 | } |
ericoneill | 13:f7ec3f026634 | 318 | } |
ericoneill | 13:f7ec3f026634 | 319 | |
ericoneill | 13:f7ec3f026634 | 320 | currDir = hardLeft + (approxPos)/((float) 127)*(hardRight-hardLeft); |
ericoneill | 13:f7ec3f026634 | 321 | prevApproxPos = approxPos; |
ericoneill | 13:f7ec3f026634 | 322 | |
ericoneill | 13:f7ec3f026634 | 323 | } |
ericoneill | 13:f7ec3f026634 | 324 | |
ericoneill | 13:f7ec3f026634 | 325 | servo.pulsewidth(currDir); |
ericoneill | 13:f7ec3f026634 | 326 | |
ericoneill | 13:f7ec3f026634 | 327 | //Start Velocity control after requisite number of encoder signals have been collected |
ericoneill | 13:f7ec3f026634 | 328 | //if(numInterrupts >= 4){ |
ericoneill | 13:f7ec3f026634 | 329 | //velocity_control(0.1f, TUNING_CONSTANT_10); |
ericoneill | 13:f7ec3f026634 | 330 | //} |
ericoneill | 13:f7ec3f026634 | 331 | |
ericoneill | 13:f7ec3f026634 | 332 | //Save current direction as previous direction |
ericoneill | 13:f7ec3f026634 | 333 | prevDir = currDir; |
ericoneill | 13:f7ec3f026634 | 334 | |
ericoneill | 13:f7ec3f026634 | 335 | //Prepare to start collecting more data |
ericoneill | 13:f7ec3f026634 | 336 | integrationCounter = 150; |
ericoneill | 13:f7ec3f026634 | 337 | |
ericoneill | 13:f7ec3f026634 | 338 | //Disable interrupts |
ericoneill | 13:f7ec3f026634 | 339 | //interrupt.fall(NULL); |
ericoneill | 13:f7ec3f026634 | 340 | //interrupt.rise(NULL);x |
ericoneill | 13:f7ec3f026634 | 341 | |
ericoneill | 13:f7ec3f026634 | 342 | //Stop timer |
ericoneill | 13:f7ec3f026634 | 343 | //t.stop(); |
ericoneill | 13:f7ec3f026634 | 344 | } |
ericoneill | 13:f7ec3f026634 | 345 | else{ |
ericoneill | 13:f7ec3f026634 | 346 | clk = 1; |
ericoneill | 13:f7ec3f026634 | 347 | wait_us(intTimMod); |
ericoneill | 13:f7ec3f026634 | 348 | ADCdata[integrationCounter - 1] = camData; |
ericoneill | 13:f7ec3f026634 | 349 | clk = 0; |
ericoneill | 13:f7ec3f026634 | 350 | } |
ericoneill | 13:f7ec3f026634 | 351 | |
ericoneill | 13:f7ec3f026634 | 352 | //clk = 0; |
ericoneill | 13:f7ec3f026634 | 353 | integrationCounter++; |
ericoneill | 13:f7ec3f026634 | 354 | if(dataCol){ |
ericoneill | 13:f7ec3f026634 | 355 | loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime++; |
ericoneill | 13:f7ec3f026634 | 356 | } |
ericoneill | 13:f7ec3f026634 | 357 | //camData. |
ericoneill | 13:f7ec3f026634 | 358 | |
mawk2311 | 7:ea395616348c | 359 | } |
ericoneill | 13:f7ec3f026634 | 360 | if (dataCol){ |
ericoneill | 13:f7ec3f026634 | 361 | //print frame data |
ericoneill | 13:f7ec3f026634 | 362 | pc.printf("printing frame data\n\r"); |
ericoneill | 13:f7ec3f026634 | 363 | //int frameSize = frames.size(); |
ericoneill | 13:f7ec3f026634 | 364 | //pc.printf("%i",frameSize); |
ericoneill | 13:f7ec3f026634 | 365 | pc.printf("["); |
ericoneill | 13:f7ec3f026634 | 366 | for(int i=0; i<numData; i++){ |
ericoneill | 13:f7ec3f026634 | 367 | if(lineCenters > 0){ |
ericoneill | 13:f7ec3f026634 | 368 | pc.printf("%i %i %f %f,",lineCenters[i], times[i], pSteering[i], pdSteering[i]); |
ericoneill | 13:f7ec3f026634 | 369 | } |
ericoneill | 13:f7ec3f026634 | 370 | } |
ericoneill | 13:f7ec3f026634 | 371 | pc.printf("]\n\r"); |
ericoneill | 0:d328ecb3fbb1 | 372 | } |
ericoneill | 0:d328ecb3fbb1 | 373 | } |
mawk2311 | 7:ea395616348c | 374 |