Michael Romain / Mbed 2 deprecated coolcarsuperfast

Just trying to update ccode.

Dependencies:   mbed

Fork of coolcarsuperfast by E=MC

main.cpp@6:f1d948d2d6c1, 2015-03-31 (annotated)

Committer:
mawk2311
Date:
Tue Mar 31 22:56:01 2015 +0000
Revision:
6:f1d948d2d6c1
Parent:
5:20223464f7aa
Child:
8:e126c900c89d
This should work at motor pulsewidths of .25; may need to adjust the camera angle.

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