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@11:b59ec039a712, 2015-04-09 (annotated)

Committer:
mawk2311
Date:
Thu Apr 09 01:13:49 2015 +0000
Revision:
11:b59ec039a712
Parent:
10:e40ad924e935
Child:
12:48b76450c4b4
Just trying to get it uploaded.

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