E=MC / Mbed 2 deprecated e=mc

.

Dependencies:   Servo mbed

main.cpp@18:9508bece39b0, 2015-04-24 (annotated)

Committer:
ericoneill
Date:
Fri Apr 24 01:31:57 2015 +0000
Revision:
18:9508bece39b0
Parent:
17:054a54d73e04 
stuff makes sense!;

Who changed what in which revision?

UserRevisionLine numberNew 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 18:9508bece39b0 157 float PIDcontrol(float lineCenter, int loopCtr){
ericoneill 14:bda4a189cbe8 158 //centerSum += lineCenter;
ericoneill 15:d746c53bf49b 159 float newLineCenter = (float)lineCenter/127.0f;
ericoneill 18:9508bece39b0 160 pdLineCenter[loopCtr] = newLineCenter;
ericoneill 14:bda4a189cbe8 161 float newTime = t.read();
ericoneill 14:bda4a189cbe8 162 timeDiff = newTime - lastTime;
ericoneill 14:bda4a189cbe8 163 lastTime = newTime;
ericoneill 15:d746c53bf49b 164 float lineDif = ((float)newLineCenter - (float)prevLineCenter)/(float) timeDiff;
ericoneill 16:f7ab57048e1e 165 lineDifData[loopCtr] = lineDif;
ericoneill 15:d746c53bf49b 166 if(prevLineCenter != newLineCenter){
ericoneill 15:d746c53bf49b 167 prevLineCenter = newLineCenter;
ericoneill 15:d746c53bf49b 168 }
ericoneill 15:d746c53bf49b 169 return p * newLineCenter + d * lineDif;
ericoneill 14:bda4a189cbe8 170
ericoneill 4:263bddc51c0f 171 }
mawk2311 5:61a0a21134f7 172
ericoneill 14:bda4a189cbe8 173 int main() {
ericoneill 14:bda4a189cbe8 174
ericoneill 14:bda4a189cbe8 175 //Alter reg values to speed up KL25Z
ericoneill 14:bda4a189cbe8 176 initADC();
mawk2311 5:61a0a21134f7 177
ericoneill 14:bda4a189cbe8 178 //Line Tracker Initializations
ericoneill 14:bda4a189cbe8 179 int integrationCounter = 0;
ericoneill 14:bda4a189cbe8 180
ericoneill 14:bda4a189cbe8 181 //Initial values for directions
ericoneill 14:bda4a189cbe8 182 currDir = 0;
ericoneill 14:bda4a189cbe8 183 prevDir = 0;
mawk2311 5:61a0a21134f7 184
ericoneill 14:bda4a189cbe8 185 // Motor Driver Initializations
ericoneill 14:bda4a189cbe8 186 motor1.period(motorPeriod);
ericoneill 14:bda4a189cbe8 187 motor2.period(motorPeriod);
ericoneill 14:bda4a189cbe8 188
ericoneill 14:bda4a189cbe8 189 // Servo Initialization
ericoneill 14:bda4a189cbe8 190 servo.period(servoPeriod);
ericoneill 14:bda4a189cbe8 191 servo.pulsewidth(hardRight);
ericoneill 14:bda4a189cbe8 192 wait(3);
ericoneill 14:bda4a189cbe8 193
ericoneill 14:bda4a189cbe8 194 motor1.pulsewidth(motorPeriod*pulsewidth);
ericoneill 14:bda4a189cbe8 195 motor2.pulsewidth(motorPeriod*pulsewidth);
ericoneill 14:bda4a189cbe8 196 break1 = 0;
ericoneill 14:bda4a189cbe8 197 break2 = 0;
ericoneill 14:bda4a189cbe8 198
ericoneill 14:bda4a189cbe8 199 firstTime = true;
ericoneill 14:bda4a189cbe8 200
ericoneill 14:bda4a189cbe8 201 t.start();
ericoneill 14:bda4a189cbe8 202
ericoneill 14:bda4a189cbe8 203 if(dataCol){
ericoneill 14:bda4a189cbe8 204 loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime = 0;
ericoneill 14:bda4a189cbe8 205 printTimer.start();
ericoneill 14:bda4a189cbe8 206 }
ericoneill 14:bda4a189cbe8 207
ericoneill 14:bda4a189cbe8 208 while(1) {
ericoneill 14:bda4a189cbe8 209 if(dataCol){
ericoneill 14:bda4a189cbe8 210 //break out of main loop if enough time has passed;
ericoneill 14:bda4a189cbe8 211 //pc.printf("%i", loopCtr);
ericoneill 14:bda4a189cbe8 212 if(loopCtr >= numData){
ericoneill 14:bda4a189cbe8 213 break;
mawk2311 7:ea395616348c 214 }
mawk2311 5:61a0a21134f7 215 }
ericoneill 14:bda4a189cbe8 216 if(integrationCounter % 151== 0){
ericoneill 14:bda4a189cbe8 217 /*
ericoneill 14:bda4a189cbe8 218 //Disable interrupts
ericoneill 14:bda4a189cbe8 219 interrupt.fall(NULL);
ericoneill 14:bda4a189cbe8 220 interrupt.rise(NULL);
ericoneill 14:bda4a189cbe8 221 */
ericoneill 14:bda4a189cbe8 222
ericoneill 14:bda4a189cbe8 223 //Send start of integration signal
ericoneill 14:bda4a189cbe8 224 si = 1;
ericoneill 14:bda4a189cbe8 225 clk = 1;
ericoneill 14:bda4a189cbe8 226
ericoneill 14:bda4a189cbe8 227 si = 0;
ericoneill 14:bda4a189cbe8 228 clk = 0;
ericoneill 14:bda4a189cbe8 229
ericoneill 14:bda4a189cbe8 230 //Reset timing counter for integration
ericoneill 14:bda4a189cbe8 231 integrationCounter = 0;
ericoneill 14:bda4a189cbe8 232
ericoneill 14:bda4a189cbe8 233 //Reset line tracking variables
ericoneill 14:bda4a189cbe8 234 maxAccum = 0;
ericoneill 14:bda4a189cbe8 235 maxCount = 0;
ericoneill 14:bda4a189cbe8 236 approxPos = 0;
ericoneill 14:bda4a189cbe8 237
ericoneill 14:bda4a189cbe8 238 space = false;
mawk2311 5:61a0a21134f7 239
ericoneill 14:bda4a189cbe8 240 }
ericoneill 14:bda4a189cbe8 241 else if (integrationCounter > 129){
ericoneill 14:bda4a189cbe8 242 //Start Timer
ericoneill 14:bda4a189cbe8 243 //t.start();
ericoneill 14:bda4a189cbe8 244
ericoneill 14:bda4a189cbe8 245 //Enable interrupts
ericoneill 14:bda4a189cbe8 246 //interrupt.fall(&fallInterrupt);
ericoneill 14:bda4a189cbe8 247 //interrupt.rise(&riseInterrupt);
ericoneill 14:bda4a189cbe8 248
ericoneill 14:bda4a189cbe8 249 if (firstTime){
ericoneill 14:bda4a189cbe8 250
ericoneill 14:bda4a189cbe8 251 maxVal = ADCdata[10];
ericoneill 14:bda4a189cbe8 252 for (int c = 11; c < 118; c++) {
ericoneill 14:bda4a189cbe8 253 if (ADCdata[c] > maxVal){
ericoneill 14:bda4a189cbe8 254 maxVal = ADCdata[c];
ericoneill 14:bda4a189cbe8 255 maxLoc = c;
ericoneill 14:bda4a189cbe8 256 }
ericoneill 14:bda4a189cbe8 257 }
ericoneill 14:bda4a189cbe8 258
ericoneill 14:bda4a189cbe8 259 for (int c = 10; c < 118; c++) {
ericoneill 14:bda4a189cbe8 260 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.007f){
ericoneill 14:bda4a189cbe8 261 maxAccum += c;
ericoneill 14:bda4a189cbe8 262 maxCount++;
ericoneill 14:bda4a189cbe8 263 if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
ericoneill 14:bda4a189cbe8 264 space = true;
ericoneill 14:bda4a189cbe8 265 }
ericoneill 14:bda4a189cbe8 266 prevTrackLoc = c;
ericoneill 14:bda4a189cbe8 267 }
ericoneill 14:bda4a189cbe8 268 }
ericoneill 14:bda4a189cbe8 269
ericoneill 14:bda4a189cbe8 270 firstTime = false;
ericoneill 14:bda4a189cbe8 271 } else {
ericoneill 14:bda4a189cbe8 272
ericoneill 14:bda4a189cbe8 273 startWindow = prevApproxPos - trackWindow;
ericoneill 14:bda4a189cbe8 274 endWindow = prevApproxPos + trackWindow;
ericoneill 14:bda4a189cbe8 275 if (startWindow < 0){
ericoneill 14:bda4a189cbe8 276 startWindow = 0;
ericoneill 14:bda4a189cbe8 277 }
ericoneill 14:bda4a189cbe8 278 if (endWindow > 118){
ericoneill 14:bda4a189cbe8 279 endWindow = 118;
ericoneill 14:bda4a189cbe8 280 }
ericoneill 14:bda4a189cbe8 281 maxVal = ADCdata[10];
ericoneill 14:bda4a189cbe8 282 for (int c = startWindow; c < endWindow; c++) {
ericoneill 14:bda4a189cbe8 283 if (ADCdata[c] > maxVal){
ericoneill 14:bda4a189cbe8 284 maxVal = ADCdata[c];
ericoneill 14:bda4a189cbe8 285 maxLoc = c;
ericoneill 14:bda4a189cbe8 286 }
ericoneill 14:bda4a189cbe8 287 }
ericoneill 14:bda4a189cbe8 288
ericoneill 14:bda4a189cbe8 289 for (int c = startWindow; c < endWindow; c++) {
ericoneill 14:bda4a189cbe8 290 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.007f){
ericoneill 14:bda4a189cbe8 291 maxAccum += c;
ericoneill 14:bda4a189cbe8 292 maxCount++;
ericoneill 14:bda4a189cbe8 293 if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
ericoneill 14:bda4a189cbe8 294 space = true;
ericoneill 14:bda4a189cbe8 295 }
ericoneill 14:bda4a189cbe8 296 prevTrackLoc = c;
ericoneill 14:bda4a189cbe8 297 }
ericoneill 14:bda4a189cbe8 298 }
ericoneill 14:bda4a189cbe8 299 }
ericoneill 14:bda4a189cbe8 300 /*
ericoneill 14:bda4a189cbe8 301 //Check if we need to alter integration time due to brightness
ericoneill 14:bda4a189cbe8 302 if (maxVal < 0.15f){
ericoneill 14:bda4a189cbe8 303 intTimMod += 10;
ericoneill 14:bda4a189cbe8 304 } else if (maxVal >= 1) {
ericoneill 14:bda4a189cbe8 305 if (intTimMod > 0) {
ericoneill 14:bda4a189cbe8 306 intTimMod -= 10;
ericoneill 14:bda4a189cbe8 307 }
ericoneill 14:bda4a189cbe8 308 }
ericoneill 14:bda4a189cbe8 309 */
ericoneill 14:bda4a189cbe8 310
ericoneill 14:bda4a189cbe8 311 //Line Crossing Checks
ericoneill 16:f7ab57048e1e 312 if(dataCol){
ericoneill 16:f7ab57048e1e 313 if (space) {
ericoneill 16:f7ab57048e1e 314 currDir = prevDir;
ericoneill 15:d746c53bf49b 315 if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%8==0){
ericoneill 14:bda4a189cbe8 316 lineCenters[loopCtr] = approxPos;
ericoneill 14:bda4a189cbe8 317 times[loopCtr] = printTimer.read_ms();
ericoneill 14:bda4a189cbe8 318 pSteering[loopCtr] = currDir;
ericoneill 16:f7ab57048e1e 319 pdSteering[loopCtr] = PIDcontrol(approxPos, loopCtr);
ericoneill 14:bda4a189cbe8 320 loopCtr++;
ericoneill 14:bda4a189cbe8 321 }
ericoneill 16:f7ab57048e1e 322 firstTime = true;
ericoneill 16:f7ab57048e1e 323 } else {
ericoneill 16:f7ab57048e1e 324 approxPos = (float)maxAccum/(float)maxCount;
ericoneill 16:f7ab57048e1e 325 currDir = hardLeft + (approxPos)/((float) 127)*(hardRight-hardLeft);
ericoneill 16:f7ab57048e1e 326 if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%8==0){
ericoneill 16:f7ab57048e1e 327 lineCenters[loopCtr] = approxPos;
ericoneill 16:f7ab57048e1e 328 times[loopCtr] = printTimer.read_ms();
ericoneill 16:f7ab57048e1e 329 pSteering[loopCtr] = currDir;
ericoneill 16:f7ab57048e1e 330 pdSteering[loopCtr] = PIDcontrol(approxPos, loopCtr);
ericoneill 16:f7ab57048e1e 331 loopCtr++;
ericoneill 16:f7ab57048e1e 332 }
ericoneill 16:f7ab57048e1e 333
ericoneill 16:f7ab57048e1e 334
ericoneill 16:f7ab57048e1e 335 prevApproxPos = approxPos;
ericoneill 16:f7ab57048e1e 336
ericoneill 14:bda4a189cbe8 337 }
ericoneill 14:bda4a189cbe8 338 }
ericoneill 14:bda4a189cbe8 339
ericoneill 14:bda4a189cbe8 340 servo.pulsewidth(currDir);
ericoneill 14:bda4a189cbe8 341
ericoneill 14:bda4a189cbe8 342 //Start Velocity control after requisite number of encoder signals have been collected
ericoneill 14:bda4a189cbe8 343 //if(numInterrupts >= 4){
ericoneill 14:bda4a189cbe8 344 //velocity_control(0.1f, TUNING_CONSTANT_10);
ericoneill 14:bda4a189cbe8 345 //}
ericoneill 14:bda4a189cbe8 346
ericoneill 14:bda4a189cbe8 347 //Save current direction as previous direction
ericoneill 14:bda4a189cbe8 348 prevDir = currDir;
ericoneill 14:bda4a189cbe8 349
ericoneill 14:bda4a189cbe8 350 //Prepare to start collecting more data
ericoneill 14:bda4a189cbe8 351 integrationCounter = 150;
ericoneill 14:bda4a189cbe8 352
ericoneill 14:bda4a189cbe8 353 //Disable interrupts
ericoneill 14:bda4a189cbe8 354 //interrupt.fall(NULL);
ericoneill 14:bda4a189cbe8 355 //interrupt.rise(NULL);x
ericoneill 14:bda4a189cbe8 356
ericoneill 14:bda4a189cbe8 357 //Stop timer
ericoneill 14:bda4a189cbe8 358 //t.stop();
ericoneill 14:bda4a189cbe8 359 }
ericoneill 14:bda4a189cbe8 360 else{
ericoneill 14:bda4a189cbe8 361 clk = 1;
ericoneill 14:bda4a189cbe8 362 wait_us(intTimMod);
ericoneill 14:bda4a189cbe8 363 ADCdata[integrationCounter - 1] = camData;
ericoneill 14:bda4a189cbe8 364 clk = 0;
ericoneill 14:bda4a189cbe8 365 }
ericoneill 14:bda4a189cbe8 366
ericoneill 14:bda4a189cbe8 367 //clk = 0;
ericoneill 14:bda4a189cbe8 368 integrationCounter++;
ericoneill 14:bda4a189cbe8 369 if(dataCol){
ericoneill 14:bda4a189cbe8 370 loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime++;
ericoneill 14:bda4a189cbe8 371 }
ericoneill 14:bda4a189cbe8 372 //camData.
ericoneill 14:bda4a189cbe8 373
mawk2311 7:ea395616348c 374 }
ericoneill 14:bda4a189cbe8 375 if (dataCol){
ericoneill 14:bda4a189cbe8 376 //print frame data
ericoneill 14:bda4a189cbe8 377 pc.printf("printing frame data\n\r");
ericoneill 14:bda4a189cbe8 378 //int frameSize = frames.size();
ericoneill 14:bda4a189cbe8 379 //pc.printf("%i",frameSize);
ericoneill 16:f7ab57048e1e 380 for(int i=0; i<numData; i+=10){
ericoneill 14:bda4a189cbe8 381 if(lineCenters > 0){
cheryl_he 17:054a54d73e04 382 /*pc.printf("LINE #: %i\n\r",i);
ericoneill 16:f7ab57048e1e 383 pc.printf("line center: %i\n\r", lineCenters[i]);
ericoneill 16:f7ab57048e1e 384 pc.printf("time: %i\n\r", times[i]);
ericoneill 16:f7ab57048e1e 385 pc.printf("pSteer: %f\n\r", pSteering[i]);
ericoneill 16:f7ab57048e1e 386 pc.printf("pdSteer: %f\n\r", pdSteering[i]);
ericoneill 16:f7ab57048e1e 387 pc.printf("lineDif: %f\n\r", lineDifData[i]);
ericoneill 16:f7ab57048e1e 388 pc.printf("pdLineCenter: %f\n\r", pdLineCenter[i]);
cheryl_he 17:054a54d73e04 389 */
cheryl_he 17:054a54d73e04 390 pc.printf("%i %i %f %f %f %f\n\r",lineCenters[i], times[i], pSteering[i], pdSteering[i], lineDifData[i], pdLineCenter[i]);
ericoneill 14:bda4a189cbe8 391 }
ericoneill 14:bda4a189cbe8 392 }
ericoneill 0:d328ecb3fbb1 393 }
ericoneill 0:d328ecb3fbb1 394 }
mawk2311 7:ea395616348c 395