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@14:888495814f3c, 2015-04-09 (annotated)

Committer:
mawk2311
Date:
Thu Apr 09 21:05:01 2015 +0000
Revision:
14:888495814f3c
Parent:
13:5be515371946
Child:
15:55e9fffc653a
Working on getting it running on the track outside.

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