E=MC / Mbed 2 deprecated e=mc

.

Dependencies:   Servo mbed

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?

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 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