Michael Romain / Mbed 2 deprecated coolcarsuperfast

Just trying to update ccode.

Dependencies:   mbed

Fork of coolcarsuperfast by E=MC

main.cpp@9:ad08181ad1cc, 2015-04-03 (annotated)

Committer:
ericoneill
Date:
Fri Apr 03 05:06:01 2015 +0000
Revision:
9:ad08181ad1cc
Parent:
8:e126c900c89d
Child:
10:e40ad924e935
recent;

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 6:f1d948d2d6c1 32 int spaceThresh = 5;
mawk2311 5:20223464f7aa 33 bool space;
mawk2311 0:ad375c052b4c 34
mawk2311 0:ad375c052b4c 35 //Servo turning parameters
mawk2311 0:ad375c052b4c 36 float straight = 0.00155f;
mawk2311 5:20223464f7aa 37 float hardLeft = 0.0010f;
mawk2311 5:20223464f7aa 38 float hardRight = 0.0021f;
mawk2311 0:ad375c052b4c 39
mawk2311 5:20223464f7aa 40 //Servo Directions
mawk2311 5:20223464f7aa 41 float currDir;
mawk2311 5:20223464f7aa 42 float prevDir;
mawk2311 5:20223464f7aa 43
ericoneill 7:6d5ddcf12cf3 44 // All linescan data for the period the car is running on. To be printed after a set amount of time
ericoneill 7:6d5ddcf12cf3 45 //std::vector<std::vector<int> > frames;
ericoneill 7:6d5ddcf12cf3 46 const int numData = 1000;
ericoneill 7:6d5ddcf12cf3 47 int lineCenters [numData] = {0};
ericoneill 7:6d5ddcf12cf3 48 int times [numData] = {0};
ericoneill 7:6d5ddcf12cf3 49 int loopCtr = 0;
ericoneill 7:6d5ddcf12cf3 50
mawk2311 0:ad375c052b4c 51 //End of Line Tracking Variables -------------------------
mawk2311 0:ad375c052b4c 52
mawk2311 0:ad375c052b4c 53 //Encoder and Motor Driver Variables ---------------------
mawk2311 0:ad375c052b4c 54
mawk2311 0:ad375c052b4c 55 //Intervals used during encoder data collection to measure velocity
mawk2311 0:ad375c052b4c 56 int interval1=0;
mawk2311 0:ad375c052b4c 57 int interval2=0;
mawk2311 0:ad375c052b4c 58 int interval3=0;
mawk2311 0:ad375c052b4c 59 int avg_interval=0;
mawk2311 0:ad375c052b4c 60 int lastchange1 = 0;
mawk2311 0:ad375c052b4c 61 int lastchange2 = 0;
mawk2311 0:ad375c052b4c 62 int lastchange3 = 0;
mawk2311 0:ad375c052b4c 63 int lastchange4 = 0;
mawk2311 0:ad375c052b4c 64
mawk2311 0:ad375c052b4c 65 //Variables used to for velocity control
mawk2311 0:ad375c052b4c 66 float avg_speed = 0;
mawk2311 5:20223464f7aa 67 float last_speed = 0;
mawk2311 5:20223464f7aa 68
mawk2311 0:ad375c052b4c 69 float stall_check = 0;
mawk2311 0:ad375c052b4c 70 float tuning_val = 1;
mawk2311 0:ad375c052b4c 71
mawk2311 6:f1d948d2d6c1 72 int numInterrupts = 0;
mawk2311 5:20223464f7aa 73
ericoneill 9:ad08181ad1cc 74 float pulsewidth = 0.15f;
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 6:f1d948d2d6c1 162 //Function for speeding up KL25Z ADC
mawk2311 6:f1d948d2d6c1 163 void initADC(void){
mawk2311 6:f1d948d2d6c1 164
mawk2311 6:f1d948d2d6c1 165 ADC0->CFG1 = ADC0->CFG1 & (
mawk2311 6:f1d948d2d6c1 166 ~(
mawk2311 6:f1d948d2d6c1 167 0x80 // LDLPC = 0 ; no low-power mode
mawk2311 6:f1d948d2d6c1 168 | 0x60 // ADIV = 1
mawk2311 6:f1d948d2d6c1 169 | 0x10 // Sample time short
mawk2311 6:f1d948d2d6c1 170 | 0x03 // input clock = BUS CLK
mawk2311 6:f1d948d2d6c1 171 )
mawk2311 6:f1d948d2d6c1 172 ) ; // clkdiv <= 1
mawk2311 6:f1d948d2d6c1 173 ADC0->CFG2 = ADC0->CFG2
mawk2311 6:f1d948d2d6c1 174 | 0x03 ; // Logsample Time 11 = 2 extra ADCK
mawk2311 6:f1d948d2d6c1 175 ADC0->SC3 = ADC0->SC3
mawk2311 6:f1d948d2d6c1 176 & (~(0x03)) ; // hardware avarage off
mawk2311 6:f1d948d2d6c1 177 }
mawk2311 6:f1d948d2d6c1 178
mawk2311 1:55e0aaf71bda 179
mawk2311 0:ad375c052b4c 180 // Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 0:ad375c052b4c 181
mawk2311 0:ad375c052b4c 182 float get_speed(){
mawk2311 0:ad375c052b4c 183 float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
mawk2311 0:ad375c052b4c 184 float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
mawk2311 0:ad375c052b4c 185 return linearSpeed;
mawk2311 0:ad375c052b4c 186 }
mawk2311 0:ad375c052b4c 187
mawk2311 0:ad375c052b4c 188 float get_avg_speed(float num_samples, float delay) {
mawk2311 0:ad375c052b4c 189
mawk2311 0:ad375c052b4c 190 float avg_avg_speed = 0;
mawk2311 0:ad375c052b4c 191
mawk2311 0:ad375c052b4c 192 for (int c = 0; c < num_samples; c++) {
mawk2311 0:ad375c052b4c 193 if (num_samples == num_samples_small){
mawk2311 0:ad375c052b4c 194 small_avg_speed_list[c] = get_speed();
mawk2311 0:ad375c052b4c 195 } else if (num_samples == num_samples_large){
mawk2311 0:ad375c052b4c 196 large_avg_speed_list[c] = get_speed();
mawk2311 0:ad375c052b4c 197 }
mawk2311 0:ad375c052b4c 198 //wait(delay);
mawk2311 0:ad375c052b4c 199 }
mawk2311 0:ad375c052b4c 200
mawk2311 0:ad375c052b4c 201 for (int c = 1; c < num_samples; c++) {
mawk2311 0:ad375c052b4c 202 if (num_samples == num_samples_small){
mawk2311 0:ad375c052b4c 203 avg_avg_speed += small_avg_speed_list[c];
mawk2311 0:ad375c052b4c 204 } else if (num_samples == num_samples_large){
mawk2311 0:ad375c052b4c 205 avg_avg_speed += large_avg_speed_list[c];
mawk2311 0:ad375c052b4c 206 }
mawk2311 0:ad375c052b4c 207 }
mawk2311 0:ad375c052b4c 208 return avg_avg_speed/num_samples;
mawk2311 0:ad375c052b4c 209 }
mawk2311 0:ad375c052b4c 210
mawk2311 0:ad375c052b4c 211 void velocity_control(float duty_cyc, float tuning_const) {
mawk2311 0:ad375c052b4c 212
mawk2311 0:ad375c052b4c 213 avg_speed = get_speed();//get_avg_speed(num_samples_small, delay_small);
mawk2311 0:ad375c052b4c 214
mawk2311 5:20223464f7aa 215 //When determined speed is infinity or 0, set the speed to the last agreeable speed
mawk2311 6:f1d948d2d6c1 216 /*if (avg_speed > 100 || avg_speed == 0){
mawk2311 5:20223464f7aa 217 avg_speed = last_speed;
mawk2311 6:f1d948d2d6c1 218 }*/
mawk2311 5:20223464f7aa 219
mawk2311 5:20223464f7aa 220 pc.printf("\n\r%f", avg_speed);
mawk2311 5:20223464f7aa 221 if (avg_speed == stall_check && tuning_const != 0 && avg_speed == 0) {
mawk2311 0:ad375c052b4c 222 avg_speed = 0;
mawk2311 0:ad375c052b4c 223 tuning_val += TUNE_AMT;
mawk2311 0:ad375c052b4c 224 } else if((avg_speed - tuning_const) > TUNE_THRESH){
mawk2311 0:ad375c052b4c 225 tuning_val -= TUNE_AMT;
mawk2311 0:ad375c052b4c 226 stall_check = avg_speed;
mawk2311 5:20223464f7aa 227
mawk2311 5:20223464f7aa 228 } else if (tuning_const - avg_speed > TUNE_THRESH && avg_speed != 0){
mawk2311 5:20223464f7aa 229 tuning_val += TUNE_AMT;
mawk2311 0:ad375c052b4c 230 stall_check = avg_speed;
mawk2311 0:ad375c052b4c 231 } else {
mawk2311 0:ad375c052b4c 232 //tuning_val = 1;
mawk2311 0:ad375c052b4c 233 stall_check = avg_speed;
mawk2311 0:ad375c052b4c 234 }
mawk2311 0:ad375c052b4c 235
mawk2311 5:20223464f7aa 236 if (tuning_val < .5){
mawk2311 5:20223464f7aa 237 tuning_val = .5;
mawk2311 5:20223464f7aa 238 }
mawk2311 5:20223464f7aa 239 pc.printf("\n\rTuning Val: %f", tuning_val);
mawk2311 5:20223464f7aa 240 motor1.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
mawk2311 5:20223464f7aa 241 motor2.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
mawk2311 5:20223464f7aa 242
mawk2311 5:20223464f7aa 243 if (avg_speed != 0){
mawk2311 5:20223464f7aa 244 last_speed = avg_speed;
mawk2311 5:20223464f7aa 245 }
mawk2311 5:20223464f7aa 246
mawk2311 0:ad375c052b4c 247 }
mawk2311 0:ad375c052b4c 248
mawk2311 6:f1d948d2d6c1 249 // Interrupt Functions for Encoder ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~``
mawk2311 0:ad375c052b4c 250 void fallInterrupt(){
mawk2311 0:ad375c052b4c 251
mawk2311 0:ad375c052b4c 252 int time = t.read_us();
mawk2311 0:ad375c052b4c 253 interval1 = time - lastchange2;
mawk2311 0:ad375c052b4c 254 interval2 = lastchange1-lastchange3;
mawk2311 0:ad375c052b4c 255 interval3 = lastchange2 - lastchange4;
mawk2311 0:ad375c052b4c 256 avg_interval = (interval1 + interval2 + interval3)/3;
mawk2311 0:ad375c052b4c 257
mawk2311 0:ad375c052b4c 258 lastchange4 = lastchange3;
mawk2311 0:ad375c052b4c 259 lastchange3 = lastchange2;
mawk2311 0:ad375c052b4c 260 lastchange2 = lastchange1;
mawk2311 0:ad375c052b4c 261 lastchange1 = time;
mawk2311 6:f1d948d2d6c1 262
mawk2311 6:f1d948d2d6c1 263 numInterrupts++;
mawk2311 0:ad375c052b4c 264 }
mawk2311 0:ad375c052b4c 265
mawk2311 0:ad375c052b4c 266 void riseInterrupt(){
mawk2311 0:ad375c052b4c 267 int time = t.read_us();
mawk2311 0:ad375c052b4c 268 interval1 = time - lastchange2;
mawk2311 0:ad375c052b4c 269 interval2 = lastchange1-lastchange3;
mawk2311 0:ad375c052b4c 270 interval3 = lastchange2 - lastchange4;
mawk2311 0:ad375c052b4c 271 avg_interval = (interval1 + interval2 + interval3)/3;
mawk2311 0:ad375c052b4c 272
mawk2311 0:ad375c052b4c 273 lastchange4 = lastchange3;
mawk2311 0:ad375c052b4c 274 lastchange3 = lastchange2;
mawk2311 0:ad375c052b4c 275 lastchange2 = lastchange1;
mawk2311 0:ad375c052b4c 276 lastchange1 = time;
mawk2311 6:f1d948d2d6c1 277
mawk2311 6:f1d948d2d6c1 278 numInterrupts++;
mawk2311 0:ad375c052b4c 279 }
mawk2311 0:ad375c052b4c 280
mawk2311 0:ad375c052b4c 281
mawk2311 0:ad375c052b4c 282 //End of Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 0:ad375c052b4c 283
mawk2311 0:ad375c052b4c 284 int main() {
mawk2311 6:f1d948d2d6c1 285
mawk2311 6:f1d948d2d6c1 286 //Alter reg values to speed up KL25Z
mawk2311 6:f1d948d2d6c1 287 initADC();
mawk2311 6:f1d948d2d6c1 288
mawk2311 0:ad375c052b4c 289 //Line Tracker Initializations
mawk2311 0:ad375c052b4c 290 int integrationCounter = 0;
mawk2311 0:ad375c052b4c 291
mawk2311 6:f1d948d2d6c1 292 //Initial values for directions
mawk2311 5:20223464f7aa 293 currDir = 0;
mawk2311 5:20223464f7aa 294 prevDir = 0;
mawk2311 5:20223464f7aa 295
mawk2311 0:ad375c052b4c 296 // Motor Driver Initializations
mawk2311 5:20223464f7aa 297 motor1.period(motorPeriod);
mawk2311 5:20223464f7aa 298 motor2.period(motorPeriod);
mawk2311 0:ad375c052b4c 299
mawk2311 5:20223464f7aa 300 // Servo Initialization
mawk2311 5:20223464f7aa 301 servo.period(servoPeriod);
mawk2311 5:20223464f7aa 302
mawk2311 5:20223464f7aa 303 wait(3);
mawk2311 0:ad375c052b4c 304
mawk2311 5:20223464f7aa 305 motor1.pulsewidth(motorPeriod*pulsewidth);
mawk2311 5:20223464f7aa 306 motor2.pulsewidth(motorPeriod*pulsewidth);
mawk2311 0:ad375c052b4c 307 break1 = 0;
mawk2311 0:ad375c052b4c 308 break2 = 0;
mawk2311 0:ad375c052b4c 309
ericoneill 9:ad08181ad1cc 310 int loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime = 0;
mawk2311 5:20223464f7aa 311 t.start();
ericoneill 7:6d5ddcf12cf3 312 printTimer.start();
mawk2311 1:55e0aaf71bda 313 //wait(5);
ericoneill 8:e126c900c89d 314
mawk2311 0:ad375c052b4c 315 while(1) {
ericoneill 7:6d5ddcf12cf3 316 //break out of main loop if enough time has passed;
ericoneill 7:6d5ddcf12cf3 317 if(loopCtr >= numData){
ericoneill 7:6d5ddcf12cf3 318 break;
ericoneill 7:6d5ddcf12cf3 319 }
mawk2311 0:ad375c052b4c 320 if(integrationCounter % 151== 0){
mawk2311 0:ad375c052b4c 321 //Disable interrupts
mawk2311 5:20223464f7aa 322 interrupt.fall(NULL);
mawk2311 5:20223464f7aa 323 interrupt.rise(NULL);
ericoneill 7:6d5ddcf12cf3 324
ericoneill 7:6d5ddcf12cf3 325 /*
ericoneill 7:6d5ddcf12cf3 326 // copy frame into the vector to be printed
ericoneill 7:6d5ddcf12cf3 327 std::vector<int> temp;
ericoneill 7:6d5ddcf12cf3 328 temp.reserve(128); //performance
ericoneill 7:6d5ddcf12cf3 329 for(int i=0; i<128; i++){
ericoneill 7:6d5ddcf12cf3 330 temp.push_back(ADCdata[i]);
ericoneill 7:6d5ddcf12cf3 331 }
ericoneill 7:6d5ddcf12cf3 332 frames.push_back(temp);
ericoneill 7:6d5ddcf12cf3 333 */
ericoneill 8:e126c900c89d 334
mawk2311 0:ad375c052b4c 335 //Send start of integration signal
mawk2311 0:ad375c052b4c 336 si = 1;
mawk2311 0:ad375c052b4c 337 clk = 1;
mawk2311 0:ad375c052b4c 338
mawk2311 0:ad375c052b4c 339 si = 0;
mawk2311 0:ad375c052b4c 340 clk = 0;
mawk2311 0:ad375c052b4c 341
mawk2311 0:ad375c052b4c 342 //Reset timing counter for integration
mawk2311 0:ad375c052b4c 343 integrationCounter = 0;
mawk2311 0:ad375c052b4c 344
mawk2311 0:ad375c052b4c 345 //Reset line tracking variables
mawk2311 5:20223464f7aa 346 maxAccum = 0;
mawk2311 5:20223464f7aa 347 maxCount = 0;
mawk2311 0:ad375c052b4c 348 approxPos = 0;
mawk2311 0:ad375c052b4c 349
mawk2311 5:20223464f7aa 350 space = false;
mawk2311 6:f1d948d2d6c1 351
mawk2311 0:ad375c052b4c 352 }
mawk2311 0:ad375c052b4c 353 else if (integrationCounter > 129){
mawk2311 5:20223464f7aa 354 //Start Timer
mawk2311 6:f1d948d2d6c1 355 //t.start();
mawk2311 0:ad375c052b4c 356
mawk2311 5:20223464f7aa 357 //Enable interrupts
mawk2311 6:f1d948d2d6c1 358 //interrupt.fall(&fallInterrupt);
mawk2311 6:f1d948d2d6c1 359 //interrupt.rise(&riseInterrupt);
mawk2311 5:20223464f7aa 360
mawk2311 5:20223464f7aa 361 maxVal = ADCdata[10];
mawk2311 5:20223464f7aa 362 for (int c = 11; c < 118; c++) {
mawk2311 5:20223464f7aa 363 if (ADCdata[c] > maxVal){
mawk2311 5:20223464f7aa 364 maxVal = ADCdata[c];
mawk2311 5:20223464f7aa 365 maxLoc = c;
mawk2311 0:ad375c052b4c 366 }
mawk2311 0:ad375c052b4c 367 }
mawk2311 0:ad375c052b4c 368
mawk2311 1:55e0aaf71bda 369 for (int c = 10; c < 118; c++) {
mawk2311 5:20223464f7aa 370 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.07f && ADCdata[c] > 0.1f){
mawk2311 5:20223464f7aa 371 maxAccum += c;
mawk2311 5:20223464f7aa 372 maxCount++;
mawk2311 6:f1d948d2d6c1 373 if (c > prevTrackLoc + spaceThresh){
mawk2311 5:20223464f7aa 374 space = true;
mawk2311 5:20223464f7aa 375 }
mawk2311 5:20223464f7aa 376 prevTrackLoc = c;
mawk2311 0:ad375c052b4c 377 }
mawk2311 0:ad375c052b4c 378 }
mawk2311 0:ad375c052b4c 379
mawk2311 6:f1d948d2d6c1 380 //Line Crossing Checks
mawk2311 6:f1d948d2d6c1 381 if (maxCount >= 15 || space) {
mawk2311 5:20223464f7aa 382 currDir = prevDir;
mawk2311 5:20223464f7aa 383 } else {
mawk2311 5:20223464f7aa 384 approxPos = (float)maxAccum/(float)maxCount;
ericoneill 8:e126c900c89d 385
ericoneill 9:ad08181ad1cc 386 if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%3==0){
ericoneill 9:ad08181ad1cc 387 lineCenters[loopCtr] = approxPos;
ericoneill 9:ad08181ad1cc 388 times[loopCtr] = printTimer.read_ms();
ericoneill 9:ad08181ad1cc 389 loopCtr++;
ericoneill 9:ad08181ad1cc 390 }
mawk2311 5:20223464f7aa 391 currDir = hardLeft + approxPos/((float) 127)*(hardRight-hardLeft);
ericoneill 9:ad08181ad1cc 392
ericoneill 9:ad08181ad1cc 393
mawk2311 5:20223464f7aa 394 }
mawk2311 1:55e0aaf71bda 395
mawk2311 6:f1d948d2d6c1 396 //Change speed when turning at different angles
mawk2311 4:09c68df71785 397 /*if(approxPos > 0 && approxPos <= 45){
mawk2311 6:f1d948d2d6c1 398 motor1.pulsewidth(motorPeriod*(pulsewidth/2));
mawk2311 6:f1d948d2d6c1 399 motor2.pulsewidth(motorPeriod*(pulsewidth/2));
mawk2311 1:55e0aaf71bda 400 } else if (approxPos > 45 && approxPos <= 95){
mawk2311 5:20223464f7aa 401 motor1.pulsewidth(motorPeriod*pulsewidth);
mawk2311 5:20223464f7aa 402 motor2.pulsewidth(motorPeriod*pulsewidth);
mawk2311 1:55e0aaf71bda 403 } else {
mawk2311 6:f1d948d2d6c1 404 motor1.pulsewidth(motorPeriod*(pulsewidth/2));
mawk2311 6:f1d948d2d6c1 405 motor2.pulsewidth(motorPeriod*(pulsewidth/2));
mawk2311 1:55e0aaf71bda 406 }*/
mawk2311 4:09c68df71785 407
mawk2311 5:20223464f7aa 408 servo.pulsewidth(currDir);
mawk2311 5:20223464f7aa 409
mawk2311 6:f1d948d2d6c1 410 //Start Velocity control after requisite number of encoder signals have been collected
mawk2311 6:f1d948d2d6c1 411 //if(numInterrupts >= 4){
mawk2311 6:f1d948d2d6c1 412 //velocity_control(0.1f, TUNING_CONSTANT_10);
mawk2311 6:f1d948d2d6c1 413 //}
mawk2311 4:09c68df71785 414
mawk2311 6:f1d948d2d6c1 415 //Save current direction as previous direction
mawk2311 5:20223464f7aa 416 prevDir = currDir;
mawk2311 0:ad375c052b4c 417
mawk2311 6:f1d948d2d6c1 418 //Prepare to start collecting more data
mawk2311 0:ad375c052b4c 419 integrationCounter = 150;
mawk2311 6:f1d948d2d6c1 420
mawk2311 6:f1d948d2d6c1 421 //Disable interrupts
mawk2311 6:f1d948d2d6c1 422 //interrupt.fall(NULL);
mawk2311 6:f1d948d2d6c1 423 //interrupt.rise(NULL);
mawk2311 6:f1d948d2d6c1 424
mawk2311 6:f1d948d2d6c1 425 //Stop timer
mawk2311 6:f1d948d2d6c1 426 //t.stop();
mawk2311 0:ad375c052b4c 427 }
mawk2311 0:ad375c052b4c 428 else{
mawk2311 0:ad375c052b4c 429 clk = 1;
mawk2311 5:20223464f7aa 430 wait_us(10);
mawk2311 3:e867c4e984df 431 ADCdata[integrationCounter - 1] = camData;
mawk2311 0:ad375c052b4c 432 clk = 0;
mawk2311 0:ad375c052b4c 433 }
mawk2311 0:ad375c052b4c 434
mawk2311 0:ad375c052b4c 435 //clk = 0;
mawk2311 0:ad375c052b4c 436 integrationCounter++;
ericoneill 9:ad08181ad1cc 437 loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime++;
mawk2311 0:ad375c052b4c 438 //camData.
mawk2311 0:ad375c052b4c 439
mawk2311 0:ad375c052b4c 440 }
ericoneill 7:6d5ddcf12cf3 441
ericoneill 7:6d5ddcf12cf3 442 //print frame data
ericoneill 7:6d5ddcf12cf3 443 pc.printf("printing frame data\n\r");
ericoneill 7:6d5ddcf12cf3 444 //int frameSize = frames.size();
ericoneill 7:6d5ddcf12cf3 445 //pc.printf("%i",frameSize);
ericoneill 7:6d5ddcf12cf3 446 pc.printf("[");
ericoneill 7:6d5ddcf12cf3 447 for(int i=0; i<numData; i++){
ericoneill 7:6d5ddcf12cf3 448 if(lineCenters > 0){
ericoneill 7:6d5ddcf12cf3 449 pc.printf("%i %i,",lineCenters[i], times[i]);
ericoneill 7:6d5ddcf12cf3 450 }
ericoneill 7:6d5ddcf12cf3 451 }
ericoneill 7:6d5ddcf12cf3 452 pc.printf("]\n\r");
mawk2311 0:ad375c052b4c 453 }
ericoneill 9:ad08181ad1cc 454