Michael Romain / Mbed 2 deprecated Shitty_figure8

Framework for doing the figure 8 track.

Dependencies:   mbed

main.cpp@1:55e0aaf71bda, 2015-03-20 (annotated)

Committer:
mawk2311
Date:
Fri Mar 20 10:18:20 2015 +0000
Revision:
1:55e0aaf71bda
Parent:
0:ad375c052b4c
Child:
2:a04e2757d372
pushing

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