Michael Romain / Mbed 2 deprecated coolcarsuperfast

Just trying to update ccode.

Dependencies:   mbed

Fork of coolcarsuperfast by E=MC

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

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