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@13:5be515371946, 2015-04-09 (annotated)

Committer:
mawk2311
Date:
Thu Apr 09 05:48:35 2015 +0000
Revision:
13:5be515371946
Parent:
12:48b76450c4b4
Child:
14:888495814f3c
Got it to do the figure 8 at 2.1m/s! It  wasn't super pretty and it ended up crashing after 5 laps or so, but hey, that's pretty neat! and much faster than we'll ever actually need to take it.

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