Michael Romain / Mbed 2 deprecated coolcarsuperfast

Just trying to update ccode.

Dependencies:   mbed

Fork of coolcarsuperfast by E=MC

main.cpp.orig@8:e126c900c89d, 2015-04-03 (annotated)

Committer:
ericoneill
Date:
Fri Apr 03 00:34:57 2015 +0000
Revision:
8:e126c900c89d
merged;

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 }