E=MC / Mbed 2 deprecated coolcarsuperfast2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MODSERIAL telemetry-master

Fork of coolcarsuperfast by Michael Romain

main.cpp@25:dbadea3c526d, 2015-05-07 (annotated)

Committer:
cheryl_he
Date:
Thu May 07 01:23:06 2015 +0000
Revision:
25:dbadea3c526d
Parent:
24:7fc204a3d013
Child:
26:7439102e0d9f
BLUETOOTHWORKS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mawk2311 0:ad375c052b4c 1 #include "mbed.h"
cheryl_he 24:7fc204a3d013 2 #include "telemetry.h"
cheryl_he 24:7fc204a3d013 3 #include "telemetry-mbed.h"
cheryl_he 24:7fc204a3d013 4 #include "MODSERIAL.h"
cheryl_he 24:7fc204a3d013 5
mawk2311 0:ad375c052b4c 6 #include "stdlib.h"
ericoneill 7:6d5ddcf12cf3 7 #include <vector>
cheryl_he 24:7fc204a3d013 8
cheryl_he 25:dbadea3c526d 9 MODSERIAL telemetry_serial(PTA2, PTA1);
cheryl_he 24:7fc204a3d013 10 telemetry::MbedHal telemetry_hal(telemetry_serial);
cheryl_he 24:7fc204a3d013 11 telemetry::Telemetry telemetry_obj(telemetry_hal);
cheryl_he 24:7fc204a3d013 12
cheryl_he 24:7fc204a3d013 13 telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0);
cheryl_he 25:dbadea3c526d 14 telemetry::NumericArray<float, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0);
cheryl_he 24:7fc204a3d013 15 telemetry::Numeric<float> tele_motor_pwm(telemetry_obj, "motor", "Motor PWM", "%DC", 0);
cheryl_he 24:7fc204a3d013 16
mawk2311 0:ad375c052b4c 17 //Outputs
mawk2311 0:ad375c052b4c 18 DigitalOut led1(LED1);
mawk2311 0:ad375c052b4c 19 DigitalOut clk(PTD5);
mawk2311 0:ad375c052b4c 20 DigitalOut si(PTD4);
mawk2311 5:20223464f7aa 21 PwmOut motor1(PTA12);
mawk2311 5:20223464f7aa 22 PwmOut motor2(PTA4);
mawk2311 5:20223464f7aa 23 DigitalOut break1(PTC7);
mawk2311 5:20223464f7aa 24 DigitalOut break2(PTC0);
mawk2311 0:ad375c052b4c 25 PwmOut servo(PTA5);
mawk2311 0:ad375c052b4c 26
cheryl_he 25:dbadea3c526d 27 //Serial bt(PTA2, PTA1);
cheryl_he 24:7fc204a3d013 28 //Serial pc(USBTX, USBRX); // tx, rx
mawk2311 5:20223464f7aa 29
mawk2311 0:ad375c052b4c 30 //Inputs
mawk2311 0:ad375c052b4c 31 AnalogIn camData(PTC2);
mawk2311 0:ad375c052b4c 32
mawk2311 0:ad375c052b4c 33 //Encoder setup and variables
mawk2311 0:ad375c052b4c 34 InterruptIn interrupt(PTA13);
mawk2311 0:ad375c052b4c 35
mawk2311 0:ad375c052b4c 36 //Line Tracking Variables --------------------------------
mawk2311 0:ad375c052b4c 37 float ADCdata [128];
mawk2311 5:20223464f7aa 38 float maxAccum;
mawk2311 5:20223464f7aa 39 float maxCount;
mawk2311 0:ad375c052b4c 40 float approxPos;
mawk2311 12:48b76450c4b4 41 float prevApproxPos;
mawk2311 15:55e9fffc653a 42 int trackWindow = 30;
mawk2311 14:888495814f3c 43 int startWindow;
mawk2311 14:888495814f3c 44 int endWindow;
mawk2311 5:20223464f7aa 45 float maxVal;
mawk2311 5:20223464f7aa 46 int maxLoc;
mawk2311 12:48b76450c4b4 47
mawk2311 12:48b76450c4b4 48 bool firstTime;
mawk2311 11:b59ec039a712 49
mawk2311 10:e40ad924e935 50 //Data Collection
mawk2311 10:e40ad924e935 51 bool dataCol = false;
mawk2311 11:b59ec039a712 52 int loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime;
mawk2311 10:e40ad924e935 53
mawk2311 5:20223464f7aa 54 //Line Crossing variables
mawk2311 5:20223464f7aa 55 int prevTrackLoc;
mawk2311 18:7941524e0d28 56 int spaceThresh = 3;
mawk2311 18:7941524e0d28 57 int widthThresh = 10;
mawk2311 5:20223464f7aa 58 bool space;
mawk2311 0:ad375c052b4c 59
mawk2311 0:ad375c052b4c 60 //Servo turning parameters
mawk2311 0:ad375c052b4c 61 float straight = 0.00155f;
mawk2311 16:79106efd7a57 62 float hardLeft = 0.0012f;
mawk2311 16:79106efd7a57 63 float hardRight = 0.0020f;
mawk2311 12:48b76450c4b4 64 //float hardLeft = 0.0010f;
mawk2311 12:48b76450c4b4 65 //float hardRight = 0.00195f;
mawk2311 0:ad375c052b4c 66
mawk2311 5:20223464f7aa 67 //Servo Directions
mawk2311 5:20223464f7aa 68 float currDir;
mawk2311 5:20223464f7aa 69 float prevDir;
mawk2311 5:20223464f7aa 70
ericoneill 7:6d5ddcf12cf3 71 // All linescan data for the period the car is running on. To be printed after a set amount of time
ericoneill 7:6d5ddcf12cf3 72 //std::vector<std::vector<int> > frames;
ericoneill 7:6d5ddcf12cf3 73 const int numData = 1000;
ericoneill 7:6d5ddcf12cf3 74 int lineCenters [numData] = {0};
ericoneill 7:6d5ddcf12cf3 75 int times [numData] = {0};
ericoneill 7:6d5ddcf12cf3 76 int loopCtr = 0;
ericoneill 7:6d5ddcf12cf3 77
mawk2311 0:ad375c052b4c 78 //End of Line Tracking Variables -------------------------
mawk2311 0:ad375c052b4c 79
mawk2311 0:ad375c052b4c 80 //Encoder and Motor Driver Variables ---------------------
mawk2311 0:ad375c052b4c 81
mawk2311 0:ad375c052b4c 82 //Intervals used during encoder data collection to measure velocity
mawk2311 0:ad375c052b4c 83 int interval1=0;
mawk2311 0:ad375c052b4c 84 int interval2=0;
mawk2311 0:ad375c052b4c 85 int interval3=0;
mawk2311 0:ad375c052b4c 86 int avg_interval=0;
mawk2311 0:ad375c052b4c 87 int lastchange1 = 0;
mawk2311 0:ad375c052b4c 88 int lastchange2 = 0;
mawk2311 0:ad375c052b4c 89 int lastchange3 = 0;
mawk2311 0:ad375c052b4c 90 int lastchange4 = 0;
mawk2311 0:ad375c052b4c 91
mawk2311 0:ad375c052b4c 92 //Variables used to for velocity control
mawk2311 0:ad375c052b4c 93 float avg_speed = 0;
mawk2311 5:20223464f7aa 94 float last_speed = 0;
mawk2311 5:20223464f7aa 95
mawk2311 0:ad375c052b4c 96 float stall_check = 0;
mawk2311 0:ad375c052b4c 97 float tuning_val = 1;
mawk2311 0:ad375c052b4c 98
mawk2311 6:f1d948d2d6c1 99 int numInterrupts = 0;
mawk2311 5:20223464f7aa 100
mawk2311 19:d9746cc2ec80 101 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Tuning Values that can make it or break it ~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 23:bf38d7d2255a 102 float pulsewidth = 0.2f;
mawk2311 19:d9746cc2ec80 103 int intTimMod = 0;
mawk2311 23:bf38d7d2255a 104 float maxValThresh = .12; //~.1 for earlier in the day, reduce it (maybe something like .05 - .01 or something) as it gets darker
mawk2311 23:bf38d7d2255a 105 bool turnSpeedControl = true; //have increased PWMs when turning when true.
mawk2311 14:888495814f3c 106 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 16:79106efd7a57 107
mawk2311 5:20223464f7aa 108 // Hardware periods
mawk2311 5:20223464f7aa 109 float motorPeriod = .0025;
mawk2311 5:20223464f7aa 110 float servoPeriod = .0025;
mawk2311 5:20223464f7aa 111
mawk2311 0:ad375c052b4c 112 Timer t;
mawk2311 0:ad375c052b4c 113 Timer servoTimer;
ericoneill 7:6d5ddcf12cf3 114 Timer printTimer; //after printTimer reaches a certain value the main loop will terminate and print the frames
mawk2311 0:ad375c052b4c 115
mawk2311 0:ad375c052b4c 116 //End of Encoder and Motor Driver Variables ----------------------
mawk2311 0:ad375c052b4c 117
mawk2311 6:f1d948d2d6c1 118 //Function for speeding up KL25Z ADC
mawk2311 6:f1d948d2d6c1 119 void initADC(void){
mawk2311 6:f1d948d2d6c1 120
mawk2311 6:f1d948d2d6c1 121 ADC0->CFG1 = ADC0->CFG1 & (
mawk2311 6:f1d948d2d6c1 122 ~(
mawk2311 6:f1d948d2d6c1 123 0x80 // LDLPC = 0 ; no low-power mode
mawk2311 6:f1d948d2d6c1 124 | 0x60 // ADIV = 1
mawk2311 6:f1d948d2d6c1 125 | 0x10 // Sample time short
mawk2311 6:f1d948d2d6c1 126 | 0x03 // input clock = BUS CLK
mawk2311 6:f1d948d2d6c1 127 )
mawk2311 6:f1d948d2d6c1 128 ) ; // clkdiv <= 1
mawk2311 6:f1d948d2d6c1 129 ADC0->CFG2 = ADC0->CFG2
mawk2311 6:f1d948d2d6c1 130 | 0x03 ; // Logsample Time 11 = 2 extra ADCK
mawk2311 6:f1d948d2d6c1 131 ADC0->SC3 = ADC0->SC3
mawk2311 6:f1d948d2d6c1 132 & (~(0x03)) ; // hardware avarage off
mawk2311 6:f1d948d2d6c1 133 }
mawk2311 6:f1d948d2d6c1 134
mawk2311 0:ad375c052b4c 135 int main() {
cheryl_he 25:dbadea3c526d 136 bluetooth_power.write(1);
cheryl_he 24:7fc204a3d013 137 telemetry_serial.baud(115200);
cheryl_he 24:7fc204a3d013 138 telemetry_obj.transmit_header();
cheryl_he 24:7fc204a3d013 139
mawk2311 6:f1d948d2d6c1 140 //Alter reg values to speed up KL25Z
mawk2311 6:f1d948d2d6c1 141 initADC();
mawk2311 6:f1d948d2d6c1 142
mawk2311 0:ad375c052b4c 143 //Line Tracker Initializations
mawk2311 0:ad375c052b4c 144 int integrationCounter = 0;
mawk2311 0:ad375c052b4c 145
mawk2311 6:f1d948d2d6c1 146 //Initial values for directions
mawk2311 5:20223464f7aa 147 currDir = 0;
mawk2311 5:20223464f7aa 148 prevDir = 0;
mawk2311 5:20223464f7aa 149
mawk2311 0:ad375c052b4c 150 // Motor Driver Initializations
mawk2311 5:20223464f7aa 151 motor1.period(motorPeriod);
mawk2311 5:20223464f7aa 152 motor2.period(motorPeriod);
mawk2311 0:ad375c052b4c 153
mawk2311 5:20223464f7aa 154 // Servo Initialization
mawk2311 5:20223464f7aa 155 servo.period(servoPeriod);
mawk2311 15:55e9fffc653a 156 servo.pulsewidth(hardRight);
mawk2311 5:20223464f7aa 157 wait(3);
mawk2311 0:ad375c052b4c 158
mawk2311 12:48b76450c4b4 159 motor1.pulsewidth(motorPeriod*pulsewidth);
mawk2311 12:48b76450c4b4 160 motor2.pulsewidth(motorPeriod*pulsewidth);
mawk2311 0:ad375c052b4c 161 break1 = 0;
mawk2311 0:ad375c052b4c 162 break2 = 0;
mawk2311 0:ad375c052b4c 163
mawk2311 12:48b76450c4b4 164 firstTime = true;
mawk2311 12:48b76450c4b4 165
cheryl_he 24:7fc204a3d013 166 t.start();
mawk2311 10:e40ad924e935 167
mawk2311 10:e40ad924e935 168 if(dataCol){
mawk2311 11:b59ec039a712 169 loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime = 0;
mawk2311 10:e40ad924e935 170 printTimer.start();
mawk2311 10:e40ad924e935 171 }
cheryl_he 24:7fc204a3d013 172 //uint16_t* data = camData.read();
mawk2311 0:ad375c052b4c 173 while(1) {
mawk2311 10:e40ad924e935 174 if(dataCol){
mawk2311 10:e40ad924e935 175 //break out of main loop if enough time has passed;
mawk2311 10:e40ad924e935 176 if(loopCtr >= numData && dataCol){
mawk2311 10:e40ad924e935 177 break;
mawk2311 10:e40ad924e935 178 }
ericoneill 7:6d5ddcf12cf3 179 }
mawk2311 0:ad375c052b4c 180 if(integrationCounter % 151== 0){
mawk2311 10:e40ad924e935 181 /*
mawk2311 0:ad375c052b4c 182 //Disable interrupts
mawk2311 5:20223464f7aa 183 interrupt.fall(NULL);
mawk2311 5:20223464f7aa 184 interrupt.rise(NULL);
ericoneill 7:6d5ddcf12cf3 185 */
mawk2311 10:e40ad924e935 186
mawk2311 0:ad375c052b4c 187 //Send start of integration signal
mawk2311 0:ad375c052b4c 188 si = 1;
mawk2311 0:ad375c052b4c 189 clk = 1;
mawk2311 0:ad375c052b4c 190
mawk2311 0:ad375c052b4c 191 si = 0;
mawk2311 0:ad375c052b4c 192 clk = 0;
mawk2311 0:ad375c052b4c 193
mawk2311 0:ad375c052b4c 194 //Reset timing counter for integration
mawk2311 0:ad375c052b4c 195 integrationCounter = 0;
mawk2311 0:ad375c052b4c 196
mawk2311 0:ad375c052b4c 197 //Reset line tracking variables
mawk2311 5:20223464f7aa 198 maxAccum = 0;
mawk2311 5:20223464f7aa 199 maxCount = 0;
mawk2311 0:ad375c052b4c 200 approxPos = 0;
mawk2311 0:ad375c052b4c 201
mawk2311 5:20223464f7aa 202 space = false;
mawk2311 6:f1d948d2d6c1 203
mawk2311 0:ad375c052b4c 204 }
mawk2311 0:ad375c052b4c 205 else if (integrationCounter > 129){
mawk2311 5:20223464f7aa 206 //Start Timer
mawk2311 6:f1d948d2d6c1 207 //t.start();
mawk2311 0:ad375c052b4c 208
mawk2311 5:20223464f7aa 209 //Enable interrupts
mawk2311 6:f1d948d2d6c1 210 //interrupt.fall(&fallInterrupt);
mawk2311 6:f1d948d2d6c1 211 //interrupt.rise(&riseInterrupt);
cheryl_he 25:dbadea3c526d 212 tele_time_ms = t.read_ms();
cheryl_he 25:dbadea3c526d 213 for (uint16_t i=0; i<128; i++) {
cheryl_he 25:dbadea3c526d 214 tele_linescan[i] = ADCdata[i];
cheryl_he 25:dbadea3c526d 215 }
cheryl_he 25:dbadea3c526d 216 telemetry_obj.do_io();
mawk2311 12:48b76450c4b4 217 if (firstTime){
mawk2311 12:48b76450c4b4 218
mawk2311 12:48b76450c4b4 219 maxVal = ADCdata[10];
mawk2311 12:48b76450c4b4 220 for (int c = 11; c < 118; c++) {
mawk2311 12:48b76450c4b4 221 if (ADCdata[c] > maxVal){
mawk2311 12:48b76450c4b4 222 maxVal = ADCdata[c];
mawk2311 12:48b76450c4b4 223 maxLoc = c;
mawk2311 12:48b76450c4b4 224 }
mawk2311 0:ad375c052b4c 225 }
mawk2311 12:48b76450c4b4 226
mawk2311 12:48b76450c4b4 227 for (int c = 10; c < 118; c++) {
mawk2311 19:d9746cc2ec80 228 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < maxValThresh){
mawk2311 12:48b76450c4b4 229 maxAccum += c;
mawk2311 12:48b76450c4b4 230 maxCount++;
mawk2311 15:55e9fffc653a 231 if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
mawk2311 12:48b76450c4b4 232 space = true;
mawk2311 12:48b76450c4b4 233 }
mawk2311 12:48b76450c4b4 234 prevTrackLoc = c;
mawk2311 12:48b76450c4b4 235 }
mawk2311 12:48b76450c4b4 236 }
mawk2311 12:48b76450c4b4 237
mawk2311 16:79106efd7a57 238 //firstTime = false;
mawk2311 12:48b76450c4b4 239 } else {
mawk2311 12:48b76450c4b4 240
mawk2311 14:888495814f3c 241 startWindow = prevApproxPos - trackWindow;
mawk2311 14:888495814f3c 242 endWindow = prevApproxPos + trackWindow;
mawk2311 14:888495814f3c 243 if (startWindow < 0){
mawk2311 14:888495814f3c 244 startWindow = 0;
mawk2311 14:888495814f3c 245 }
mawk2311 14:888495814f3c 246 if (endWindow > 118){
mawk2311 14:888495814f3c 247 endWindow = 118;
mawk2311 14:888495814f3c 248 }
mawk2311 12:48b76450c4b4 249 maxVal = ADCdata[10];
mawk2311 14:888495814f3c 250 for (int c = startWindow; c < endWindow; c++) {
mawk2311 12:48b76450c4b4 251 if (ADCdata[c] > maxVal){
mawk2311 12:48b76450c4b4 252 maxVal = ADCdata[c];
mawk2311 12:48b76450c4b4 253 maxLoc = c;
mawk2311 12:48b76450c4b4 254 }
mawk2311 12:48b76450c4b4 255 }
mawk2311 12:48b76450c4b4 256
mawk2311 14:888495814f3c 257 for (int c = startWindow; c < endWindow; c++) {
mawk2311 19:d9746cc2ec80 258 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < maxValThresh){
mawk2311 11:b59ec039a712 259 maxAccum += c;
mawk2311 11:b59ec039a712 260 maxCount++;
mawk2311 15:55e9fffc653a 261 if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
mawk2311 11:b59ec039a712 262 space = true;
mawk2311 11:b59ec039a712 263 }
mawk2311 11:b59ec039a712 264 prevTrackLoc = c;
mawk2311 5:20223464f7aa 265 }
mawk2311 12:48b76450c4b4 266 }
mawk2311 11:b59ec039a712 267 }
mawk2311 12:48b76450c4b4 268 /*
mawk2311 11:b59ec039a712 269 //Check if we need to alter integration time due to brightness
mawk2311 11:b59ec039a712 270 if (maxVal < 0.15f){
mawk2311 11:b59ec039a712 271 intTimMod += 10;
mawk2311 11:b59ec039a712 272 } else if (maxVal >= 1) {
mawk2311 11:b59ec039a712 273 if (intTimMod > 0) {
mawk2311 11:b59ec039a712 274 intTimMod -= 10;
mawk2311 0:ad375c052b4c 275 }
mawk2311 0:ad375c052b4c 276 }
mawk2311 12:48b76450c4b4 277 */
mawk2311 11:b59ec039a712 278
mawk2311 6:f1d948d2d6c1 279 //Line Crossing Checks
mawk2311 10:e40ad924e935 280 if (space) {
mawk2311 5:20223464f7aa 281 currDir = prevDir;
mawk2311 15:55e9fffc653a 282 firstTime = true;
mawk2311 5:20223464f7aa 283 } else {
mawk2311 15:55e9fffc653a 284 approxPos = (float)maxAccum/(float)maxCount;
ericoneill 8:e126c900c89d 285
mawk2311 10:e40ad924e935 286 if(dataCol){
mawk2311 10:e40ad924e935 287 if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%3==0){
mawk2311 10:e40ad924e935 288 lineCenters[loopCtr] = approxPos;
mawk2311 10:e40ad924e935 289 times[loopCtr] = printTimer.read_ms();
mawk2311 10:e40ad924e935 290 loopCtr++;
mawk2311 10:e40ad924e935 291 }
ericoneill 9:ad08181ad1cc 292 }
mawk2311 10:e40ad924e935 293
mawk2311 16:79106efd7a57 294 currDir = hardLeft + (approxPos)/((float) 118)*(hardRight-hardLeft);
mawk2311 12:48b76450c4b4 295 prevApproxPos = approxPos;
ericoneill 9:ad08181ad1cc 296
mawk2311 5:20223464f7aa 297 }
mawk2311 16:79106efd7a57 298
mawk2311 16:79106efd7a57 299 if (turnSpeedControl){
mawk2311 16:79106efd7a57 300 //Change speed when turning at different angles
mawk2311 16:79106efd7a57 301 if(approxPos > 0 && approxPos <= 45){
mawk2311 19:d9746cc2ec80 302 motor1.pulsewidth(motorPeriod*(pulsewidth*2.0f));
mawk2311 19:d9746cc2ec80 303 motor2.pulsewidth(motorPeriod*(pulsewidth*2.0f));
mawk2311 19:d9746cc2ec80 304 } else if (approxPos > 45 && approxPos <= 55){
mawk2311 19:d9746cc2ec80 305 motor1.pulsewidth(motorPeriod*pulsewidth*1.5f);
mawk2311 19:d9746cc2ec80 306 motor2.pulsewidth(motorPeriod*pulsewidth*1.5f);
mawk2311 19:d9746cc2ec80 307 } else if (approxPos > 55 && approxPos <= 85){
mawk2311 16:79106efd7a57 308 motor1.pulsewidth(motorPeriod*pulsewidth);
mawk2311 16:79106efd7a57 309 motor2.pulsewidth(motorPeriod*pulsewidth);
mawk2311 19:d9746cc2ec80 310 } else if (approxPos > 85 && approxPos <= 95){
mawk2311 19:d9746cc2ec80 311 motor1.pulsewidth(motorPeriod*pulsewidth*1.5f);
mawk2311 19:d9746cc2ec80 312 motor2.pulsewidth(motorPeriod*pulsewidth*1.5f);
mawk2311 16:79106efd7a57 313 } else {
mawk2311 19:d9746cc2ec80 314 motor1.pulsewidth(motorPeriod*(pulsewidth*2.0f));
mawk2311 19:d9746cc2ec80 315 motor2.pulsewidth(motorPeriod*(pulsewidth*2.0f));
mawk2311 16:79106efd7a57 316 }
mawk2311 16:79106efd7a57 317 }
mawk2311 16:79106efd7a57 318
mawk2311 5:20223464f7aa 319 servo.pulsewidth(currDir);
mawk2311 5:20223464f7aa 320
mawk2311 6:f1d948d2d6c1 321 //Start Velocity control after requisite number of encoder signals have been collected
mawk2311 6:f1d948d2d6c1 322 //if(numInterrupts >= 4){
mawk2311 6:f1d948d2d6c1 323 //velocity_control(0.1f, TUNING_CONSTANT_10);
mawk2311 6:f1d948d2d6c1 324 //}
mawk2311 4:09c68df71785 325
mawk2311 6:f1d948d2d6c1 326 //Save current direction as previous direction
mawk2311 5:20223464f7aa 327 prevDir = currDir;
mawk2311 0:ad375c052b4c 328
mawk2311 6:f1d948d2d6c1 329 //Prepare to start collecting more data
mawk2311 0:ad375c052b4c 330 integrationCounter = 150;
mawk2311 6:f1d948d2d6c1 331
mawk2311 6:f1d948d2d6c1 332 //Disable interrupts
mawk2311 6:f1d948d2d6c1 333 //interrupt.fall(NULL);
mawk2311 6:f1d948d2d6c1 334 //interrupt.rise(NULL);
mawk2311 6:f1d948d2d6c1 335
mawk2311 6:f1d948d2d6c1 336 //Stop timer
mawk2311 6:f1d948d2d6c1 337 //t.stop();
mawk2311 0:ad375c052b4c 338 }
mawk2311 0:ad375c052b4c 339 else{
mawk2311 0:ad375c052b4c 340 clk = 1;
mawk2311 14:888495814f3c 341 wait_us(intTimMod);
mawk2311 3:e867c4e984df 342 ADCdata[integrationCounter - 1] = camData;
mawk2311 0:ad375c052b4c 343 clk = 0;
mawk2311 0:ad375c052b4c 344 }
mawk2311 0:ad375c052b4c 345
mawk2311 0:ad375c052b4c 346 //clk = 0;
mawk2311 0:ad375c052b4c 347 integrationCounter++;
mawk2311 10:e40ad924e935 348 if(dataCol){
mawk2311 10:e40ad924e935 349 loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime++;
mawk2311 10:e40ad924e935 350 }
mawk2311 0:ad375c052b4c 351 //camData.
cheryl_he 25:dbadea3c526d 352
mawk2311 0:ad375c052b4c 353
mawk2311 0:ad375c052b4c 354 }
cheryl_he 24:7fc204a3d013 355 /*
mawk2311 10:e40ad924e935 356 if (dataCol){
mawk2311 10:e40ad924e935 357 //print frame data
mawk2311 10:e40ad924e935 358 pc.printf("printing frame data\n\r");
mawk2311 10:e40ad924e935 359 //int frameSize = frames.size();
mawk2311 10:e40ad924e935 360 //pc.printf("%i",frameSize);
mawk2311 10:e40ad924e935 361 pc.printf("[");
mawk2311 10:e40ad924e935 362 for(int i=0; i<numData; i++){
mawk2311 10:e40ad924e935 363 if(lineCenters > 0){
mawk2311 10:e40ad924e935 364 pc.printf("%i %i,",lineCenters[i], times[i]);
mawk2311 10:e40ad924e935 365 }
ericoneill 7:6d5ddcf12cf3 366 }
mawk2311 10:e40ad924e935 367 pc.printf("]\n\r");
ericoneill 7:6d5ddcf12cf3 368 }
cheryl_he 24:7fc204a3d013 369 */
mawk2311 0:ad375c052b4c 370 }
ericoneill 9:ad08181ad1cc 371