E=MC / Mbed 2 deprecated Shitty_figure8

^

Dependencies:   mbed

Fork of Shitty_figure8 by Michael Romain

main.cpp@2:b1226838afb9, 2015-03-20 (annotated)

Committer:
cheryl_he
Date:
Fri Mar 20 10:41:03 2015 +0000
Revision:
2:b1226838afb9
Parent:
1:55e0aaf71bda 
added int track_found to break out of loop if we lose the track

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 0:ad375c052b4c 8 PwmOut motor1(PTA4);
mawk2311 0:ad375c052b4c 9 PwmOut motor2(PTA12);
mawk2311 0:ad375c052b4c 10 DigitalOut break1(PTC12);
mawk2311 0:ad375c052b4c 11 DigitalOut break2(PTC13);
mawk2311 0:ad375c052b4c 12 PwmOut servo(PTA5);
mawk2311 0:ad375c052b4c 13
mawk2311 0:ad375c052b4c 14 //Inputs
mawk2311 0:ad375c052b4c 15 AnalogIn camData(PTC2);
mawk2311 0:ad375c052b4c 16
mawk2311 0:ad375c052b4c 17 //Encoder setup and variables
mawk2311 0:ad375c052b4c 18 InterruptIn interrupt(PTA13);
mawk2311 0:ad375c052b4c 19
mawk2311 0:ad375c052b4c 20 //Line Tracking Variables --------------------------------
mawk2311 0:ad375c052b4c 21 float ADCdata [128];
mawk2311 0:ad375c052b4c 22 float minAccum;
mawk2311 0:ad375c052b4c 23 float minCount;
mawk2311 0:ad375c052b4c 24 float approxPos;
mawk2311 0:ad375c052b4c 25 float minVal;
mawk2311 0:ad375c052b4c 26 int minLoc;
cheryl_he 2:b1226838afb9 27 int track_found = 1; // 1 if found
mawk2311 0:ad375c052b4c 28
mawk2311 0:ad375c052b4c 29 //Servo turning parameters
mawk2311 0:ad375c052b4c 30 float straight = 0.00155f;
mawk2311 0:ad375c052b4c 31 float hardLeft = 0.0013f;
mawk2311 0:ad375c052b4c 32 float slightLeft = 0.00145f;
mawk2311 0:ad375c052b4c 33 float hardRight = 0.0018f;
mawk2311 0:ad375c052b4c 34 float slightRight = 0.00165f;
mawk2311 0:ad375c052b4c 35
mawk2311 0:ad375c052b4c 36 //End of Line Tracking Variables -------------------------
mawk2311 0:ad375c052b4c 37
mawk2311 0:ad375c052b4c 38 //Encoder and Motor Driver Variables ---------------------
mawk2311 0:ad375c052b4c 39
mawk2311 0:ad375c052b4c 40 //Intervals used during encoder data collection to measure velocity
mawk2311 0:ad375c052b4c 41 int interval1=0;
mawk2311 0:ad375c052b4c 42 int interval2=0;
mawk2311 0:ad375c052b4c 43 int interval3=0;
mawk2311 0:ad375c052b4c 44 int avg_interval=0;
mawk2311 0:ad375c052b4c 45 int lastchange1 = 0;
mawk2311 0:ad375c052b4c 46 int lastchange2 = 0;
mawk2311 0:ad375c052b4c 47 int lastchange3 = 0;
mawk2311 0:ad375c052b4c 48 int lastchange4 = 0;
mawk2311 0:ad375c052b4c 49
mawk2311 0:ad375c052b4c 50 //Variables used to for velocity control
mawk2311 0:ad375c052b4c 51 float avg_speed = 0;
mawk2311 0:ad375c052b4c 52 float stall_check = 0;
mawk2311 0:ad375c052b4c 53 float tuning_val = 1;
mawk2311 0:ad375c052b4c 54
mawk2311 0:ad375c052b4c 55 Timer t;
mawk2311 0:ad375c052b4c 56 Timer servoTimer;
mawk2311 0:ad375c052b4c 57
mawk2311 0:ad375c052b4c 58 //Observed average speeds for each duty cycle
mawk2311 0:ad375c052b4c 59 const float DESIRED_SPEED = 0.5;
mawk2311 0:ad375c052b4c 60 const float TUNING_CONSTANT_10 = 1.10;
mawk2311 0:ad375c052b4c 61 const float TUNING_CONSTANT_20 = 3.00;
mawk2311 0:ad375c052b4c 62 const float TUNING_CONSTANT_30 = 4.30;
mawk2311 0:ad375c052b4c 63 const float TUNING_CONSTANT_50 = 6.880;
mawk2311 0:ad375c052b4c 64 const float PI = 3.14159;
mawk2311 0:ad375c052b4c 65 const float WHEEL_CIRCUMFERENCE = .05*PI;
mawk2311 0:ad375c052b4c 66
mawk2311 0:ad375c052b4c 67 //Velocity Control Tuning Constants
mawk2311 0:ad375c052b4c 68 const float TUNE_THRESH = 0.5f;
mawk2311 0:ad375c052b4c 69 const float TUNE_AMT = 0.1f;
mawk2311 0:ad375c052b4c 70
mawk2311 0:ad375c052b4c 71 //Parameters specifying sample sizes and delays for small and large average speed samples
mawk2311 0:ad375c052b4c 72 float num_samples_small = 3.0f;
mawk2311 0:ad375c052b4c 73 float delay_small = 0.05f;
mawk2311 0:ad375c052b4c 74 float num_samples_large = 100.0f;
mawk2311 0:ad375c052b4c 75 float delay_large = 0.1f;
mawk2311 0:ad375c052b4c 76
mawk2311 0:ad375c052b4c 77 //Large and small arrays used to get average velocity values
mawk2311 0:ad375c052b4c 78 float large_avg_speed_list [100];
mawk2311 0:ad375c052b4c 79 float small_avg_speed_list [10];
mawk2311 0:ad375c052b4c 80
mawk2311 0:ad375c052b4c 81 //End of Encoder and Motor Driver Variables ----------------------
mawk2311 0:ad375c052b4c 82
mawk2311 1:55e0aaf71bda 83 //Line Tracker Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 1:55e0aaf71bda 84
mawk2311 1:55e0aaf71bda 85 int find_track(float line[]){
mawk2311 1:55e0aaf71bda 86 int track_location = -1;
mawk2311 1:55e0aaf71bda 87 float slope_threshold = .05;
mawk2311 1:55e0aaf71bda 88 bool downslope_indices [128] = {false};
mawk2311 1:55e0aaf71bda 89 bool upslope_indices [128] = {false};
mawk2311 1:55e0aaf71bda 90 for(int i=10; i<118; i++){
mawk2311 1:55e0aaf71bda 91 if(line[i+1] - line[i] < -slope_threshold && line[i+2] - line[i+1] < -slope_threshold){
mawk2311 1:55e0aaf71bda 92 downslope_indices[i] = true;
mawk2311 1:55e0aaf71bda 93 }
mawk2311 1:55e0aaf71bda 94 if(line[i+1] - line[i] > slope_threshold && line[i+2] - line[i+1] > slope_threshold){
mawk2311 1:55e0aaf71bda 95 upslope_indices[i] = true;
mawk2311 1:55e0aaf71bda 96 }
mawk2311 1:55e0aaf71bda 97 }
mawk2311 1:55e0aaf71bda 98 int numDownslopes = 0;
mawk2311 1:55e0aaf71bda 99 int numUpslopes = 0;
mawk2311 1:55e0aaf71bda 100 for(int i=0; i<128; i++){
mawk2311 1:55e0aaf71bda 101 if(downslope_indices[i] == true){
mawk2311 1:55e0aaf71bda 102 numDownslopes ++;
mawk2311 1:55e0aaf71bda 103 }
mawk2311 1:55e0aaf71bda 104 if(upslope_indices[i] == true){
mawk2311 1:55e0aaf71bda 105 numUpslopes ++;
mawk2311 1:55e0aaf71bda 106 }
mawk2311 1:55e0aaf71bda 107 }
mawk2311 1:55e0aaf71bda 108 int downslope_locs [numDownslopes];
mawk2311 1:55e0aaf71bda 109 int upslope_locs [numUpslopes];
mawk2311 1:55e0aaf71bda 110 int dsctr = 0;
mawk2311 1:55e0aaf71bda 111 int usctr = 0;
mawk2311 1:55e0aaf71bda 112
mawk2311 1:55e0aaf71bda 113 for (int i=0; i<128; i++){
mawk2311 1:55e0aaf71bda 114 if(downslope_indices[i] == true){
mawk2311 1:55e0aaf71bda 115 downslope_locs[dsctr] = i;
mawk2311 1:55e0aaf71bda 116 dsctr++;
mawk2311 1:55e0aaf71bda 117 }
mawk2311 1:55e0aaf71bda 118 if(upslope_indices[i] == true){
mawk2311 1:55e0aaf71bda 119 upslope_locs[usctr] = i;
mawk2311 1:55e0aaf71bda 120 usctr++;
mawk2311 1:55e0aaf71bda 121 }
mawk2311 1:55e0aaf71bda 122 }
mawk2311 1:55e0aaf71bda 123
mawk2311 1:55e0aaf71bda 124 for(int i=0; i<numDownslopes; i++){
mawk2311 1:55e0aaf71bda 125 for(int j=0; j<numUpslopes; j++){
mawk2311 1:55e0aaf71bda 126 if(upslope_locs[j] - downslope_locs[i] >=4 && upslope_locs[j] - downslope_locs[i] <=5){
cheryl_he 2:b1226838afb9 127 track_location = downslope_locs[i] + 2;
mawk2311 1:55e0aaf71bda 128 }
mawk2311 1:55e0aaf71bda 129 }
mawk2311 1:55e0aaf71bda 130 }
cheryl_he 2:b1226838afb9 131 if (numUpslopes == 0 && numDownslopes == 0){
cheryl_he 2:b1226838afb9 132 track_found = 0;
cheryl_he 2:b1226838afb9 133 }
mawk2311 1:55e0aaf71bda 134
mawk2311 1:55e0aaf71bda 135 return track_location;
mawk2311 1:55e0aaf71bda 136 }
mawk2311 1:55e0aaf71bda 137
mawk2311 1:55e0aaf71bda 138
mawk2311 0:ad375c052b4c 139 // Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 0:ad375c052b4c 140
mawk2311 0:ad375c052b4c 141 float get_speed(){
mawk2311 0:ad375c052b4c 142 float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
mawk2311 0:ad375c052b4c 143 float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
mawk2311 0:ad375c052b4c 144 return linearSpeed;
mawk2311 0:ad375c052b4c 145 }
mawk2311 0:ad375c052b4c 146
mawk2311 0:ad375c052b4c 147 float get_avg_speed(float num_samples, float delay) {
mawk2311 0:ad375c052b4c 148
mawk2311 0:ad375c052b4c 149 float avg_avg_speed = 0;
mawk2311 0:ad375c052b4c 150
mawk2311 0:ad375c052b4c 151 for (int c = 0; c < num_samples; c++) {
mawk2311 0:ad375c052b4c 152 if (num_samples == num_samples_small){
mawk2311 0:ad375c052b4c 153 small_avg_speed_list[c] = get_speed();
mawk2311 0:ad375c052b4c 154 } else if (num_samples == num_samples_large){
mawk2311 0:ad375c052b4c 155 large_avg_speed_list[c] = get_speed();
mawk2311 0:ad375c052b4c 156 }
mawk2311 0:ad375c052b4c 157 //wait(delay);
mawk2311 0:ad375c052b4c 158 }
mawk2311 0:ad375c052b4c 159
mawk2311 0:ad375c052b4c 160 for (int c = 1; c < num_samples; c++) {
mawk2311 0:ad375c052b4c 161 if (num_samples == num_samples_small){
mawk2311 0:ad375c052b4c 162 avg_avg_speed += small_avg_speed_list[c];
mawk2311 0:ad375c052b4c 163 } else if (num_samples == num_samples_large){
mawk2311 0:ad375c052b4c 164 avg_avg_speed += large_avg_speed_list[c];
mawk2311 0:ad375c052b4c 165 }
mawk2311 0:ad375c052b4c 166 }
mawk2311 0:ad375c052b4c 167 return avg_avg_speed/num_samples;
mawk2311 0:ad375c052b4c 168 }
mawk2311 0:ad375c052b4c 169
mawk2311 0:ad375c052b4c 170 void velocity_control(float duty_cyc, float tuning_const) {
mawk2311 0:ad375c052b4c 171
mawk2311 0:ad375c052b4c 172 avg_speed = get_speed();//get_avg_speed(num_samples_small, delay_small);
mawk2311 0:ad375c052b4c 173
mawk2311 0:ad375c052b4c 174 if (avg_speed == stall_check) {
mawk2311 0:ad375c052b4c 175 avg_speed = 0;
mawk2311 0:ad375c052b4c 176 tuning_val += TUNE_AMT;
mawk2311 0:ad375c052b4c 177 } else if((avg_speed - tuning_const) > TUNE_THRESH){
mawk2311 0:ad375c052b4c 178 tuning_val -= TUNE_AMT;
mawk2311 0:ad375c052b4c 179 stall_check = avg_speed;
mawk2311 0:ad375c052b4c 180 } else if (tuning_const - avg_speed > TUNE_THRESH){
mawk2311 0:ad375c052b4c 181 //tuning_val += TUNE_AMT;
mawk2311 0:ad375c052b4c 182 stall_check = avg_speed;
mawk2311 0:ad375c052b4c 183 } else {
mawk2311 0:ad375c052b4c 184 //tuning_val = 1;
mawk2311 0:ad375c052b4c 185 stall_check = avg_speed;
mawk2311 0:ad375c052b4c 186 }
mawk2311 0:ad375c052b4c 187 motor1.pulsewidth(.0025 * duty_cyc * tuning_val);
mawk2311 0:ad375c052b4c 188 motor2.pulsewidth(.0025 * duty_cyc * tuning_val);
mawk2311 0:ad375c052b4c 189
mawk2311 0:ad375c052b4c 190 }
mawk2311 0:ad375c052b4c 191
mawk2311 0:ad375c052b4c 192 void fallInterrupt(){
mawk2311 0:ad375c052b4c 193
mawk2311 0:ad375c052b4c 194 int time = t.read_us();
mawk2311 0:ad375c052b4c 195 interval1 = time - lastchange2;
mawk2311 0:ad375c052b4c 196 interval2 = lastchange1-lastchange3;
mawk2311 0:ad375c052b4c 197 interval3 = lastchange2 - lastchange4;
mawk2311 0:ad375c052b4c 198 avg_interval = (interval1 + interval2 + interval3)/3;
mawk2311 0:ad375c052b4c 199
mawk2311 0:ad375c052b4c 200 lastchange4 = lastchange3;
mawk2311 0:ad375c052b4c 201 lastchange3 = lastchange2;
mawk2311 0:ad375c052b4c 202 lastchange2 = lastchange1;
mawk2311 0:ad375c052b4c 203 lastchange1 = time;
mawk2311 0:ad375c052b4c 204 }
mawk2311 0:ad375c052b4c 205
mawk2311 0:ad375c052b4c 206 void riseInterrupt(){
mawk2311 0:ad375c052b4c 207 int time = t.read_us();
mawk2311 0:ad375c052b4c 208 interval1 = time - lastchange2;
mawk2311 0:ad375c052b4c 209 interval2 = lastchange1-lastchange3;
mawk2311 0:ad375c052b4c 210 interval3 = lastchange2 - lastchange4;
mawk2311 0:ad375c052b4c 211 avg_interval = (interval1 + interval2 + interval3)/3;
mawk2311 0:ad375c052b4c 212
mawk2311 0:ad375c052b4c 213 lastchange4 = lastchange3;
mawk2311 0:ad375c052b4c 214 lastchange3 = lastchange2;
mawk2311 0:ad375c052b4c 215 lastchange2 = lastchange1;
mawk2311 0:ad375c052b4c 216 lastchange1 = time;
mawk2311 0:ad375c052b4c 217 }
mawk2311 0:ad375c052b4c 218
mawk2311 0:ad375c052b4c 219
mawk2311 0:ad375c052b4c 220 //End of Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 0:ad375c052b4c 221
mawk2311 0:ad375c052b4c 222 int main() {
mawk2311 0:ad375c052b4c 223 //Line Tracker Initializations
mawk2311 0:ad375c052b4c 224 int integrationCounter = 0;
mawk2311 0:ad375c052b4c 225
mawk2311 0:ad375c052b4c 226 // Motor Driver Initializations
mawk2311 0:ad375c052b4c 227 motor1.period(.0025);
mawk2311 0:ad375c052b4c 228 motor2.period(.0025);
mawk2311 1:55e0aaf71bda 229 //interrupt.fall(&fallInterrupt);
mawk2311 1:55e0aaf71bda 230 //interrupt.rise(&riseInterrupt);
mawk2311 0:ad375c052b4c 231
mawk2311 1:55e0aaf71bda 232 wait(5);
mawk2311 0:ad375c052b4c 233
mawk2311 1:55e0aaf71bda 234 motor1.pulsewidth(.0025*.105);
mawk2311 1:55e0aaf71bda 235 motor2.pulsewidth(.0025*.105);
mawk2311 0:ad375c052b4c 236 break1 = 0;
mawk2311 0:ad375c052b4c 237 break2 = 0;
mawk2311 0:ad375c052b4c 238
mawk2311 1:55e0aaf71bda 239 //t.start();
mawk2311 0:ad375c052b4c 240
mawk2311 1:55e0aaf71bda 241 //wait(5);
mawk2311 0:ad375c052b4c 242
cheryl_he 2:b1226838afb9 243 while(1 && track_found) {
mawk2311 0:ad375c052b4c 244 if(integrationCounter % 151== 0){
mawk2311 0:ad375c052b4c 245 //Disable interrupts
mawk2311 0:ad375c052b4c 246 //__disable_irq();
mawk2311 0:ad375c052b4c 247 //interrupt.fall(NULL);
mawk2311 0:ad375c052b4c 248 //interrupt.rise(NULL);
mawk2311 0:ad375c052b4c 249
mawk2311 0:ad375c052b4c 250 //Send start of integration signal
mawk2311 0:ad375c052b4c 251 si = 1;
mawk2311 0:ad375c052b4c 252 clk = 1;
mawk2311 0:ad375c052b4c 253
mawk2311 0:ad375c052b4c 254 si = 0;
mawk2311 0:ad375c052b4c 255 clk = 0;
mawk2311 0:ad375c052b4c 256
mawk2311 0:ad375c052b4c 257 //Reset timing counter for integration
mawk2311 0:ad375c052b4c 258 integrationCounter = 0;
mawk2311 0:ad375c052b4c 259
mawk2311 0:ad375c052b4c 260 //Reset line tracking variables
mawk2311 0:ad375c052b4c 261 minAccum = 0;
mawk2311 0:ad375c052b4c 262 minCount = 0;
mawk2311 0:ad375c052b4c 263 approxPos = 0;
mawk2311 0:ad375c052b4c 264
mawk2311 0:ad375c052b4c 265 //Reset Encoder and Motor Driver Variables
mawk2311 0:ad375c052b4c 266 interval1=0;
mawk2311 0:ad375c052b4c 267 interval2=0;
mawk2311 0:ad375c052b4c 268 interval3=0;
mawk2311 0:ad375c052b4c 269 avg_interval=0;
mawk2311 0:ad375c052b4c 270 lastchange1 = 0;
mawk2311 0:ad375c052b4c 271 lastchange2 = 0;
mawk2311 0:ad375c052b4c 272 lastchange3 = 0;
mawk2311 0:ad375c052b4c 273 lastchange4 = 0;
mawk2311 0:ad375c052b4c 274
mawk2311 0:ad375c052b4c 275 t.reset();
mawk2311 0:ad375c052b4c 276
mawk2311 0:ad375c052b4c 277 }
mawk2311 0:ad375c052b4c 278 else if (integrationCounter > 129){
mawk2311 0:ad375c052b4c 279 //Enable interrupts
mawk2311 0:ad375c052b4c 280 //__enable_irq();
mawk2311 0:ad375c052b4c 281 //interrupt.fall(&fallInterrupt);
mawk2311 0:ad375c052b4c 282 //interrupt.rise(&riseInterrupt);
mawk2311 0:ad375c052b4c 283
mawk2311 0:ad375c052b4c 284 minVal = ADCdata[15];
mawk2311 1:55e0aaf71bda 285 for (int c = 10; c < 118; c++) {
mawk2311 0:ad375c052b4c 286 if (ADCdata[c] < minVal){
mawk2311 0:ad375c052b4c 287 minVal = ADCdata[c];
mawk2311 0:ad375c052b4c 288 minLoc = c;
mawk2311 0:ad375c052b4c 289 }
mawk2311 0:ad375c052b4c 290 }
mawk2311 0:ad375c052b4c 291
mawk2311 1:55e0aaf71bda 292 for (int c = 10; c < 118; c++) {
mawk2311 1:55e0aaf71bda 293 if (ADCdata[c] >= minVal && ADCdata[c] - minVal < 0.08f && ADCdata[c] > 0.1f){
mawk2311 0:ad375c052b4c 294 minAccum += c;
mawk2311 0:ad375c052b4c 295 minCount++;
mawk2311 0:ad375c052b4c 296 }
mawk2311 0:ad375c052b4c 297 }
mawk2311 0:ad375c052b4c 298
mawk2311 0:ad375c052b4c 299 approxPos = (float)minAccum/(float)minCount;
mawk2311 0:ad375c052b4c 300
mawk2311 1:55e0aaf71bda 301
mawk2311 1:55e0aaf71bda 302 //approxPos = find_track(ADCdata);
mawk2311 1:55e0aaf71bda 303
mawk2311 1:55e0aaf71bda 304 if(approxPos > 0 && approxPos <= 25){
mawk2311 0:ad375c052b4c 305 servo.pulsewidth(hardLeft);
mawk2311 1:55e0aaf71bda 306 motor1.pulsewidth(.0025*.095);
mawk2311 1:55e0aaf71bda 307 motor2.pulsewidth(.0025*.095);
mawk2311 1:55e0aaf71bda 308 } else if (approxPos > 25 && approxPos <= 40){
mawk2311 0:ad375c052b4c 309 servo.pulsewidth(slightLeft);
mawk2311 1:55e0aaf71bda 310 motor1.pulsewidth(.0025*.1);
mawk2311 1:55e0aaf71bda 311 motor2.pulsewidth(.0025*.1);
mawk2311 1:55e0aaf71bda 312 } else if (approxPos > 40 && approxPos <= 90){
mawk2311 0:ad375c052b4c 313 servo.pulsewidth(straight);
mawk2311 1:55e0aaf71bda 314 motor1.pulsewidth(.0025*.105);
mawk2311 1:55e0aaf71bda 315 motor2.pulsewidth(.0025*.105);
mawk2311 0:ad375c052b4c 316 } else if (approxPos > 90 && approxPos <= 105){
mawk2311 1:55e0aaf71bda 317 servo.pulsewidth(slightRight);
mawk2311 1:55e0aaf71bda 318 motor1.pulsewidth(.0025*.1);
mawk2311 1:55e0aaf71bda 319 motor2.pulsewidth(.0025*.1);
mawk2311 0:ad375c052b4c 320 } else if (approxPos > 105 && approxPos <= 128){
mawk2311 0:ad375c052b4c 321 servo.pulsewidth(hardRight);
mawk2311 1:55e0aaf71bda 322 motor1.pulsewidth(.0025*.095);
mawk2311 1:55e0aaf71bda 323 motor2.pulsewidth(.0025*.095);
mawk2311 0:ad375c052b4c 324 }
mawk2311 1:55e0aaf71bda 325 /*
mawk2311 1:55e0aaf71bda 326 if(approxPos > 0 && approxPos <= 45){
mawk2311 1:55e0aaf71bda 327 servo.pulsewidth(hardLeft);
mawk2311 1:55e0aaf71bda 328 } else if (approxPos > 45 && approxPos <= 95){
mawk2311 1:55e0aaf71bda 329 servo.pulsewidth(straight);
mawk2311 1:55e0aaf71bda 330 } else {
mawk2311 1:55e0aaf71bda 331 servo.pulsewidth(hardRight);
mawk2311 1:55e0aaf71bda 332 }*/
mawk2311 1:55e0aaf71bda 333 //velocity_control(0.05f, DESIRED_SPEED);
mawk2311 0:ad375c052b4c 334
mawk2311 0:ad375c052b4c 335 integrationCounter = 150;
mawk2311 0:ad375c052b4c 336 }
mawk2311 0:ad375c052b4c 337 else{
mawk2311 0:ad375c052b4c 338 clk = 1;
mawk2311 1:55e0aaf71bda 339 wait_us(220);
mawk2311 1:55e0aaf71bda 340 ADCdata[integrationCounter - 1] = 1 - camData;
mawk2311 0:ad375c052b4c 341 clk = 0;
mawk2311 0:ad375c052b4c 342 }
cheryl_he 2:b1226838afb9 343
mawk2311 0:ad375c052b4c 344 //clk = 0;
mawk2311 0:ad375c052b4c 345 integrationCounter++;
mawk2311 0:ad375c052b4c 346 //camData.
mawk2311 0:ad375c052b4c 347
mawk2311 0:ad375c052b4c 348 }
mawk2311 0:ad375c052b4c 349 }