Michael Romain / Mbed 2 deprecated coolcarsuperfast

Just trying to update ccode.

Dependencies:   mbed

Fork of coolcarsuperfast by E=MC

main.cpp@7:6d5ddcf12cf3, 2015-04-03 (annotated)

Committer:
ericoneill
Date:
Fri Apr 03 00:21:13 2015 +0000
Revision:
7:6d5ddcf12cf3
Parent:
5:20223464f7aa
Child:
8:e126c900c89d
printing data;

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"
ericoneill 7:6d5ddcf12cf3 3 #include <vector>
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
ericoneill 7:6d5ddcf12cf3 45 // All linescan data for the period the car is running on. To be printed after a set amount of time
ericoneill 7:6d5ddcf12cf3 46 //std::vector<std::vector<int> > frames;
ericoneill 7:6d5ddcf12cf3 47 const int numData = 1000;
ericoneill 7:6d5ddcf12cf3 48 int lineCenters [numData] = {0};
ericoneill 7:6d5ddcf12cf3 49 int times [numData] = {0};
ericoneill 7:6d5ddcf12cf3 50 int loopCtr = 0;
ericoneill 7:6d5ddcf12cf3 51
mawk2311 0:ad375c052b4c 52 //End of Line Tracking Variables -------------------------
mawk2311 0:ad375c052b4c 53
mawk2311 0:ad375c052b4c 54 //Encoder and Motor Driver Variables ---------------------
mawk2311 0:ad375c052b4c 55
mawk2311 0:ad375c052b4c 56 //Intervals used during encoder data collection to measure velocity
mawk2311 0:ad375c052b4c 57 int interval1=0;
mawk2311 0:ad375c052b4c 58 int interval2=0;
mawk2311 0:ad375c052b4c 59 int interval3=0;
mawk2311 0:ad375c052b4c 60 int avg_interval=0;
mawk2311 0:ad375c052b4c 61 int lastchange1 = 0;
mawk2311 0:ad375c052b4c 62 int lastchange2 = 0;
mawk2311 0:ad375c052b4c 63 int lastchange3 = 0;
mawk2311 0:ad375c052b4c 64 int lastchange4 = 0;
mawk2311 0:ad375c052b4c 65
mawk2311 0:ad375c052b4c 66 //Variables used to for velocity control
mawk2311 0:ad375c052b4c 67 float avg_speed = 0;
mawk2311 5:20223464f7aa 68 float last_speed = 0;
mawk2311 5:20223464f7aa 69
mawk2311 0:ad375c052b4c 70 float stall_check = 0;
mawk2311 0:ad375c052b4c 71 float tuning_val = 1;
mawk2311 0:ad375c052b4c 72
mawk2311 5:20223464f7aa 73
mawk2311 5:20223464f7aa 74 float pulsewidth = 0.18f;
mawk2311 5:20223464f7aa 75
mawk2311 5:20223464f7aa 76 // Hardware periods
mawk2311 5:20223464f7aa 77 float motorPeriod = .0025;
mawk2311 5:20223464f7aa 78 float servoPeriod = .0025;
mawk2311 5:20223464f7aa 79
mawk2311 0:ad375c052b4c 80 Timer t;
mawk2311 0:ad375c052b4c 81 Timer servoTimer;
ericoneill 7:6d5ddcf12cf3 82 Timer printTimer; //after printTimer reaches a certain value the main loop will terminate and print the frames
ericoneill 7:6d5ddcf12cf3 83 const float printTime = 5.0f;
mawk2311 0:ad375c052b4c 84
mawk2311 0:ad375c052b4c 85 //Observed average speeds for each duty cycle
mawk2311 5:20223464f7aa 86 const float DESIRED_SPEED = 1;
mawk2311 5:20223464f7aa 87 const float TUNING_CONSTANT_10 = 1.90;
mawk2311 0:ad375c052b4c 88 const float TUNING_CONSTANT_20 = 3.00;
mawk2311 0:ad375c052b4c 89 const float TUNING_CONSTANT_30 = 4.30;
mawk2311 0:ad375c052b4c 90 const float TUNING_CONSTANT_50 = 6.880;
mawk2311 0:ad375c052b4c 91 const float PI = 3.14159;
mawk2311 0:ad375c052b4c 92 const float WHEEL_CIRCUMFERENCE = .05*PI;
mawk2311 0:ad375c052b4c 93
mawk2311 0:ad375c052b4c 94 //Velocity Control Tuning Constants
mawk2311 0:ad375c052b4c 95 const float TUNE_THRESH = 0.5f;
mawk2311 0:ad375c052b4c 96 const float TUNE_AMT = 0.1f;
mawk2311 0:ad375c052b4c 97
mawk2311 0:ad375c052b4c 98 //Parameters specifying sample sizes and delays for small and large average speed samples
mawk2311 0:ad375c052b4c 99 float num_samples_small = 3.0f;
mawk2311 0:ad375c052b4c 100 float delay_small = 0.05f;
mawk2311 0:ad375c052b4c 101 float num_samples_large = 100.0f;
mawk2311 0:ad375c052b4c 102 float delay_large = 0.1f;
mawk2311 0:ad375c052b4c 103
mawk2311 0:ad375c052b4c 104 //Large and small arrays used to get average velocity values
mawk2311 0:ad375c052b4c 105 float large_avg_speed_list [100];
mawk2311 0:ad375c052b4c 106 float small_avg_speed_list [10];
mawk2311 0:ad375c052b4c 107
mawk2311 0:ad375c052b4c 108 //End of Encoder and Motor Driver Variables ----------------------
mawk2311 0:ad375c052b4c 109
mawk2311 1:55e0aaf71bda 110 //Line Tracker Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 1:55e0aaf71bda 111
mawk2311 1:55e0aaf71bda 112 int find_track(float line[]){
mawk2311 1:55e0aaf71bda 113 int track_location = -1;
mawk2311 1:55e0aaf71bda 114 float slope_threshold = .05;
mawk2311 1:55e0aaf71bda 115 bool downslope_indices [128] = {false};
mawk2311 1:55e0aaf71bda 116 bool upslope_indices [128] = {false};
mawk2311 1:55e0aaf71bda 117 for(int i=10; i<118; i++){
mawk2311 1:55e0aaf71bda 118 if(line[i+1] - line[i] < -slope_threshold && line[i+2] - line[i+1] < -slope_threshold){
mawk2311 1:55e0aaf71bda 119 downslope_indices[i] = true;
mawk2311 1:55e0aaf71bda 120 }
mawk2311 1:55e0aaf71bda 121 if(line[i+1] - line[i] > slope_threshold && line[i+2] - line[i+1] > slope_threshold){
mawk2311 1:55e0aaf71bda 122 upslope_indices[i] = true;
mawk2311 1:55e0aaf71bda 123 }
mawk2311 1:55e0aaf71bda 124 }
mawk2311 1:55e0aaf71bda 125 int numDownslopes = 0;
mawk2311 1:55e0aaf71bda 126 int numUpslopes = 0;
mawk2311 1:55e0aaf71bda 127 for(int i=0; i<128; i++){
mawk2311 1:55e0aaf71bda 128 if(downslope_indices[i] == true){
mawk2311 1:55e0aaf71bda 129 numDownslopes ++;
mawk2311 1:55e0aaf71bda 130 }
mawk2311 1:55e0aaf71bda 131 if(upslope_indices[i] == true){
mawk2311 1:55e0aaf71bda 132 numUpslopes ++;
mawk2311 1:55e0aaf71bda 133 }
mawk2311 1:55e0aaf71bda 134 }
mawk2311 1:55e0aaf71bda 135 int downslope_locs [numDownslopes];
mawk2311 1:55e0aaf71bda 136 int upslope_locs [numUpslopes];
mawk2311 1:55e0aaf71bda 137 int dsctr = 0;
mawk2311 1:55e0aaf71bda 138 int usctr = 0;
mawk2311 1:55e0aaf71bda 139
mawk2311 1:55e0aaf71bda 140 for (int i=0; i<128; i++){
mawk2311 1:55e0aaf71bda 141 if(downslope_indices[i] == true){
mawk2311 1:55e0aaf71bda 142 downslope_locs[dsctr] = i;
mawk2311 1:55e0aaf71bda 143 dsctr++;
mawk2311 1:55e0aaf71bda 144 }
mawk2311 1:55e0aaf71bda 145 if(upslope_indices[i] == true){
mawk2311 1:55e0aaf71bda 146 upslope_locs[usctr] = i;
mawk2311 1:55e0aaf71bda 147 usctr++;
mawk2311 1:55e0aaf71bda 148 }
mawk2311 1:55e0aaf71bda 149 }
mawk2311 1:55e0aaf71bda 150
mawk2311 1:55e0aaf71bda 151 for(int i=0; i<numDownslopes; i++){
mawk2311 1:55e0aaf71bda 152 for(int j=0; j<numUpslopes; j++){
mawk2311 1:55e0aaf71bda 153 if(upslope_locs[j] - downslope_locs[i] >=4 && upslope_locs[j] - downslope_locs[i] <=5){
mawk2311 1:55e0aaf71bda 154 track_location = downslope_locs[i] + 2 ;
mawk2311 1:55e0aaf71bda 155 }
mawk2311 1:55e0aaf71bda 156 }
mawk2311 1:55e0aaf71bda 157 }
mawk2311 1:55e0aaf71bda 158
mawk2311 1:55e0aaf71bda 159 return track_location;
mawk2311 1:55e0aaf71bda 160 }
mawk2311 1:55e0aaf71bda 161
mawk2311 1:55e0aaf71bda 162
mawk2311 0:ad375c052b4c 163 // Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 0:ad375c052b4c 164
mawk2311 0:ad375c052b4c 165 float get_speed(){
mawk2311 0:ad375c052b4c 166 float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
mawk2311 0:ad375c052b4c 167 float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
mawk2311 0:ad375c052b4c 168 return linearSpeed;
mawk2311 0:ad375c052b4c 169 }
mawk2311 0:ad375c052b4c 170
mawk2311 0:ad375c052b4c 171 float get_avg_speed(float num_samples, float delay) {
mawk2311 0:ad375c052b4c 172
mawk2311 0:ad375c052b4c 173 float avg_avg_speed = 0;
mawk2311 0:ad375c052b4c 174
mawk2311 0:ad375c052b4c 175 for (int c = 0; c < num_samples; c++) {
mawk2311 0:ad375c052b4c 176 if (num_samples == num_samples_small){
mawk2311 0:ad375c052b4c 177 small_avg_speed_list[c] = get_speed();
mawk2311 0:ad375c052b4c 178 } else if (num_samples == num_samples_large){
mawk2311 0:ad375c052b4c 179 large_avg_speed_list[c] = get_speed();
mawk2311 0:ad375c052b4c 180 }
mawk2311 0:ad375c052b4c 181 //wait(delay);
mawk2311 0:ad375c052b4c 182 }
mawk2311 0:ad375c052b4c 183
mawk2311 0:ad375c052b4c 184 for (int c = 1; c < num_samples; c++) {
mawk2311 0:ad375c052b4c 185 if (num_samples == num_samples_small){
mawk2311 0:ad375c052b4c 186 avg_avg_speed += small_avg_speed_list[c];
mawk2311 0:ad375c052b4c 187 } else if (num_samples == num_samples_large){
mawk2311 0:ad375c052b4c 188 avg_avg_speed += large_avg_speed_list[c];
mawk2311 0:ad375c052b4c 189 }
mawk2311 0:ad375c052b4c 190 }
mawk2311 0:ad375c052b4c 191 return avg_avg_speed/num_samples;
mawk2311 0:ad375c052b4c 192 }
mawk2311 0:ad375c052b4c 193
mawk2311 0:ad375c052b4c 194 void velocity_control(float duty_cyc, float tuning_const) {
mawk2311 0:ad375c052b4c 195
mawk2311 0:ad375c052b4c 196 avg_speed = get_speed();//get_avg_speed(num_samples_small, delay_small);
mawk2311 0:ad375c052b4c 197
mawk2311 5:20223464f7aa 198 //When determined speed is infinity or 0, set the speed to the last agreeable speed
mawk2311 5:20223464f7aa 199 if (avg_speed > 100 || avg_speed == 0){
mawk2311 5:20223464f7aa 200 avg_speed = last_speed;
mawk2311 5:20223464f7aa 201 }
mawk2311 5:20223464f7aa 202
mawk2311 5:20223464f7aa 203 pc.printf("\n\r%f", avg_speed);
mawk2311 5:20223464f7aa 204 if (avg_speed == stall_check && tuning_const != 0 && avg_speed == 0) {
mawk2311 0:ad375c052b4c 205 avg_speed = 0;
mawk2311 0:ad375c052b4c 206 tuning_val += TUNE_AMT;
mawk2311 0:ad375c052b4c 207 } else if((avg_speed - tuning_const) > TUNE_THRESH){
mawk2311 0:ad375c052b4c 208 tuning_val -= TUNE_AMT;
mawk2311 0:ad375c052b4c 209 stall_check = avg_speed;
mawk2311 5:20223464f7aa 210
mawk2311 5:20223464f7aa 211 } else if (tuning_const - avg_speed > TUNE_THRESH && avg_speed != 0){
mawk2311 5:20223464f7aa 212 tuning_val += TUNE_AMT;
mawk2311 0:ad375c052b4c 213 stall_check = avg_speed;
mawk2311 0:ad375c052b4c 214 } else {
mawk2311 0:ad375c052b4c 215 //tuning_val = 1;
mawk2311 0:ad375c052b4c 216 stall_check = avg_speed;
mawk2311 0:ad375c052b4c 217 }
mawk2311 0:ad375c052b4c 218
mawk2311 5:20223464f7aa 219 if (tuning_val < .5){
mawk2311 5:20223464f7aa 220 tuning_val = .5;
mawk2311 5:20223464f7aa 221 }
mawk2311 5:20223464f7aa 222 pc.printf("\n\rTuning Val: %f", tuning_val);
mawk2311 5:20223464f7aa 223 motor1.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
mawk2311 5:20223464f7aa 224 motor2.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
mawk2311 5:20223464f7aa 225
mawk2311 5:20223464f7aa 226 if (avg_speed != 0){
mawk2311 5:20223464f7aa 227 last_speed = avg_speed;
mawk2311 5:20223464f7aa 228 }
mawk2311 5:20223464f7aa 229
mawk2311 0:ad375c052b4c 230 }
mawk2311 0:ad375c052b4c 231
mawk2311 0:ad375c052b4c 232 void fallInterrupt(){
mawk2311 0:ad375c052b4c 233
mawk2311 0:ad375c052b4c 234 int time = t.read_us();
mawk2311 0:ad375c052b4c 235 interval1 = time - lastchange2;
mawk2311 0:ad375c052b4c 236 interval2 = lastchange1-lastchange3;
mawk2311 0:ad375c052b4c 237 interval3 = lastchange2 - lastchange4;
mawk2311 0:ad375c052b4c 238 avg_interval = (interval1 + interval2 + interval3)/3;
mawk2311 0:ad375c052b4c 239
mawk2311 0:ad375c052b4c 240 lastchange4 = lastchange3;
mawk2311 0:ad375c052b4c 241 lastchange3 = lastchange2;
mawk2311 0:ad375c052b4c 242 lastchange2 = lastchange1;
mawk2311 0:ad375c052b4c 243 lastchange1 = time;
mawk2311 0:ad375c052b4c 244 }
mawk2311 0:ad375c052b4c 245
mawk2311 0:ad375c052b4c 246 void riseInterrupt(){
mawk2311 0:ad375c052b4c 247 int time = t.read_us();
mawk2311 0:ad375c052b4c 248 interval1 = time - lastchange2;
mawk2311 0:ad375c052b4c 249 interval2 = lastchange1-lastchange3;
mawk2311 0:ad375c052b4c 250 interval3 = lastchange2 - lastchange4;
mawk2311 0:ad375c052b4c 251 avg_interval = (interval1 + interval2 + interval3)/3;
mawk2311 0:ad375c052b4c 252
mawk2311 0:ad375c052b4c 253 lastchange4 = lastchange3;
mawk2311 0:ad375c052b4c 254 lastchange3 = lastchange2;
mawk2311 0:ad375c052b4c 255 lastchange2 = lastchange1;
mawk2311 0:ad375c052b4c 256 lastchange1 = time;
mawk2311 0:ad375c052b4c 257 }
mawk2311 0:ad375c052b4c 258
mawk2311 0:ad375c052b4c 259
mawk2311 0:ad375c052b4c 260 //End of Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 0:ad375c052b4c 261
mawk2311 0:ad375c052b4c 262 int main() {
mawk2311 0:ad375c052b4c 263 //Line Tracker Initializations
mawk2311 0:ad375c052b4c 264 int integrationCounter = 0;
mawk2311 0:ad375c052b4c 265
mawk2311 5:20223464f7aa 266 //Initial values for directions (meaningless currently)
mawk2311 5:20223464f7aa 267 currDir = 0;
mawk2311 5:20223464f7aa 268 prevDir = 0;
mawk2311 5:20223464f7aa 269
mawk2311 0:ad375c052b4c 270 // Motor Driver Initializations
mawk2311 5:20223464f7aa 271 motor1.period(motorPeriod);
mawk2311 5:20223464f7aa 272 motor2.period(motorPeriod);
mawk2311 0:ad375c052b4c 273
mawk2311 5:20223464f7aa 274 // Servo Initialization
mawk2311 5:20223464f7aa 275 servo.period(servoPeriod);
mawk2311 5:20223464f7aa 276
mawk2311 5:20223464f7aa 277 wait(3);
mawk2311 0:ad375c052b4c 278
mawk2311 5:20223464f7aa 279 motor1.pulsewidth(motorPeriod*pulsewidth);
mawk2311 5:20223464f7aa 280 motor2.pulsewidth(motorPeriod*pulsewidth);
mawk2311 0:ad375c052b4c 281 break1 = 0;
mawk2311 0:ad375c052b4c 282 break2 = 0;
mawk2311 0:ad375c052b4c 283
mawk2311 5:20223464f7aa 284 t.start();
ericoneill 7:6d5ddcf12cf3 285 printTimer.start();
mawk2311 1:55e0aaf71bda 286 //wait(5);
mawk2311 0:ad375c052b4c 287
mawk2311 0:ad375c052b4c 288 while(1) {
mawk2311 0:ad375c052b4c 289
ericoneill 7:6d5ddcf12cf3 290 //break out of main loop if enough time has passed;
ericoneill 7:6d5ddcf12cf3 291 if(loopCtr >= numData){
ericoneill 7:6d5ddcf12cf3 292 break;
ericoneill 7:6d5ddcf12cf3 293 }
mawk2311 0:ad375c052b4c 294 if(integrationCounter % 151== 0){
mawk2311 0:ad375c052b4c 295 //Disable interrupts
mawk2311 5:20223464f7aa 296 interrupt.fall(NULL);
mawk2311 5:20223464f7aa 297 interrupt.rise(NULL);
ericoneill 7:6d5ddcf12cf3 298
ericoneill 7:6d5ddcf12cf3 299 /*
ericoneill 7:6d5ddcf12cf3 300 // copy frame into the vector to be printed
ericoneill 7:6d5ddcf12cf3 301 std::vector<int> temp;
ericoneill 7:6d5ddcf12cf3 302 temp.reserve(128); //performance
ericoneill 7:6d5ddcf12cf3 303 for(int i=0; i<128; i++){
ericoneill 7:6d5ddcf12cf3 304 temp.push_back(ADCdata[i]);
ericoneill 7:6d5ddcf12cf3 305 }
ericoneill 7:6d5ddcf12cf3 306 frames.push_back(temp);
ericoneill 7:6d5ddcf12cf3 307 */
mawk2311 0:ad375c052b4c 308 //Send start of integration signal
mawk2311 0:ad375c052b4c 309 si = 1;
mawk2311 0:ad375c052b4c 310 clk = 1;
mawk2311 0:ad375c052b4c 311
mawk2311 0:ad375c052b4c 312 si = 0;
mawk2311 0:ad375c052b4c 313 clk = 0;
mawk2311 0:ad375c052b4c 314
mawk2311 0:ad375c052b4c 315 //Reset timing counter for integration
mawk2311 0:ad375c052b4c 316 integrationCounter = 0;
mawk2311 0:ad375c052b4c 317
mawk2311 0:ad375c052b4c 318 //Reset line tracking variables
mawk2311 5:20223464f7aa 319 maxAccum = 0;
mawk2311 5:20223464f7aa 320 maxCount = 0;
mawk2311 0:ad375c052b4c 321 approxPos = 0;
mawk2311 0:ad375c052b4c 322
mawk2311 5:20223464f7aa 323 space = false;
mawk2311 5:20223464f7aa 324
mawk2311 5:20223464f7aa 325 /* Velocity control junk
mawk2311 0:ad375c052b4c 326 //Reset Encoder and Motor Driver Variables
mawk2311 0:ad375c052b4c 327 interval1=0;
mawk2311 0:ad375c052b4c 328 interval2=0;
mawk2311 0:ad375c052b4c 329 interval3=0;
mawk2311 5:20223464f7aa 330 //avg_interval=0;
mawk2311 0:ad375c052b4c 331 lastchange1 = 0;
mawk2311 0:ad375c052b4c 332 lastchange2 = 0;
mawk2311 0:ad375c052b4c 333 lastchange3 = 0;
mawk2311 0:ad375c052b4c 334 lastchange4 = 0;
mawk2311 5:20223464f7aa 335 //stall_check = 0;
mawk2311 5:20223464f7aa 336 */
mawk2311 0:ad375c052b4c 337 }
mawk2311 0:ad375c052b4c 338 else if (integrationCounter > 129){
mawk2311 5:20223464f7aa 339 //Start Timer
mawk2311 5:20223464f7aa 340 t.start();
mawk2311 0:ad375c052b4c 341
mawk2311 5:20223464f7aa 342 //Enable interrupts
mawk2311 5:20223464f7aa 343 interrupt.fall(&fallInterrupt);
mawk2311 5:20223464f7aa 344 interrupt.rise(&riseInterrupt);
mawk2311 5:20223464f7aa 345
mawk2311 5:20223464f7aa 346 maxVal = ADCdata[10];
mawk2311 5:20223464f7aa 347 for (int c = 11; c < 118; c++) {
mawk2311 5:20223464f7aa 348 if (ADCdata[c] > maxVal){
mawk2311 5:20223464f7aa 349 maxVal = ADCdata[c];
mawk2311 5:20223464f7aa 350 maxLoc = c;
mawk2311 0:ad375c052b4c 351 }
mawk2311 0:ad375c052b4c 352 }
mawk2311 0:ad375c052b4c 353
mawk2311 1:55e0aaf71bda 354 for (int c = 10; c < 118; c++) {
mawk2311 5:20223464f7aa 355 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.07f && ADCdata[c] > 0.1f){
mawk2311 5:20223464f7aa 356 maxAccum += c;
mawk2311 5:20223464f7aa 357 maxCount++;
mawk2311 5:20223464f7aa 358 if (c != prevTrackLoc + 1){
mawk2311 5:20223464f7aa 359 space = true;
mawk2311 5:20223464f7aa 360 }
mawk2311 5:20223464f7aa 361 prevTrackLoc = c;
mawk2311 0:ad375c052b4c 362 }
mawk2311 0:ad375c052b4c 363 }
mawk2311 0:ad375c052b4c 364
mawk2311 5:20223464f7aa 365 if (maxCount >= 25) {
mawk2311 5:20223464f7aa 366 currDir = prevDir;
mawk2311 5:20223464f7aa 367 } else {
mawk2311 5:20223464f7aa 368 approxPos = (float)maxAccum/(float)maxCount;
ericoneill 7:6d5ddcf12cf3 369 lineCenters[loopCtr] = approxPos;
ericoneill 7:6d5ddcf12cf3 370 times[loopCtr] = printTimer.read_ms();
mawk2311 5:20223464f7aa 371 currDir = hardLeft + approxPos/((float) 127)*(hardRight-hardLeft);
mawk2311 5:20223464f7aa 372 }
mawk2311 1:55e0aaf71bda 373
mawk2311 1:55e0aaf71bda 374 //approxPos = find_track(ADCdata);
mawk2311 1:55e0aaf71bda 375
mawk2311 4:09c68df71785 376 /*if(approxPos > 0 && approxPos <= 45){
mawk2311 5:20223464f7aa 377 motor1.pulsewidth(motorPeriod*(pulsewidth-.015));
mawk2311 5:20223464f7aa 378 motor2.pulsewidth(motorPeriod*(pulsewidth-.015));
mawk2311 1:55e0aaf71bda 379 } else if (approxPos > 45 && approxPos <= 95){
mawk2311 5:20223464f7aa 380 motor1.pulsewidth(motorPeriod*pulsewidth);
mawk2311 5:20223464f7aa 381 motor2.pulsewidth(motorPeriod*pulsewidth);
mawk2311 1:55e0aaf71bda 382 } else {
mawk2311 5:20223464f7aa 383 motor1.pulsewidth(motorPeriod*(pulsewidth-.015));
mawk2311 5:20223464f7aa 384 motor2.pulsewidth(motorPeriod*(pulsewidth-.015));
mawk2311 1:55e0aaf71bda 385 }*/
mawk2311 4:09c68df71785 386
mawk2311 5:20223464f7aa 387 servo.pulsewidth(currDir);
mawk2311 5:20223464f7aa 388
mawk2311 5:20223464f7aa 389 //delay for velocity control
mawk2311 5:20223464f7aa 390 //wait_ms(10);
mawk2311 5:20223464f7aa 391
mawk2311 4:09c68df71785 392
mawk2311 5:20223464f7aa 393 //Stop Timer
mawk2311 5:20223464f7aa 394 //t.stop();
mawk2311 5:20223464f7aa 395
mawk2311 5:20223464f7aa 396 //Reset Timer
mawk2311 5:20223464f7aa 397 //t.reset();
mawk2311 5:20223464f7aa 398
mawk2311 5:20223464f7aa 399 //velocity_control(0.1f, TUNING_CONSTANT_10);
mawk2311 5:20223464f7aa 400
mawk2311 5:20223464f7aa 401 prevDir = currDir;
mawk2311 0:ad375c052b4c 402
mawk2311 0:ad375c052b4c 403 integrationCounter = 150;
mawk2311 0:ad375c052b4c 404 }
mawk2311 0:ad375c052b4c 405 else{
mawk2311 0:ad375c052b4c 406 clk = 1;
mawk2311 5:20223464f7aa 407 wait_us(10);
mawk2311 3:e867c4e984df 408 ADCdata[integrationCounter - 1] = camData;
mawk2311 0:ad375c052b4c 409 clk = 0;
mawk2311 0:ad375c052b4c 410 }
mawk2311 0:ad375c052b4c 411
mawk2311 0:ad375c052b4c 412 //clk = 0;
mawk2311 0:ad375c052b4c 413 integrationCounter++;
ericoneill 7:6d5ddcf12cf3 414 loopCtr++;
mawk2311 0:ad375c052b4c 415 //camData.
mawk2311 0:ad375c052b4c 416
mawk2311 0:ad375c052b4c 417 }
ericoneill 7:6d5ddcf12cf3 418
ericoneill 7:6d5ddcf12cf3 419 //print frame data
ericoneill 7:6d5ddcf12cf3 420 pc.printf("printing frame data\n\r");
ericoneill 7:6d5ddcf12cf3 421 //int frameSize = frames.size();
ericoneill 7:6d5ddcf12cf3 422 //pc.printf("%i",frameSize);
ericoneill 7:6d5ddcf12cf3 423 pc.printf("[");
ericoneill 7:6d5ddcf12cf3 424 for(int i=0; i<numData; i++){
ericoneill 7:6d5ddcf12cf3 425 if(lineCenters > 0){
ericoneill 7:6d5ddcf12cf3 426 pc.printf("%i %i,",lineCenters[i], times[i]);
ericoneill 7:6d5ddcf12cf3 427 }
ericoneill 7:6d5ddcf12cf3 428 }
ericoneill 7:6d5ddcf12cf3 429 pc.printf("]\n\r");
mawk2311 0:ad375c052b4c 430 }