Michael Romain / Mbed 2 deprecated coolcarsuperfast

Just trying to update ccode.

Dependencies:   mbed

Fork of coolcarsuperfast by E=MC

main.cpp.orig@10:e40ad924e935, 2015-04-06 (annotated)

Committer:
mawk2311
Date:
Mon Apr 06 19:28:45 2015 +0000
Revision:
10:e40ad924e935
Parent:
8:e126c900c89d 
Added a boolean that we can flip as well as if conditions so that we can more easily activate data collection. Also tweaked the hard right value for the servo. With this changed, it doesn't jump to extremes nearly as much when turning right.

Who changed what in which revision?

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