Michael Romain / Mbed 2 deprecated coolcarsuperfast

Just trying to update ccode.

Dependencies:   mbed

Fork of coolcarsuperfast by E=MC

main.cpp@5:20223464f7aa, 2015-03-30 (annotated)

Committer:
mawk2311
Date:
Mon Mar 30 20:07:39 2015 +0000
Revision:
5:20223464f7aa
Parent:
4:09c68df71785
Child:
6:f1d948d2d6c1
Child:
7:6d5ddcf12cf3
Got the car working at ~1.365m/s

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 5:20223464f7aa 32 bool space;
mawk2311 0:ad375c052b4c 33
mawk2311 0:ad375c052b4c 34 //Servo turning parameters
mawk2311 0:ad375c052b4c 35 float straight = 0.00155f;
mawk2311 5:20223464f7aa 36 float hardLeft = 0.0010f;
mawk2311 0:ad375c052b4c 37 float slightLeft = 0.00145f;
mawk2311 5:20223464f7aa 38 float hardRight = 0.0021f;
mawk2311 0:ad375c052b4c 39 float slightRight = 0.00165f;
mawk2311 0:ad375c052b4c 40
mawk2311 5:20223464f7aa 41 //Servo Directions
mawk2311 5:20223464f7aa 42 float currDir;
mawk2311 5:20223464f7aa 43 float prevDir;
mawk2311 5:20223464f7aa 44
mawk2311 0:ad375c052b4c 45 //End of Line Tracking Variables -------------------------
mawk2311 0:ad375c052b4c 46
mawk2311 0:ad375c052b4c 47 //Encoder and Motor Driver Variables ---------------------
mawk2311 0:ad375c052b4c 48
mawk2311 0:ad375c052b4c 49 //Intervals used during encoder data collection to measure velocity
mawk2311 0:ad375c052b4c 50 int interval1=0;
mawk2311 0:ad375c052b4c 51 int interval2=0;
mawk2311 0:ad375c052b4c 52 int interval3=0;
mawk2311 0:ad375c052b4c 53 int avg_interval=0;
mawk2311 0:ad375c052b4c 54 int lastchange1 = 0;
mawk2311 0:ad375c052b4c 55 int lastchange2 = 0;
mawk2311 0:ad375c052b4c 56 int lastchange3 = 0;
mawk2311 0:ad375c052b4c 57 int lastchange4 = 0;
mawk2311 0:ad375c052b4c 58
mawk2311 0:ad375c052b4c 59 //Variables used to for velocity control
mawk2311 0:ad375c052b4c 60 float avg_speed = 0;
mawk2311 5:20223464f7aa 61 float last_speed = 0;
mawk2311 5:20223464f7aa 62
mawk2311 0:ad375c052b4c 63 float stall_check = 0;
mawk2311 0:ad375c052b4c 64 float tuning_val = 1;
mawk2311 0:ad375c052b4c 65
mawk2311 5:20223464f7aa 66
mawk2311 5:20223464f7aa 67 float pulsewidth = 0.18f;
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 1:55e0aaf71bda 153
mawk2311 0:ad375c052b4c 154 // Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 0:ad375c052b4c 155
mawk2311 0:ad375c052b4c 156 float get_speed(){
mawk2311 0:ad375c052b4c 157 float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
mawk2311 0:ad375c052b4c 158 float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
mawk2311 0:ad375c052b4c 159 return linearSpeed;
mawk2311 0:ad375c052b4c 160 }
mawk2311 0:ad375c052b4c 161
mawk2311 0:ad375c052b4c 162 float get_avg_speed(float num_samples, float delay) {
mawk2311 0:ad375c052b4c 163
mawk2311 0:ad375c052b4c 164 float avg_avg_speed = 0;
mawk2311 0:ad375c052b4c 165
mawk2311 0:ad375c052b4c 166 for (int c = 0; c < num_samples; c++) {
mawk2311 0:ad375c052b4c 167 if (num_samples == num_samples_small){
mawk2311 0:ad375c052b4c 168 small_avg_speed_list[c] = get_speed();
mawk2311 0:ad375c052b4c 169 } else if (num_samples == num_samples_large){
mawk2311 0:ad375c052b4c 170 large_avg_speed_list[c] = get_speed();
mawk2311 0:ad375c052b4c 171 }
mawk2311 0:ad375c052b4c 172 //wait(delay);
mawk2311 0:ad375c052b4c 173 }
mawk2311 0:ad375c052b4c 174
mawk2311 0:ad375c052b4c 175 for (int c = 1; c < num_samples; c++) {
mawk2311 0:ad375c052b4c 176 if (num_samples == num_samples_small){
mawk2311 0:ad375c052b4c 177 avg_avg_speed += small_avg_speed_list[c];
mawk2311 0:ad375c052b4c 178 } else if (num_samples == num_samples_large){
mawk2311 0:ad375c052b4c 179 avg_avg_speed += large_avg_speed_list[c];
mawk2311 0:ad375c052b4c 180 }
mawk2311 0:ad375c052b4c 181 }
mawk2311 0:ad375c052b4c 182 return avg_avg_speed/num_samples;
mawk2311 0:ad375c052b4c 183 }
mawk2311 0:ad375c052b4c 184
mawk2311 0:ad375c052b4c 185 void velocity_control(float duty_cyc, float tuning_const) {
mawk2311 0:ad375c052b4c 186
mawk2311 0:ad375c052b4c 187 avg_speed = get_speed();//get_avg_speed(num_samples_small, delay_small);
mawk2311 0:ad375c052b4c 188
mawk2311 5:20223464f7aa 189 //When determined speed is infinity or 0, set the speed to the last agreeable speed
mawk2311 5:20223464f7aa 190 if (avg_speed > 100 || avg_speed == 0){
mawk2311 5:20223464f7aa 191 avg_speed = last_speed;
mawk2311 5:20223464f7aa 192 }
mawk2311 5:20223464f7aa 193
mawk2311 5:20223464f7aa 194 pc.printf("\n\r%f", avg_speed);
mawk2311 5:20223464f7aa 195 if (avg_speed == stall_check && tuning_const != 0 && avg_speed == 0) {
mawk2311 0:ad375c052b4c 196 avg_speed = 0;
mawk2311 0:ad375c052b4c 197 tuning_val += TUNE_AMT;
mawk2311 0:ad375c052b4c 198 } else if((avg_speed - tuning_const) > TUNE_THRESH){
mawk2311 0:ad375c052b4c 199 tuning_val -= TUNE_AMT;
mawk2311 0:ad375c052b4c 200 stall_check = avg_speed;
mawk2311 5:20223464f7aa 201
mawk2311 5:20223464f7aa 202 } else if (tuning_const - avg_speed > TUNE_THRESH && avg_speed != 0){
mawk2311 5:20223464f7aa 203 tuning_val += TUNE_AMT;
mawk2311 0:ad375c052b4c 204 stall_check = avg_speed;
mawk2311 0:ad375c052b4c 205 } else {
mawk2311 0:ad375c052b4c 206 //tuning_val = 1;
mawk2311 0:ad375c052b4c 207 stall_check = avg_speed;
mawk2311 0:ad375c052b4c 208 }
mawk2311 0:ad375c052b4c 209
mawk2311 5:20223464f7aa 210 if (tuning_val < .5){
mawk2311 5:20223464f7aa 211 tuning_val = .5;
mawk2311 5:20223464f7aa 212 }
mawk2311 5:20223464f7aa 213 pc.printf("\n\rTuning Val: %f", tuning_val);
mawk2311 5:20223464f7aa 214 motor1.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
mawk2311 5:20223464f7aa 215 motor2.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
mawk2311 5:20223464f7aa 216
mawk2311 5:20223464f7aa 217 if (avg_speed != 0){
mawk2311 5:20223464f7aa 218 last_speed = avg_speed;
mawk2311 5:20223464f7aa 219 }
mawk2311 5:20223464f7aa 220
mawk2311 0:ad375c052b4c 221 }
mawk2311 0:ad375c052b4c 222
mawk2311 0:ad375c052b4c 223 void fallInterrupt(){
mawk2311 0:ad375c052b4c 224
mawk2311 0:ad375c052b4c 225 int time = t.read_us();
mawk2311 0:ad375c052b4c 226 interval1 = time - lastchange2;
mawk2311 0:ad375c052b4c 227 interval2 = lastchange1-lastchange3;
mawk2311 0:ad375c052b4c 228 interval3 = lastchange2 - lastchange4;
mawk2311 0:ad375c052b4c 229 avg_interval = (interval1 + interval2 + interval3)/3;
mawk2311 0:ad375c052b4c 230
mawk2311 0:ad375c052b4c 231 lastchange4 = lastchange3;
mawk2311 0:ad375c052b4c 232 lastchange3 = lastchange2;
mawk2311 0:ad375c052b4c 233 lastchange2 = lastchange1;
mawk2311 0:ad375c052b4c 234 lastchange1 = time;
mawk2311 0:ad375c052b4c 235 }
mawk2311 0:ad375c052b4c 236
mawk2311 0:ad375c052b4c 237 void riseInterrupt(){
mawk2311 0:ad375c052b4c 238 int time = t.read_us();
mawk2311 0:ad375c052b4c 239 interval1 = time - lastchange2;
mawk2311 0:ad375c052b4c 240 interval2 = lastchange1-lastchange3;
mawk2311 0:ad375c052b4c 241 interval3 = lastchange2 - lastchange4;
mawk2311 0:ad375c052b4c 242 avg_interval = (interval1 + interval2 + interval3)/3;
mawk2311 0:ad375c052b4c 243
mawk2311 0:ad375c052b4c 244 lastchange4 = lastchange3;
mawk2311 0:ad375c052b4c 245 lastchange3 = lastchange2;
mawk2311 0:ad375c052b4c 246 lastchange2 = lastchange1;
mawk2311 0:ad375c052b4c 247 lastchange1 = time;
mawk2311 0:ad375c052b4c 248 }
mawk2311 0:ad375c052b4c 249
mawk2311 0:ad375c052b4c 250
mawk2311 0:ad375c052b4c 251 //End of Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 0:ad375c052b4c 252
mawk2311 0:ad375c052b4c 253 int main() {
mawk2311 0:ad375c052b4c 254 //Line Tracker Initializations
mawk2311 0:ad375c052b4c 255 int integrationCounter = 0;
mawk2311 0:ad375c052b4c 256
mawk2311 5:20223464f7aa 257 //Initial values for directions (meaningless currently)
mawk2311 5:20223464f7aa 258 currDir = 0;
mawk2311 5:20223464f7aa 259 prevDir = 0;
mawk2311 5:20223464f7aa 260
mawk2311 0:ad375c052b4c 261 // Motor Driver Initializations
mawk2311 5:20223464f7aa 262 motor1.period(motorPeriod);
mawk2311 5:20223464f7aa 263 motor2.period(motorPeriod);
mawk2311 0:ad375c052b4c 264
mawk2311 5:20223464f7aa 265 // Servo Initialization
mawk2311 5:20223464f7aa 266 servo.period(servoPeriod);
mawk2311 5:20223464f7aa 267
mawk2311 5:20223464f7aa 268 wait(3);
mawk2311 0:ad375c052b4c 269
mawk2311 5:20223464f7aa 270 motor1.pulsewidth(motorPeriod*pulsewidth);
mawk2311 5:20223464f7aa 271 motor2.pulsewidth(motorPeriod*pulsewidth);
mawk2311 0:ad375c052b4c 272 break1 = 0;
mawk2311 0:ad375c052b4c 273 break2 = 0;
mawk2311 0:ad375c052b4c 274
mawk2311 5:20223464f7aa 275 t.start();
mawk2311 0:ad375c052b4c 276
mawk2311 1:55e0aaf71bda 277 //wait(5);
mawk2311 0:ad375c052b4c 278
mawk2311 0:ad375c052b4c 279 while(1) {
mawk2311 0:ad375c052b4c 280
mawk2311 0:ad375c052b4c 281 if(integrationCounter % 151== 0){
mawk2311 0:ad375c052b4c 282 //Disable interrupts
mawk2311 5:20223464f7aa 283 interrupt.fall(NULL);
mawk2311 5:20223464f7aa 284 interrupt.rise(NULL);
mawk2311 0:ad375c052b4c 285
mawk2311 0:ad375c052b4c 286 //Send start of integration signal
mawk2311 0:ad375c052b4c 287 si = 1;
mawk2311 0:ad375c052b4c 288 clk = 1;
mawk2311 0:ad375c052b4c 289
mawk2311 0:ad375c052b4c 290 si = 0;
mawk2311 0:ad375c052b4c 291 clk = 0;
mawk2311 0:ad375c052b4c 292
mawk2311 0:ad375c052b4c 293 //Reset timing counter for integration
mawk2311 0:ad375c052b4c 294 integrationCounter = 0;
mawk2311 0:ad375c052b4c 295
mawk2311 0:ad375c052b4c 296 //Reset line tracking variables
mawk2311 5:20223464f7aa 297 maxAccum = 0;
mawk2311 5:20223464f7aa 298 maxCount = 0;
mawk2311 0:ad375c052b4c 299 approxPos = 0;
mawk2311 0:ad375c052b4c 300
mawk2311 5:20223464f7aa 301 space = false;
mawk2311 5:20223464f7aa 302
mawk2311 5:20223464f7aa 303 /* Velocity control junk
mawk2311 0:ad375c052b4c 304 //Reset Encoder and Motor Driver Variables
mawk2311 0:ad375c052b4c 305 interval1=0;
mawk2311 0:ad375c052b4c 306 interval2=0;
mawk2311 0:ad375c052b4c 307 interval3=0;
mawk2311 5:20223464f7aa 308 //avg_interval=0;
mawk2311 0:ad375c052b4c 309 lastchange1 = 0;
mawk2311 0:ad375c052b4c 310 lastchange2 = 0;
mawk2311 0:ad375c052b4c 311 lastchange3 = 0;
mawk2311 0:ad375c052b4c 312 lastchange4 = 0;
mawk2311 5:20223464f7aa 313 //stall_check = 0;
mawk2311 5:20223464f7aa 314 */
mawk2311 0:ad375c052b4c 315 }
mawk2311 0:ad375c052b4c 316 else if (integrationCounter > 129){
mawk2311 5:20223464f7aa 317 //Start Timer
mawk2311 5:20223464f7aa 318 t.start();
mawk2311 0:ad375c052b4c 319
mawk2311 5:20223464f7aa 320 //Enable interrupts
mawk2311 5:20223464f7aa 321 interrupt.fall(&fallInterrupt);
mawk2311 5:20223464f7aa 322 interrupt.rise(&riseInterrupt);
mawk2311 5:20223464f7aa 323
mawk2311 5:20223464f7aa 324 maxVal = ADCdata[10];
mawk2311 5:20223464f7aa 325 for (int c = 11; c < 118; c++) {
mawk2311 5:20223464f7aa 326 if (ADCdata[c] > maxVal){
mawk2311 5:20223464f7aa 327 maxVal = ADCdata[c];
mawk2311 5:20223464f7aa 328 maxLoc = c;
mawk2311 0:ad375c052b4c 329 }
mawk2311 0:ad375c052b4c 330 }
mawk2311 0:ad375c052b4c 331
mawk2311 1:55e0aaf71bda 332 for (int c = 10; c < 118; c++) {
mawk2311 5:20223464f7aa 333 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.07f && ADCdata[c] > 0.1f){
mawk2311 5:20223464f7aa 334 maxAccum += c;
mawk2311 5:20223464f7aa 335 maxCount++;
mawk2311 5:20223464f7aa 336 if (c != prevTrackLoc + 1){
mawk2311 5:20223464f7aa 337 space = true;
mawk2311 5:20223464f7aa 338 }
mawk2311 5:20223464f7aa 339 prevTrackLoc = c;
mawk2311 0:ad375c052b4c 340 }
mawk2311 0:ad375c052b4c 341 }
mawk2311 0:ad375c052b4c 342
mawk2311 5:20223464f7aa 343 if (maxCount >= 25) {
mawk2311 5:20223464f7aa 344 currDir = prevDir;
mawk2311 5:20223464f7aa 345 } else {
mawk2311 5:20223464f7aa 346 approxPos = (float)maxAccum/(float)maxCount;
mawk2311 5:20223464f7aa 347 currDir = hardLeft + approxPos/((float) 127)*(hardRight-hardLeft);
mawk2311 5:20223464f7aa 348 }
mawk2311 1:55e0aaf71bda 349
mawk2311 1:55e0aaf71bda 350 //approxPos = find_track(ADCdata);
mawk2311 1:55e0aaf71bda 351
mawk2311 4:09c68df71785 352 /*if(approxPos > 0 && approxPos <= 45){
mawk2311 5:20223464f7aa 353 motor1.pulsewidth(motorPeriod*(pulsewidth-.015));
mawk2311 5:20223464f7aa 354 motor2.pulsewidth(motorPeriod*(pulsewidth-.015));
mawk2311 1:55e0aaf71bda 355 } else if (approxPos > 45 && approxPos <= 95){
mawk2311 5:20223464f7aa 356 motor1.pulsewidth(motorPeriod*pulsewidth);
mawk2311 5:20223464f7aa 357 motor2.pulsewidth(motorPeriod*pulsewidth);
mawk2311 1:55e0aaf71bda 358 } else {
mawk2311 5:20223464f7aa 359 motor1.pulsewidth(motorPeriod*(pulsewidth-.015));
mawk2311 5:20223464f7aa 360 motor2.pulsewidth(motorPeriod*(pulsewidth-.015));
mawk2311 1:55e0aaf71bda 361 }*/
mawk2311 4:09c68df71785 362
mawk2311 5:20223464f7aa 363 servo.pulsewidth(currDir);
mawk2311 5:20223464f7aa 364
mawk2311 5:20223464f7aa 365 //delay for velocity control
mawk2311 5:20223464f7aa 366 //wait_ms(10);
mawk2311 5:20223464f7aa 367
mawk2311 4:09c68df71785 368
mawk2311 5:20223464f7aa 369 //Stop Timer
mawk2311 5:20223464f7aa 370 //t.stop();
mawk2311 5:20223464f7aa 371
mawk2311 5:20223464f7aa 372 //Reset Timer
mawk2311 5:20223464f7aa 373 //t.reset();
mawk2311 5:20223464f7aa 374
mawk2311 5:20223464f7aa 375 //velocity_control(0.1f, TUNING_CONSTANT_10);
mawk2311 5:20223464f7aa 376
mawk2311 5:20223464f7aa 377 prevDir = currDir;
mawk2311 0:ad375c052b4c 378
mawk2311 0:ad375c052b4c 379 integrationCounter = 150;
mawk2311 0:ad375c052b4c 380 }
mawk2311 0:ad375c052b4c 381 else{
mawk2311 0:ad375c052b4c 382 clk = 1;
mawk2311 5:20223464f7aa 383 wait_us(10);
mawk2311 3:e867c4e984df 384 ADCdata[integrationCounter - 1] = camData;
mawk2311 0:ad375c052b4c 385 clk = 0;
mawk2311 0:ad375c052b4c 386 }
mawk2311 0:ad375c052b4c 387
mawk2311 0:ad375c052b4c 388 //clk = 0;
mawk2311 0:ad375c052b4c 389 integrationCounter++;
mawk2311 0:ad375c052b4c 390 //camData.
mawk2311 0:ad375c052b4c 391
mawk2311 0:ad375c052b4c 392 }
mawk2311 0:ad375c052b4c 393 }