E=MC / Mbed 2 deprecated e=mc

.

Dependencies:   Servo mbed

main.cpp@15:d746c53bf49b, 2015-04-24 (annotated)

Committer:
ericoneill
Date:
Fri Apr 24 00:24:36 2015 +0000
Revision:
15:d746c53bf49b
Parent:
14:bda4a189cbe8
Child:
16:f7ab57048e1e
fuckin zeroes;

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 14:bda4a189cbe8 67 int loopCtr = 0;
ericoneill 14:bda4a189cbe8 68
ericoneill 14:bda4a189cbe8 69 //End of Line Tracking Variables -------------------------
ericoneill 14:bda4a189cbe8 70
ericoneill 14:bda4a189cbe8 71 //Encoder and Motor Driver Variables ---------------------
ericoneill 14:bda4a189cbe8 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 14:bda4a189cbe8 85 float last_speed = 0;
ericoneill 14:bda4a189cbe8 86
mawk2311 7:ea395616348c 87 float stall_check = 0;
mawk2311 7:ea395616348c 88 float tuning_val = 1;
mawk2311 5:61a0a21134f7 89
ericoneill 14:bda4a189cbe8 90 int numInterrupts = 0;
ericoneill 14:bda4a189cbe8 91
ericoneill 14:bda4a189cbe8 92 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~PWM & Integration Time ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ericoneill 14:bda4a189cbe8 93 float pulsewidth = 0.14f;
ericoneill 14:bda4a189cbe8 94 int intTimMod = 0;
ericoneill 14:bda4a189cbe8 95 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ericoneill 14:bda4a189cbe8 96 // Hardware periods
ericoneill 14:bda4a189cbe8 97 float motorPeriod = .0025;
ericoneill 14:bda4a189cbe8 98 float servoPeriod = .0025;
ericoneill 14:bda4a189cbe8 99
ericoneill 1:8e5821dec0f7 100 Timer t;
ericoneill 8:f3e0b4814888 101 Timer servoTimer;
ericoneill 14:bda4a189cbe8 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 14:bda4a189cbe8 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 14:bda4a189cbe8 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 14:bda4a189cbe8 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 14:bda4a189cbe8 123 //End of Encoder and Motor Driver Variables ----------------------
ericoneill 14:bda4a189cbe8 124
ericoneill 14:bda4a189cbe8 125 //Line Tracker Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ericoneill 14:bda4a189cbe8 126
ericoneill 14:bda4a189cbe8 127 //PIDcontrol -----------------------------------------------------
ericoneill 14:bda4a189cbe8 128 int centerSum = 0;
ericoneill 15:d746c53bf49b 129 float prevLineCenter = 0.0f;
ericoneill 14:bda4a189cbe8 130 int prevTime = 0;
ericoneill 14:bda4a189cbe8 131 int p,i,d = 1;
ericoneill 14:bda4a189cbe8 132 float timeDiff = 0.0f;
ericoneill 14:bda4a189cbe8 133 float lastTime = 0.0f;
ericoneill 14:bda4a189cbe8 134
ericoneill 14:bda4a189cbe8 135 //Function for speeding up KL25Z ADC
ericoneill 14:bda4a189cbe8 136 void initADC(void){
ericoneill 14:bda4a189cbe8 137
ericoneill 14:bda4a189cbe8 138 ADC0->CFG1 = ADC0->CFG1 & (
ericoneill 14:bda4a189cbe8 139 ~(
ericoneill 14:bda4a189cbe8 140 0x80 // LDLPC = 0 ; no low-power mode
ericoneill 14:bda4a189cbe8 141 | 0x60 // ADIV = 1
ericoneill 14:bda4a189cbe8 142 | 0x10 // Sample time short
ericoneill 14:bda4a189cbe8 143 | 0x03 // input clock = BUS CLK
ericoneill 14:bda4a189cbe8 144 )
ericoneill 14:bda4a189cbe8 145 ) ; // clkdiv <= 1
ericoneill 14:bda4a189cbe8 146 ADC0->CFG2 = ADC0->CFG2
ericoneill 14:bda4a189cbe8 147 | 0x03 ; // Logsample Time 11 = 2 extra ADCK
ericoneill 14:bda4a189cbe8 148 ADC0->SC3 = ADC0->SC3
ericoneill 14:bda4a189cbe8 149 & (~(0x03)) ; // hardware avarage off
ericoneill 14:bda4a189cbe8 150 }
ericoneill 14:bda4a189cbe8 151
ericoneill 14:bda4a189cbe8 152
ericoneill 14:bda4a189cbe8 153 // Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ericoneill 14:bda4a189cbe8 154
ericoneill 14:bda4a189cbe8 155 float PIDcontrol(int lineCenter){
ericoneill 14:bda4a189cbe8 156 //centerSum += lineCenter;
ericoneill 15:d746c53bf49b 157 float newLineCenter = (float)lineCenter/127.0f;
ericoneill 14:bda4a189cbe8 158 float newTime = t.read();
ericoneill 14:bda4a189cbe8 159 timeDiff = newTime - lastTime;
ericoneill 14:bda4a189cbe8 160 lastTime = newTime;
ericoneill 15:d746c53bf49b 161 float lineDif = ((float)newLineCenter - (float)prevLineCenter)/(float) timeDiff;
ericoneill 15:d746c53bf49b 162 if(prevLineCenter != newLineCenter){
ericoneill 15:d746c53bf49b 163 prevLineCenter = newLineCenter;
ericoneill 15:d746c53bf49b 164 }
ericoneill 15:d746c53bf49b 165 return p * newLineCenter + d * lineDif;
ericoneill 14:bda4a189cbe8 166
ericoneill 4:263bddc51c0f 167 }
mawk2311 5:61a0a21134f7 168
ericoneill 14:bda4a189cbe8 169 int main() {
ericoneill 14:bda4a189cbe8 170
ericoneill 14:bda4a189cbe8 171 //Alter reg values to speed up KL25Z
ericoneill 14:bda4a189cbe8 172 initADC();
mawk2311 5:61a0a21134f7 173
ericoneill 14:bda4a189cbe8 174 //Line Tracker Initializations
ericoneill 14:bda4a189cbe8 175 int integrationCounter = 0;
ericoneill 14:bda4a189cbe8 176
ericoneill 14:bda4a189cbe8 177 //Initial values for directions
ericoneill 14:bda4a189cbe8 178 currDir = 0;
ericoneill 14:bda4a189cbe8 179 prevDir = 0;
mawk2311 5:61a0a21134f7 180
ericoneill 14:bda4a189cbe8 181 // Motor Driver Initializations
ericoneill 14:bda4a189cbe8 182 motor1.period(motorPeriod);
ericoneill 14:bda4a189cbe8 183 motor2.period(motorPeriod);
ericoneill 14:bda4a189cbe8 184
ericoneill 14:bda4a189cbe8 185 // Servo Initialization
ericoneill 14:bda4a189cbe8 186 servo.period(servoPeriod);
ericoneill 14:bda4a189cbe8 187 servo.pulsewidth(hardRight);
ericoneill 14:bda4a189cbe8 188 wait(3);
ericoneill 14:bda4a189cbe8 189
ericoneill 14:bda4a189cbe8 190 motor1.pulsewidth(motorPeriod*pulsewidth);
ericoneill 14:bda4a189cbe8 191 motor2.pulsewidth(motorPeriod*pulsewidth);
ericoneill 14:bda4a189cbe8 192 break1 = 0;
ericoneill 14:bda4a189cbe8 193 break2 = 0;
ericoneill 14:bda4a189cbe8 194
ericoneill 14:bda4a189cbe8 195 firstTime = true;
ericoneill 14:bda4a189cbe8 196
ericoneill 14:bda4a189cbe8 197 t.start();
ericoneill 14:bda4a189cbe8 198
ericoneill 14:bda4a189cbe8 199 if(dataCol){
ericoneill 14:bda4a189cbe8 200 loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime = 0;
ericoneill 14:bda4a189cbe8 201 printTimer.start();
ericoneill 14:bda4a189cbe8 202 }
ericoneill 14:bda4a189cbe8 203
ericoneill 14:bda4a189cbe8 204 while(1) {
ericoneill 14:bda4a189cbe8 205 if(dataCol){
ericoneill 14:bda4a189cbe8 206 //break out of main loop if enough time has passed;
ericoneill 14:bda4a189cbe8 207 //pc.printf("%i", loopCtr);
ericoneill 14:bda4a189cbe8 208 if(loopCtr >= numData){
ericoneill 14:bda4a189cbe8 209 break;
mawk2311 7:ea395616348c 210 }
mawk2311 5:61a0a21134f7 211 }
ericoneill 14:bda4a189cbe8 212 if(integrationCounter % 151== 0){
ericoneill 14:bda4a189cbe8 213 /*
ericoneill 14:bda4a189cbe8 214 //Disable interrupts
ericoneill 14:bda4a189cbe8 215 interrupt.fall(NULL);
ericoneill 14:bda4a189cbe8 216 interrupt.rise(NULL);
ericoneill 14:bda4a189cbe8 217 */
ericoneill 14:bda4a189cbe8 218
ericoneill 14:bda4a189cbe8 219 //Send start of integration signal
ericoneill 14:bda4a189cbe8 220 si = 1;
ericoneill 14:bda4a189cbe8 221 clk = 1;
ericoneill 14:bda4a189cbe8 222
ericoneill 14:bda4a189cbe8 223 si = 0;
ericoneill 14:bda4a189cbe8 224 clk = 0;
ericoneill 14:bda4a189cbe8 225
ericoneill 14:bda4a189cbe8 226 //Reset timing counter for integration
ericoneill 14:bda4a189cbe8 227 integrationCounter = 0;
ericoneill 14:bda4a189cbe8 228
ericoneill 14:bda4a189cbe8 229 //Reset line tracking variables
ericoneill 14:bda4a189cbe8 230 maxAccum = 0;
ericoneill 14:bda4a189cbe8 231 maxCount = 0;
ericoneill 14:bda4a189cbe8 232 approxPos = 0;
ericoneill 14:bda4a189cbe8 233
ericoneill 14:bda4a189cbe8 234 space = false;
mawk2311 5:61a0a21134f7 235
ericoneill 14:bda4a189cbe8 236 }
ericoneill 14:bda4a189cbe8 237 else if (integrationCounter > 129){
ericoneill 14:bda4a189cbe8 238 //Start Timer
ericoneill 14:bda4a189cbe8 239 //t.start();
ericoneill 14:bda4a189cbe8 240
ericoneill 14:bda4a189cbe8 241 //Enable interrupts
ericoneill 14:bda4a189cbe8 242 //interrupt.fall(&fallInterrupt);
ericoneill 14:bda4a189cbe8 243 //interrupt.rise(&riseInterrupt);
ericoneill 14:bda4a189cbe8 244
ericoneill 14:bda4a189cbe8 245 if (firstTime){
ericoneill 14:bda4a189cbe8 246
ericoneill 14:bda4a189cbe8 247 maxVal = ADCdata[10];
ericoneill 14:bda4a189cbe8 248 for (int c = 11; c < 118; c++) {
ericoneill 14:bda4a189cbe8 249 if (ADCdata[c] > maxVal){
ericoneill 14:bda4a189cbe8 250 maxVal = ADCdata[c];
ericoneill 14:bda4a189cbe8 251 maxLoc = c;
ericoneill 14:bda4a189cbe8 252 }
ericoneill 14:bda4a189cbe8 253 }
ericoneill 14:bda4a189cbe8 254
ericoneill 14:bda4a189cbe8 255 for (int c = 10; c < 118; c++) {
ericoneill 14:bda4a189cbe8 256 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.007f){
ericoneill 14:bda4a189cbe8 257 maxAccum += c;
ericoneill 14:bda4a189cbe8 258 maxCount++;
ericoneill 14:bda4a189cbe8 259 if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
ericoneill 14:bda4a189cbe8 260 space = true;
ericoneill 14:bda4a189cbe8 261 }
ericoneill 14:bda4a189cbe8 262 prevTrackLoc = c;
ericoneill 14:bda4a189cbe8 263 }
ericoneill 14:bda4a189cbe8 264 }
ericoneill 14:bda4a189cbe8 265
ericoneill 14:bda4a189cbe8 266 firstTime = false;
ericoneill 14:bda4a189cbe8 267 } else {
ericoneill 14:bda4a189cbe8 268
ericoneill 14:bda4a189cbe8 269 startWindow = prevApproxPos - trackWindow;
ericoneill 14:bda4a189cbe8 270 endWindow = prevApproxPos + trackWindow;
ericoneill 14:bda4a189cbe8 271 if (startWindow < 0){
ericoneill 14:bda4a189cbe8 272 startWindow = 0;
ericoneill 14:bda4a189cbe8 273 }
ericoneill 14:bda4a189cbe8 274 if (endWindow > 118){
ericoneill 14:bda4a189cbe8 275 endWindow = 118;
ericoneill 14:bda4a189cbe8 276 }
ericoneill 14:bda4a189cbe8 277 maxVal = ADCdata[10];
ericoneill 14:bda4a189cbe8 278 for (int c = startWindow; c < endWindow; c++) {
ericoneill 14:bda4a189cbe8 279 if (ADCdata[c] > maxVal){
ericoneill 14:bda4a189cbe8 280 maxVal = ADCdata[c];
ericoneill 14:bda4a189cbe8 281 maxLoc = c;
ericoneill 14:bda4a189cbe8 282 }
ericoneill 14:bda4a189cbe8 283 }
ericoneill 14:bda4a189cbe8 284
ericoneill 14:bda4a189cbe8 285 for (int c = startWindow; c < endWindow; c++) {
ericoneill 14:bda4a189cbe8 286 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.007f){
ericoneill 14:bda4a189cbe8 287 maxAccum += c;
ericoneill 14:bda4a189cbe8 288 maxCount++;
ericoneill 14:bda4a189cbe8 289 if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
ericoneill 14:bda4a189cbe8 290 space = true;
ericoneill 14:bda4a189cbe8 291 }
ericoneill 14:bda4a189cbe8 292 prevTrackLoc = c;
ericoneill 14:bda4a189cbe8 293 }
ericoneill 14:bda4a189cbe8 294 }
ericoneill 14:bda4a189cbe8 295 }
ericoneill 14:bda4a189cbe8 296 /*
ericoneill 14:bda4a189cbe8 297 //Check if we need to alter integration time due to brightness
ericoneill 14:bda4a189cbe8 298 if (maxVal < 0.15f){
ericoneill 14:bda4a189cbe8 299 intTimMod += 10;
ericoneill 14:bda4a189cbe8 300 } else if (maxVal >= 1) {
ericoneill 14:bda4a189cbe8 301 if (intTimMod > 0) {
ericoneill 14:bda4a189cbe8 302 intTimMod -= 10;
ericoneill 14:bda4a189cbe8 303 }
ericoneill 14:bda4a189cbe8 304 }
ericoneill 14:bda4a189cbe8 305 */
ericoneill 14:bda4a189cbe8 306
ericoneill 14:bda4a189cbe8 307 //Line Crossing Checks
ericoneill 14:bda4a189cbe8 308 if (space) {
ericoneill 14:bda4a189cbe8 309 currDir = prevDir;
ericoneill 14:bda4a189cbe8 310 firstTime = true;
ericoneill 14:bda4a189cbe8 311 } else {
ericoneill 14:bda4a189cbe8 312 approxPos = (float)maxAccum/(float)maxCount;
ericoneill 14:bda4a189cbe8 313
ericoneill 14:bda4a189cbe8 314 if(dataCol){
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 15:d746c53bf49b 319 pdSteering[loopCtr] = PIDcontrol(approxPos);
ericoneill 14:bda4a189cbe8 320 loopCtr++;
ericoneill 14:bda4a189cbe8 321 }
ericoneill 14:bda4a189cbe8 322 }
ericoneill 14:bda4a189cbe8 323
ericoneill 14:bda4a189cbe8 324 currDir = hardLeft + (approxPos)/((float) 127)*(hardRight-hardLeft);
ericoneill 14:bda4a189cbe8 325 prevApproxPos = approxPos;
ericoneill 14:bda4a189cbe8 326
ericoneill 14:bda4a189cbe8 327 }
ericoneill 14:bda4a189cbe8 328
ericoneill 14:bda4a189cbe8 329 servo.pulsewidth(currDir);
ericoneill 14:bda4a189cbe8 330
ericoneill 14:bda4a189cbe8 331 //Start Velocity control after requisite number of encoder signals have been collected
ericoneill 14:bda4a189cbe8 332 //if(numInterrupts >= 4){
ericoneill 14:bda4a189cbe8 333 //velocity_control(0.1f, TUNING_CONSTANT_10);
ericoneill 14:bda4a189cbe8 334 //}
ericoneill 14:bda4a189cbe8 335
ericoneill 14:bda4a189cbe8 336 //Save current direction as previous direction
ericoneill 14:bda4a189cbe8 337 prevDir = currDir;
ericoneill 14:bda4a189cbe8 338
ericoneill 14:bda4a189cbe8 339 //Prepare to start collecting more data
ericoneill 14:bda4a189cbe8 340 integrationCounter = 150;
ericoneill 14:bda4a189cbe8 341
ericoneill 14:bda4a189cbe8 342 //Disable interrupts
ericoneill 14:bda4a189cbe8 343 //interrupt.fall(NULL);
ericoneill 14:bda4a189cbe8 344 //interrupt.rise(NULL);x
ericoneill 14:bda4a189cbe8 345
ericoneill 14:bda4a189cbe8 346 //Stop timer
ericoneill 14:bda4a189cbe8 347 //t.stop();
ericoneill 14:bda4a189cbe8 348 }
ericoneill 14:bda4a189cbe8 349 else{
ericoneill 14:bda4a189cbe8 350 clk = 1;
ericoneill 14:bda4a189cbe8 351 wait_us(intTimMod);
ericoneill 14:bda4a189cbe8 352 ADCdata[integrationCounter - 1] = camData;
ericoneill 14:bda4a189cbe8 353 clk = 0;
ericoneill 14:bda4a189cbe8 354 }
ericoneill 14:bda4a189cbe8 355
ericoneill 14:bda4a189cbe8 356 //clk = 0;
ericoneill 14:bda4a189cbe8 357 integrationCounter++;
ericoneill 14:bda4a189cbe8 358 if(dataCol){
ericoneill 14:bda4a189cbe8 359 loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime++;
ericoneill 14:bda4a189cbe8 360 }
ericoneill 14:bda4a189cbe8 361 //camData.
ericoneill 14:bda4a189cbe8 362
mawk2311 7:ea395616348c 363 }
ericoneill 14:bda4a189cbe8 364 if (dataCol){
ericoneill 14:bda4a189cbe8 365 //print frame data
ericoneill 14:bda4a189cbe8 366 pc.printf("printing frame data\n\r");
ericoneill 14:bda4a189cbe8 367 //int frameSize = frames.size();
ericoneill 14:bda4a189cbe8 368 //pc.printf("%i",frameSize);
ericoneill 14:bda4a189cbe8 369 pc.printf("[");
ericoneill 14:bda4a189cbe8 370 for(int i=0; i<numData; i++){
ericoneill 14:bda4a189cbe8 371 if(lineCenters > 0){
ericoneill 14:bda4a189cbe8 372 pc.printf("%i %i %f %f,",lineCenters[i], times[i], pSteering[i], pdSteering[i]);
ericoneill 14:bda4a189cbe8 373 }
ericoneill 14:bda4a189cbe8 374 }
ericoneill 14:bda4a189cbe8 375 pc.printf("]\n\r");
ericoneill 0:d328ecb3fbb1 376 }
ericoneill 0:d328ecb3fbb1 377 }
mawk2311 7:ea395616348c 378