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@17:28a1f9309fdb, 2015-04-22 (annotated)

Committer:
mawk2311
Date:
Wed Apr 22 22:55:24 2015 +0000
Revision:
17:28a1f9309fdb
Parent:
16:79106efd7a57
Child:
18:7941524e0d28
Tweaked the integration time as well as the acceptable threshold for max values (line 416). Those are the only values that I think we'll need to play with before the checkoff (and speed if we're feeling it.) I'd say we just tune Sunday night.

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 17:28a1f9309fdb 43 int widthThresh = 8;
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 17:28a1f9309fdb 89 int intTimMod = 0;
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 17:28a1f9309fdb 416 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.25f){
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 17:28a1f9309fdb 460 motor1.pulsewidth(motorPeriod*(pulsewidth*1.2f));
mawk2311 17:28a1f9309fdb 461 motor2.pulsewidth(motorPeriod*(pulsewidth*1.2f));
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 17:28a1f9309fdb 466 motor1.pulsewidth(motorPeriod*(pulsewidth*1.2f));
mawk2311 17:28a1f9309fdb 467 motor2.pulsewidth(motorPeriod*(pulsewidth*1.2f));
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