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@20:d96f46dea035, 2015-04-28 (annotated)

Committer:
cheryl_he
Date:
Tue Apr 28 18:18:19 2015 +0000
Revision:
20:d96f46dea035
Parent:
16:79106efd7a57
Child:
21:00e7a29dfe09
pinout for bluetooth and linescan2

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