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@12:48b76450c4b4, 2015-04-09 (annotated)

Committer:
mawk2311
Date:
Thu Apr 09 05:07:19 2015 +0000
Revision:
12:48b76450c4b4
Parent:
11:b59ec039a712
Child:
13:5be515371946
Smoothed out turning a bit more, and I added a condition to our line tracker that remembers where the line last was and focuses in on that area when looking for the track next time. This last part has not been rigorously tested. Crossings are better.

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