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