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@16:79106efd7a57, 2015-04-11 (annotated)

Committer:
mawk2311
Date:
Sat Apr 11 00:33:05 2015 +0000
Revision:
16:79106efd7a57
Parent:
15:55e9fffc653a
Child:
17:28a1f9309fdb
Child:
20:d96f46dea035
Pretty speedy code for outside checkoff.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mawk2311 0:ad375c052b4c 1 #include "mbed.h"
mawk2311 0:ad375c052b4c 2 #include "stdlib.h"
ericoneill 7:6d5ddcf12cf3 3 #include <vector>
mawk2311 0:ad375c052b4c 4 //Outputs
mawk2311 0:ad375c052b4c 5 DigitalOut led1(LED1);
mawk2311 0:ad375c052b4c 6 DigitalOut clk(PTD5);
mawk2311 0:ad375c052b4c 7 DigitalOut si(PTD4);
mawk2311 5:20223464f7aa 8 PwmOut motor1(PTA12);
mawk2311 5:20223464f7aa 9 PwmOut motor2(PTA4);
mawk2311 5:20223464f7aa 10 DigitalOut break1(PTC7);
mawk2311 5:20223464f7aa 11 DigitalOut break2(PTC0);
mawk2311 0:ad375c052b4c 12 PwmOut servo(PTA5);
mawk2311 0:ad375c052b4c 13
mawk2311 5:20223464f7aa 14 Serial pc(USBTX, USBRX); // tx, rx
mawk2311 5:20223464f7aa 15
mawk2311 0:ad375c052b4c 16 //Inputs
mawk2311 0:ad375c052b4c 17 AnalogIn camData(PTC2);
mawk2311 0:ad375c052b4c 18
mawk2311 0:ad375c052b4c 19 //Encoder setup and variables
mawk2311 0:ad375c052b4c 20 InterruptIn interrupt(PTA13);
mawk2311 0:ad375c052b4c 21
mawk2311 0:ad375c052b4c 22 //Line Tracking Variables --------------------------------
mawk2311 0:ad375c052b4c 23 float ADCdata [128];
mawk2311 5:20223464f7aa 24 float maxAccum;
mawk2311 5:20223464f7aa 25 float maxCount;
mawk2311 0:ad375c052b4c 26 float approxPos;
mawk2311 12:48b76450c4b4 27 float prevApproxPos;
mawk2311 15:55e9fffc653a 28 int trackWindow = 30;
mawk2311 14:888495814f3c 29 int startWindow;
mawk2311 14:888495814f3c 30 int endWindow;
mawk2311 5:20223464f7aa 31 float maxVal;
mawk2311 5:20223464f7aa 32 int maxLoc;
mawk2311 12:48b76450c4b4 33
mawk2311 12:48b76450c4b4 34 bool firstTime;
mawk2311 11:b59ec039a712 35
mawk2311 10:e40ad924e935 36 //Data Collection
mawk2311 10:e40ad924e935 37 bool dataCol = false;
mawk2311 11:b59ec039a712 38 int loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime;
mawk2311 10:e40ad924e935 39
mawk2311 5:20223464f7aa 40 //Line Crossing variables
mawk2311 5:20223464f7aa 41 int prevTrackLoc;
mawk2311 15:55e9fffc653a 42 int spaceThresh = 1;
mawk2311 14:888495814f3c 43 int widthThresh = 10;
mawk2311 5:20223464f7aa 44 bool space;
mawk2311 0:ad375c052b4c 45
mawk2311 0:ad375c052b4c 46 //Servo turning parameters
mawk2311 0:ad375c052b4c 47 float straight = 0.00155f;
mawk2311 16:79106efd7a57 48 float hardLeft = 0.0012f;
mawk2311 16:79106efd7a57 49 float hardRight = 0.0020f;
mawk2311 12:48b76450c4b4 50 //float hardLeft = 0.0010f;
mawk2311 12:48b76450c4b4 51 //float hardRight = 0.00195f;
mawk2311 0:ad375c052b4c 52
mawk2311 5:20223464f7aa 53 //Servo Directions
mawk2311 5:20223464f7aa 54 float currDir;
mawk2311 5:20223464f7aa 55 float prevDir;
mawk2311 5:20223464f7aa 56
ericoneill 7:6d5ddcf12cf3 57 // All linescan data for the period the car is running on. To be printed after a set amount of time
ericoneill 7:6d5ddcf12cf3 58 //std::vector<std::vector<int> > frames;
ericoneill 7:6d5ddcf12cf3 59 const int numData = 1000;
ericoneill 7:6d5ddcf12cf3 60 int lineCenters [numData] = {0};
ericoneill 7:6d5ddcf12cf3 61 int times [numData] = {0};
ericoneill 7:6d5ddcf12cf3 62 int loopCtr = 0;
ericoneill 7:6d5ddcf12cf3 63
mawk2311 0:ad375c052b4c 64 //End of Line Tracking Variables -------------------------
mawk2311 0:ad375c052b4c 65
mawk2311 0:ad375c052b4c 66 //Encoder and Motor Driver Variables ---------------------
mawk2311 0:ad375c052b4c 67
mawk2311 0:ad375c052b4c 68 //Intervals used during encoder data collection to measure velocity
mawk2311 0:ad375c052b4c 69 int interval1=0;
mawk2311 0:ad375c052b4c 70 int interval2=0;
mawk2311 0:ad375c052b4c 71 int interval3=0;
mawk2311 0:ad375c052b4c 72 int avg_interval=0;
mawk2311 0:ad375c052b4c 73 int lastchange1 = 0;
mawk2311 0:ad375c052b4c 74 int lastchange2 = 0;
mawk2311 0:ad375c052b4c 75 int lastchange3 = 0;
mawk2311 0:ad375c052b4c 76 int lastchange4 = 0;
mawk2311 0:ad375c052b4c 77
mawk2311 0:ad375c052b4c 78 //Variables used to for velocity control
mawk2311 0:ad375c052b4c 79 float avg_speed = 0;
mawk2311 5:20223464f7aa 80 float last_speed = 0;
mawk2311 5:20223464f7aa 81
mawk2311 0:ad375c052b4c 82 float stall_check = 0;
mawk2311 0:ad375c052b4c 83 float tuning_val = 1;
mawk2311 0:ad375c052b4c 84
mawk2311 6:f1d948d2d6c1 85 int numInterrupts = 0;
mawk2311 5:20223464f7aa 86
mawk2311 14:888495814f3c 87 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~PWM & Integration Time ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 16:79106efd7a57 88 float pulsewidth = 0.20f;
mawk2311 16:79106efd7a57 89 int intTimMod = 20;
mawk2311 14:888495814f3c 90 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 16:79106efd7a57 91 bool turnSpeedControl = true;
mawk2311 16:79106efd7a57 92
mawk2311 5:20223464f7aa 93 // Hardware periods
mawk2311 5:20223464f7aa 94 float motorPeriod = .0025;
mawk2311 5:20223464f7aa 95 float servoPeriod = .0025;
mawk2311 5:20223464f7aa 96
mawk2311 0:ad375c052b4c 97 Timer t;
mawk2311 0:ad375c052b4c 98 Timer servoTimer;
ericoneill 7:6d5ddcf12cf3 99 Timer printTimer; //after printTimer reaches a certain value the main loop will terminate and print the frames
mawk2311 0:ad375c052b4c 100
mawk2311 0:ad375c052b4c 101 //Observed average speeds for each duty cycle
mawk2311 5:20223464f7aa 102 const float DESIRED_SPEED = 1;
mawk2311 5:20223464f7aa 103 const float TUNING_CONSTANT_10 = 1.90;
mawk2311 0:ad375c052b4c 104 const float TUNING_CONSTANT_20 = 3.00;
mawk2311 0:ad375c052b4c 105 const float TUNING_CONSTANT_30 = 4.30;
mawk2311 0:ad375c052b4c 106 const float TUNING_CONSTANT_50 = 6.880;
mawk2311 0:ad375c052b4c 107 const float PI = 3.14159;
mawk2311 0:ad375c052b4c 108 const float WHEEL_CIRCUMFERENCE = .05*PI;
mawk2311 0:ad375c052b4c 109
mawk2311 0:ad375c052b4c 110 //Velocity Control Tuning Constants
mawk2311 0:ad375c052b4c 111 const float TUNE_THRESH = 0.5f;
mawk2311 0:ad375c052b4c 112 const float TUNE_AMT = 0.1f;
mawk2311 0:ad375c052b4c 113
mawk2311 0:ad375c052b4c 114 //Parameters specifying sample sizes and delays for small and large average speed samples
mawk2311 0:ad375c052b4c 115 float num_samples_small = 3.0f;
mawk2311 0:ad375c052b4c 116 float delay_small = 0.05f;
mawk2311 0:ad375c052b4c 117 float num_samples_large = 100.0f;
mawk2311 0:ad375c052b4c 118 float delay_large = 0.1f;
mawk2311 0:ad375c052b4c 119
mawk2311 0:ad375c052b4c 120 //Large and small arrays used to get average velocity values
mawk2311 0:ad375c052b4c 121 float large_avg_speed_list [100];
mawk2311 0:ad375c052b4c 122 float small_avg_speed_list [10];
mawk2311 0:ad375c052b4c 123
mawk2311 0:ad375c052b4c 124 //End of Encoder and Motor Driver Variables ----------------------
mawk2311 0:ad375c052b4c 125
mawk2311 1:55e0aaf71bda 126 //Line Tracker Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 1:55e0aaf71bda 127
mawk2311 1:55e0aaf71bda 128 int find_track(float line[]){
mawk2311 1:55e0aaf71bda 129 int track_location = -1;
mawk2311 1:55e0aaf71bda 130 float slope_threshold = .05;
mawk2311 1:55e0aaf71bda 131 bool downslope_indices [128] = {false};
mawk2311 1:55e0aaf71bda 132 bool upslope_indices [128] = {false};
mawk2311 1:55e0aaf71bda 133 for(int i=10; i<118; i++){
mawk2311 1:55e0aaf71bda 134 if(line[i+1] - line[i] < -slope_threshold && line[i+2] - line[i+1] < -slope_threshold){
mawk2311 1:55e0aaf71bda 135 downslope_indices[i] = true;
mawk2311 1:55e0aaf71bda 136 }
mawk2311 1:55e0aaf71bda 137 if(line[i+1] - line[i] > slope_threshold && line[i+2] - line[i+1] > slope_threshold){
mawk2311 1:55e0aaf71bda 138 upslope_indices[i] = true;
mawk2311 1:55e0aaf71bda 139 }
mawk2311 1:55e0aaf71bda 140 }
mawk2311 1:55e0aaf71bda 141 int numDownslopes = 0;
mawk2311 1:55e0aaf71bda 142 int numUpslopes = 0;
mawk2311 1:55e0aaf71bda 143 for(int i=0; i<128; i++){
mawk2311 1:55e0aaf71bda 144 if(downslope_indices[i] == true){
mawk2311 1:55e0aaf71bda 145 numDownslopes ++;
mawk2311 1:55e0aaf71bda 146 }
mawk2311 1:55e0aaf71bda 147 if(upslope_indices[i] == true){
mawk2311 1:55e0aaf71bda 148 numUpslopes ++;
mawk2311 1:55e0aaf71bda 149 }
mawk2311 1:55e0aaf71bda 150 }
mawk2311 1:55e0aaf71bda 151 int downslope_locs [numDownslopes];
mawk2311 1:55e0aaf71bda 152 int upslope_locs [numUpslopes];
mawk2311 1:55e0aaf71bda 153 int dsctr = 0;
mawk2311 1:55e0aaf71bda 154 int usctr = 0;
mawk2311 1:55e0aaf71bda 155
mawk2311 1:55e0aaf71bda 156 for (int i=0; i<128; i++){
mawk2311 1:55e0aaf71bda 157 if(downslope_indices[i] == true){
mawk2311 1:55e0aaf71bda 158 downslope_locs[dsctr] = i;
mawk2311 1:55e0aaf71bda 159 dsctr++;
mawk2311 1:55e0aaf71bda 160 }
mawk2311 1:55e0aaf71bda 161 if(upslope_indices[i] == true){
mawk2311 1:55e0aaf71bda 162 upslope_locs[usctr] = i;
mawk2311 1:55e0aaf71bda 163 usctr++;
mawk2311 1:55e0aaf71bda 164 }
mawk2311 1:55e0aaf71bda 165 }
mawk2311 1:55e0aaf71bda 166
mawk2311 1:55e0aaf71bda 167 for(int i=0; i<numDownslopes; i++){
mawk2311 1:55e0aaf71bda 168 for(int j=0; j<numUpslopes; j++){
mawk2311 1:55e0aaf71bda 169 if(upslope_locs[j] - downslope_locs[i] >=4 && upslope_locs[j] - downslope_locs[i] <=5){
mawk2311 1:55e0aaf71bda 170 track_location = downslope_locs[i] + 2 ;
mawk2311 1:55e0aaf71bda 171 }
mawk2311 1:55e0aaf71bda 172 }
mawk2311 1:55e0aaf71bda 173 }
mawk2311 1:55e0aaf71bda 174
mawk2311 1:55e0aaf71bda 175 return track_location;
mawk2311 1:55e0aaf71bda 176 }
mawk2311 1:55e0aaf71bda 177
mawk2311 6:f1d948d2d6c1 178 //Function for speeding up KL25Z ADC
mawk2311 6:f1d948d2d6c1 179 void initADC(void){
mawk2311 6:f1d948d2d6c1 180
mawk2311 6:f1d948d2d6c1 181 ADC0->CFG1 = ADC0->CFG1 & (
mawk2311 6:f1d948d2d6c1 182 ~(
mawk2311 6:f1d948d2d6c1 183 0x80 // LDLPC = 0 ; no low-power mode
mawk2311 6:f1d948d2d6c1 184 | 0x60 // ADIV = 1
mawk2311 6:f1d948d2d6c1 185 | 0x10 // Sample time short
mawk2311 6:f1d948d2d6c1 186 | 0x03 // input clock = BUS CLK
mawk2311 6:f1d948d2d6c1 187 )
mawk2311 6:f1d948d2d6c1 188 ) ; // clkdiv <= 1
mawk2311 6:f1d948d2d6c1 189 ADC0->CFG2 = ADC0->CFG2
mawk2311 6:f1d948d2d6c1 190 | 0x03 ; // Logsample Time 11 = 2 extra ADCK
mawk2311 6:f1d948d2d6c1 191 ADC0->SC3 = ADC0->SC3
mawk2311 6:f1d948d2d6c1 192 & (~(0x03)) ; // hardware avarage off
mawk2311 6:f1d948d2d6c1 193 }
mawk2311 6:f1d948d2d6c1 194
mawk2311 1:55e0aaf71bda 195
mawk2311 0:ad375c052b4c 196 // Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 0:ad375c052b4c 197
mawk2311 0:ad375c052b4c 198 float get_speed(){
mawk2311 0:ad375c052b4c 199 float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
mawk2311 0:ad375c052b4c 200 float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
mawk2311 0:ad375c052b4c 201 return linearSpeed;
mawk2311 0:ad375c052b4c 202 }
mawk2311 0:ad375c052b4c 203
mawk2311 0:ad375c052b4c 204 float get_avg_speed(float num_samples, float delay) {
mawk2311 0:ad375c052b4c 205
mawk2311 0:ad375c052b4c 206 float avg_avg_speed = 0;
mawk2311 0:ad375c052b4c 207
mawk2311 0:ad375c052b4c 208 for (int c = 0; c < num_samples; c++) {
mawk2311 0:ad375c052b4c 209 if (num_samples == num_samples_small){
mawk2311 0:ad375c052b4c 210 small_avg_speed_list[c] = get_speed();
mawk2311 0:ad375c052b4c 211 } else if (num_samples == num_samples_large){
mawk2311 0:ad375c052b4c 212 large_avg_speed_list[c] = get_speed();
mawk2311 0:ad375c052b4c 213 }
mawk2311 0:ad375c052b4c 214 //wait(delay);
mawk2311 0:ad375c052b4c 215 }
mawk2311 0:ad375c052b4c 216
mawk2311 0:ad375c052b4c 217 for (int c = 1; c < num_samples; c++) {
mawk2311 0:ad375c052b4c 218 if (num_samples == num_samples_small){
mawk2311 0:ad375c052b4c 219 avg_avg_speed += small_avg_speed_list[c];
mawk2311 0:ad375c052b4c 220 } else if (num_samples == num_samples_large){
mawk2311 0:ad375c052b4c 221 avg_avg_speed += large_avg_speed_list[c];
mawk2311 0:ad375c052b4c 222 }
mawk2311 0:ad375c052b4c 223 }
mawk2311 0:ad375c052b4c 224 return avg_avg_speed/num_samples;
mawk2311 0:ad375c052b4c 225 }
mawk2311 0:ad375c052b4c 226
mawk2311 0:ad375c052b4c 227 void velocity_control(float duty_cyc, float tuning_const) {
mawk2311 0:ad375c052b4c 228
mawk2311 0:ad375c052b4c 229 avg_speed = get_speed();//get_avg_speed(num_samples_small, delay_small);
mawk2311 0:ad375c052b4c 230
mawk2311 5:20223464f7aa 231 //When determined speed is infinity or 0, set the speed to the last agreeable speed
mawk2311 6:f1d948d2d6c1 232 /*if (avg_speed > 100 || avg_speed == 0){
mawk2311 5:20223464f7aa 233 avg_speed = last_speed;
mawk2311 6:f1d948d2d6c1 234 }*/
mawk2311 5:20223464f7aa 235
mawk2311 5:20223464f7aa 236 pc.printf("\n\r%f", avg_speed);
mawk2311 5:20223464f7aa 237 if (avg_speed == stall_check && tuning_const != 0 && avg_speed == 0) {
mawk2311 0:ad375c052b4c 238 avg_speed = 0;
mawk2311 0:ad375c052b4c 239 tuning_val += TUNE_AMT;
mawk2311 0:ad375c052b4c 240 } else if((avg_speed - tuning_const) > TUNE_THRESH){
mawk2311 0:ad375c052b4c 241 tuning_val -= TUNE_AMT;
mawk2311 0:ad375c052b4c 242 stall_check = avg_speed;
mawk2311 5:20223464f7aa 243
mawk2311 5:20223464f7aa 244 } else if (tuning_const - avg_speed > TUNE_THRESH && avg_speed != 0){
mawk2311 5:20223464f7aa 245 tuning_val += TUNE_AMT;
mawk2311 0:ad375c052b4c 246 stall_check = avg_speed;
mawk2311 0:ad375c052b4c 247 } else {
mawk2311 0:ad375c052b4c 248 //tuning_val = 1;
mawk2311 0:ad375c052b4c 249 stall_check = avg_speed;
mawk2311 0:ad375c052b4c 250 }
mawk2311 0:ad375c052b4c 251
mawk2311 5:20223464f7aa 252 if (tuning_val < .5){
mawk2311 5:20223464f7aa 253 tuning_val = .5;
mawk2311 5:20223464f7aa 254 }
mawk2311 5:20223464f7aa 255 pc.printf("\n\rTuning Val: %f", tuning_val);
mawk2311 5:20223464f7aa 256 motor1.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
mawk2311 5:20223464f7aa 257 motor2.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
mawk2311 5:20223464f7aa 258
mawk2311 5:20223464f7aa 259 if (avg_speed != 0){
mawk2311 5:20223464f7aa 260 last_speed = avg_speed;
mawk2311 5:20223464f7aa 261 }
mawk2311 5:20223464f7aa 262
mawk2311 0:ad375c052b4c 263 }
mawk2311 0:ad375c052b4c 264
mawk2311 6:f1d948d2d6c1 265 // Interrupt Functions for Encoder ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~``
mawk2311 0:ad375c052b4c 266 void fallInterrupt(){
mawk2311 0:ad375c052b4c 267
mawk2311 0:ad375c052b4c 268 int time = t.read_us();
mawk2311 0:ad375c052b4c 269 interval1 = time - lastchange2;
mawk2311 0:ad375c052b4c 270 interval2 = lastchange1-lastchange3;
mawk2311 0:ad375c052b4c 271 interval3 = lastchange2 - lastchange4;
mawk2311 0:ad375c052b4c 272 avg_interval = (interval1 + interval2 + interval3)/3;
mawk2311 0:ad375c052b4c 273
mawk2311 0:ad375c052b4c 274 lastchange4 = lastchange3;
mawk2311 0:ad375c052b4c 275 lastchange3 = lastchange2;
mawk2311 0:ad375c052b4c 276 lastchange2 = lastchange1;
mawk2311 0:ad375c052b4c 277 lastchange1 = time;
mawk2311 6:f1d948d2d6c1 278
mawk2311 6:f1d948d2d6c1 279 numInterrupts++;
mawk2311 0:ad375c052b4c 280 }
mawk2311 0:ad375c052b4c 281
mawk2311 0:ad375c052b4c 282 void riseInterrupt(){
mawk2311 0:ad375c052b4c 283 int time = t.read_us();
mawk2311 0:ad375c052b4c 284 interval1 = time - lastchange2;
mawk2311 0:ad375c052b4c 285 interval2 = lastchange1-lastchange3;
mawk2311 0:ad375c052b4c 286 interval3 = lastchange2 - lastchange4;
mawk2311 0:ad375c052b4c 287 avg_interval = (interval1 + interval2 + interval3)/3;
mawk2311 0:ad375c052b4c 288
mawk2311 0:ad375c052b4c 289 lastchange4 = lastchange3;
mawk2311 0:ad375c052b4c 290 lastchange3 = lastchange2;
mawk2311 0:ad375c052b4c 291 lastchange2 = lastchange1;
mawk2311 0:ad375c052b4c 292 lastchange1 = time;
mawk2311 6:f1d948d2d6c1 293
mawk2311 6:f1d948d2d6c1 294 numInterrupts++;
mawk2311 0:ad375c052b4c 295 }
mawk2311 0:ad375c052b4c 296
mawk2311 0:ad375c052b4c 297
mawk2311 0:ad375c052b4c 298 //End of Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 0:ad375c052b4c 299
mawk2311 0:ad375c052b4c 300 int main() {
mawk2311 6:f1d948d2d6c1 301
mawk2311 6:f1d948d2d6c1 302 //Alter reg values to speed up KL25Z
mawk2311 6:f1d948d2d6c1 303 initADC();
mawk2311 6:f1d948d2d6c1 304
mawk2311 0:ad375c052b4c 305 //Line Tracker Initializations
mawk2311 0:ad375c052b4c 306 int integrationCounter = 0;
mawk2311 0:ad375c052b4c 307
mawk2311 6:f1d948d2d6c1 308 //Initial values for directions
mawk2311 5:20223464f7aa 309 currDir = 0;
mawk2311 5:20223464f7aa 310 prevDir = 0;
mawk2311 5:20223464f7aa 311
mawk2311 0:ad375c052b4c 312 // Motor Driver Initializations
mawk2311 5:20223464f7aa 313 motor1.period(motorPeriod);
mawk2311 5:20223464f7aa 314 motor2.period(motorPeriod);
mawk2311 0:ad375c052b4c 315
mawk2311 5:20223464f7aa 316 // Servo Initialization
mawk2311 5:20223464f7aa 317 servo.period(servoPeriod);
mawk2311 15:55e9fffc653a 318 servo.pulsewidth(hardRight);
mawk2311 5:20223464f7aa 319 wait(3);
mawk2311 0:ad375c052b4c 320
mawk2311 12:48b76450c4b4 321 motor1.pulsewidth(motorPeriod*pulsewidth);
mawk2311 12:48b76450c4b4 322 motor2.pulsewidth(motorPeriod*pulsewidth);
mawk2311 0:ad375c052b4c 323 break1 = 0;
mawk2311 0:ad375c052b4c 324 break2 = 0;
mawk2311 0:ad375c052b4c 325
mawk2311 12:48b76450c4b4 326 firstTime = true;
mawk2311 12:48b76450c4b4 327
mawk2311 10:e40ad924e935 328 //t.start();
mawk2311 10:e40ad924e935 329
mawk2311 10:e40ad924e935 330 if(dataCol){
mawk2311 11:b59ec039a712 331 loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime = 0;
mawk2311 10:e40ad924e935 332 printTimer.start();
mawk2311 10:e40ad924e935 333 }
ericoneill 8:e126c900c89d 334
mawk2311 0:ad375c052b4c 335 while(1) {
mawk2311 10:e40ad924e935 336 if(dataCol){
mawk2311 10:e40ad924e935 337 //break out of main loop if enough time has passed;
mawk2311 10:e40ad924e935 338 if(loopCtr >= numData && dataCol){
mawk2311 10:e40ad924e935 339 break;
mawk2311 10:e40ad924e935 340 }
ericoneill 7:6d5ddcf12cf3 341 }
mawk2311 0:ad375c052b4c 342 if(integrationCounter % 151== 0){
mawk2311 10:e40ad924e935 343 /*
mawk2311 0:ad375c052b4c 344 //Disable interrupts
mawk2311 5:20223464f7aa 345 interrupt.fall(NULL);
mawk2311 5:20223464f7aa 346 interrupt.rise(NULL);
ericoneill 7:6d5ddcf12cf3 347 */
mawk2311 10:e40ad924e935 348
mawk2311 0:ad375c052b4c 349 //Send start of integration signal
mawk2311 0:ad375c052b4c 350 si = 1;
mawk2311 0:ad375c052b4c 351 clk = 1;
mawk2311 0:ad375c052b4c 352
mawk2311 0:ad375c052b4c 353 si = 0;
mawk2311 0:ad375c052b4c 354 clk = 0;
mawk2311 0:ad375c052b4c 355
mawk2311 0:ad375c052b4c 356 //Reset timing counter for integration
mawk2311 0:ad375c052b4c 357 integrationCounter = 0;
mawk2311 0:ad375c052b4c 358
mawk2311 0:ad375c052b4c 359 //Reset line tracking variables
mawk2311 5:20223464f7aa 360 maxAccum = 0;
mawk2311 5:20223464f7aa 361 maxCount = 0;
mawk2311 0:ad375c052b4c 362 approxPos = 0;
mawk2311 0:ad375c052b4c 363
mawk2311 5:20223464f7aa 364 space = false;
mawk2311 6:f1d948d2d6c1 365
mawk2311 0:ad375c052b4c 366 }
mawk2311 0:ad375c052b4c 367 else if (integrationCounter > 129){
mawk2311 5:20223464f7aa 368 //Start Timer
mawk2311 6:f1d948d2d6c1 369 //t.start();
mawk2311 0:ad375c052b4c 370
mawk2311 5:20223464f7aa 371 //Enable interrupts
mawk2311 6:f1d948d2d6c1 372 //interrupt.fall(&fallInterrupt);
mawk2311 6:f1d948d2d6c1 373 //interrupt.rise(&riseInterrupt);
mawk2311 5:20223464f7aa 374
mawk2311 12:48b76450c4b4 375 if (firstTime){
mawk2311 12:48b76450c4b4 376
mawk2311 12:48b76450c4b4 377 maxVal = ADCdata[10];
mawk2311 12:48b76450c4b4 378 for (int c = 11; c < 118; c++) {
mawk2311 12:48b76450c4b4 379 if (ADCdata[c] > maxVal){
mawk2311 12:48b76450c4b4 380 maxVal = ADCdata[c];
mawk2311 12:48b76450c4b4 381 maxLoc = c;
mawk2311 12:48b76450c4b4 382 }
mawk2311 0:ad375c052b4c 383 }
mawk2311 12:48b76450c4b4 384
mawk2311 12:48b76450c4b4 385 for (int c = 10; c < 118; c++) {
mawk2311 16:79106efd7a57 386 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.01f){
mawk2311 12:48b76450c4b4 387 maxAccum += c;
mawk2311 12:48b76450c4b4 388 maxCount++;
mawk2311 15:55e9fffc653a 389 if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
mawk2311 12:48b76450c4b4 390 space = true;
mawk2311 12:48b76450c4b4 391 }
mawk2311 12:48b76450c4b4 392 prevTrackLoc = c;
mawk2311 12:48b76450c4b4 393 }
mawk2311 12:48b76450c4b4 394 }
mawk2311 12:48b76450c4b4 395
mawk2311 16:79106efd7a57 396 //firstTime = false;
mawk2311 12:48b76450c4b4 397 } else {
mawk2311 12:48b76450c4b4 398
mawk2311 14:888495814f3c 399 startWindow = prevApproxPos - trackWindow;
mawk2311 14:888495814f3c 400 endWindow = prevApproxPos + trackWindow;
mawk2311 14:888495814f3c 401 if (startWindow < 0){
mawk2311 14:888495814f3c 402 startWindow = 0;
mawk2311 14:888495814f3c 403 }
mawk2311 14:888495814f3c 404 if (endWindow > 118){
mawk2311 14:888495814f3c 405 endWindow = 118;
mawk2311 14:888495814f3c 406 }
mawk2311 12:48b76450c4b4 407 maxVal = ADCdata[10];
mawk2311 14:888495814f3c 408 for (int c = startWindow; c < endWindow; c++) {
mawk2311 12:48b76450c4b4 409 if (ADCdata[c] > maxVal){
mawk2311 12:48b76450c4b4 410 maxVal = ADCdata[c];
mawk2311 12:48b76450c4b4 411 maxLoc = c;
mawk2311 12:48b76450c4b4 412 }
mawk2311 12:48b76450c4b4 413 }
mawk2311 12:48b76450c4b4 414
mawk2311 14:888495814f3c 415 for (int c = startWindow; c < endWindow; c++) {
mawk2311 16:79106efd7a57 416 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.01f){
mawk2311 11:b59ec039a712 417 maxAccum += c;
mawk2311 11:b59ec039a712 418 maxCount++;
mawk2311 15:55e9fffc653a 419 if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
mawk2311 11:b59ec039a712 420 space = true;
mawk2311 11:b59ec039a712 421 }
mawk2311 11:b59ec039a712 422 prevTrackLoc = c;
mawk2311 5:20223464f7aa 423 }
mawk2311 12:48b76450c4b4 424 }
mawk2311 11:b59ec039a712 425 }
mawk2311 12:48b76450c4b4 426 /*
mawk2311 11:b59ec039a712 427 //Check if we need to alter integration time due to brightness
mawk2311 11:b59ec039a712 428 if (maxVal < 0.15f){
mawk2311 11:b59ec039a712 429 intTimMod += 10;
mawk2311 11:b59ec039a712 430 } else if (maxVal >= 1) {
mawk2311 11:b59ec039a712 431 if (intTimMod > 0) {
mawk2311 11:b59ec039a712 432 intTimMod -= 10;
mawk2311 0:ad375c052b4c 433 }
mawk2311 0:ad375c052b4c 434 }
mawk2311 12:48b76450c4b4 435 */
mawk2311 11:b59ec039a712 436
mawk2311 6:f1d948d2d6c1 437 //Line Crossing Checks
mawk2311 10:e40ad924e935 438 if (space) {
mawk2311 5:20223464f7aa 439 currDir = prevDir;
mawk2311 15:55e9fffc653a 440 firstTime = true;
mawk2311 5:20223464f7aa 441 } else {
mawk2311 15:55e9fffc653a 442 approxPos = (float)maxAccum/(float)maxCount;
ericoneill 8:e126c900c89d 443
mawk2311 10:e40ad924e935 444 if(dataCol){
mawk2311 10:e40ad924e935 445 if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%3==0){
mawk2311 10:e40ad924e935 446 lineCenters[loopCtr] = approxPos;
mawk2311 10:e40ad924e935 447 times[loopCtr] = printTimer.read_ms();
mawk2311 10:e40ad924e935 448 loopCtr++;
mawk2311 10:e40ad924e935 449 }
ericoneill 9:ad08181ad1cc 450 }
mawk2311 10:e40ad924e935 451
mawk2311 16:79106efd7a57 452 currDir = hardLeft + (approxPos)/((float) 118)*(hardRight-hardLeft);
mawk2311 12:48b76450c4b4 453 prevApproxPos = approxPos;
ericoneill 9:ad08181ad1cc 454
mawk2311 5:20223464f7aa 455 }
mawk2311 16:79106efd7a57 456
mawk2311 16:79106efd7a57 457 if (turnSpeedControl){
mawk2311 16:79106efd7a57 458 //Change speed when turning at different angles
mawk2311 16:79106efd7a57 459 if(approxPos > 0 && approxPos <= 45){
mawk2311 16:79106efd7a57 460 motor1.pulsewidth(motorPeriod*(pulsewidth*0.8f));
mawk2311 16:79106efd7a57 461 motor2.pulsewidth(motorPeriod*(pulsewidth*0.8f));
mawk2311 16:79106efd7a57 462 } else if (approxPos > 45 && approxPos <= 95){
mawk2311 16:79106efd7a57 463 motor1.pulsewidth(motorPeriod*pulsewidth);
mawk2311 16:79106efd7a57 464 motor2.pulsewidth(motorPeriod*pulsewidth);
mawk2311 16:79106efd7a57 465 } else {
mawk2311 16:79106efd7a57 466 motor1.pulsewidth(motorPeriod*(pulsewidth*0.8f));
mawk2311 16:79106efd7a57 467 motor2.pulsewidth(motorPeriod*(pulsewidth*0.8f));
mawk2311 16:79106efd7a57 468 }
mawk2311 16:79106efd7a57 469 }
mawk2311 16:79106efd7a57 470
mawk2311 5:20223464f7aa 471 servo.pulsewidth(currDir);
mawk2311 5:20223464f7aa 472
mawk2311 6:f1d948d2d6c1 473 //Start Velocity control after requisite number of encoder signals have been collected
mawk2311 6:f1d948d2d6c1 474 //if(numInterrupts >= 4){
mawk2311 6:f1d948d2d6c1 475 //velocity_control(0.1f, TUNING_CONSTANT_10);
mawk2311 6:f1d948d2d6c1 476 //}
mawk2311 4:09c68df71785 477
mawk2311 6:f1d948d2d6c1 478 //Save current direction as previous direction
mawk2311 5:20223464f7aa 479 prevDir = currDir;
mawk2311 0:ad375c052b4c 480
mawk2311 6:f1d948d2d6c1 481 //Prepare to start collecting more data
mawk2311 0:ad375c052b4c 482 integrationCounter = 150;
mawk2311 6:f1d948d2d6c1 483
mawk2311 6:f1d948d2d6c1 484 //Disable interrupts
mawk2311 6:f1d948d2d6c1 485 //interrupt.fall(NULL);
mawk2311 6:f1d948d2d6c1 486 //interrupt.rise(NULL);
mawk2311 6:f1d948d2d6c1 487
mawk2311 6:f1d948d2d6c1 488 //Stop timer
mawk2311 6:f1d948d2d6c1 489 //t.stop();
mawk2311 0:ad375c052b4c 490 }
mawk2311 0:ad375c052b4c 491 else{
mawk2311 0:ad375c052b4c 492 clk = 1;
mawk2311 14:888495814f3c 493 wait_us(intTimMod);
mawk2311 3:e867c4e984df 494 ADCdata[integrationCounter - 1] = camData;
mawk2311 0:ad375c052b4c 495 clk = 0;
mawk2311 0:ad375c052b4c 496 }
mawk2311 0:ad375c052b4c 497
mawk2311 0:ad375c052b4c 498 //clk = 0;
mawk2311 0:ad375c052b4c 499 integrationCounter++;
mawk2311 10:e40ad924e935 500 if(dataCol){
mawk2311 10:e40ad924e935 501 loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime++;
mawk2311 10:e40ad924e935 502 }
mawk2311 0:ad375c052b4c 503 //camData.
mawk2311 0:ad375c052b4c 504
mawk2311 0:ad375c052b4c 505 }
mawk2311 10:e40ad924e935 506 if (dataCol){
mawk2311 10:e40ad924e935 507 //print frame data
mawk2311 10:e40ad924e935 508 pc.printf("printing frame data\n\r");
mawk2311 10:e40ad924e935 509 //int frameSize = frames.size();
mawk2311 10:e40ad924e935 510 //pc.printf("%i",frameSize);
mawk2311 10:e40ad924e935 511 pc.printf("[");
mawk2311 10:e40ad924e935 512 for(int i=0; i<numData; i++){
mawk2311 10:e40ad924e935 513 if(lineCenters > 0){
mawk2311 10:e40ad924e935 514 pc.printf("%i %i,",lineCenters[i], times[i]);
mawk2311 10:e40ad924e935 515 }
ericoneill 7:6d5ddcf12cf3 516 }
mawk2311 10:e40ad924e935 517 pc.printf("]\n\r");
ericoneill 7:6d5ddcf12cf3 518 }
mawk2311 0:ad375c052b4c 519 }
ericoneill 9:ad08181ad1cc 520