Framework for doing the figure 8 track.

Dependencies:   mbed

Fork of Shitty_figure8 by Michael Romain

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "stdlib.h"
00003 #include <vector>
00004 //Outputs
00005 DigitalOut led1(LED1);
00006 DigitalOut clk(PTD5);
00007 DigitalOut si(PTD4);
00008 PwmOut motor1(PTA12);
00009 PwmOut motor2(PTA4);
00010 DigitalOut break1(PTC7);
00011 DigitalOut break2(PTC0);
00012 PwmOut servo(PTA5);
00013 
00014 Serial pc(USBTX, USBRX); // tx, rx
00015 
00016 //Inputs
00017 AnalogIn camData(PTC2);
00018 
00019 //Encoder setup and variables
00020 InterruptIn interrupt(PTA13);
00021 
00022 //Line Tracking Variables --------------------------------
00023 float ADCdata [128];
00024 float maxAccum;
00025 float maxCount;
00026 float approxPos;
00027 float maxVal;
00028 int maxLoc;
00029 
00030 //Line Crossing variables
00031 int prevTrackLoc;
00032 int spaceThresh = 5;
00033 bool space;
00034 
00035 //Servo turning parameters
00036 float straight = 0.00155f;
00037 float hardLeft = 0.0010f;
00038 float hardRight = 0.0021f;
00039 
00040 //Servo Directions
00041 float currDir;
00042 float prevDir;
00043 
00044 // All linescan data for the period the car is running on. To be printed after a set amount of time
00045 //std::vector<std::vector<int> > frames;
00046 const int numData = 1000;
00047 int lineCenters [numData] = {0};
00048 int times [numData] = {0};
00049 int loopCtr = 0;
00050 
00051 //End of Line Tracking Variables -------------------------
00052 
00053 //Encoder and Motor Driver Variables ---------------------
00054 
00055 //Intervals used during encoder data collection to measure velocity
00056 int interval1=0;
00057 int interval2=0;
00058 int interval3=0;
00059 int avg_interval=0;
00060 int lastchange1 = 0;
00061 int lastchange2 = 0;
00062 int lastchange3 = 0;
00063 int lastchange4 = 0;
00064 
00065 //Variables used to for velocity control
00066 float avg_speed = 0;
00067 float last_speed = 0;
00068 
00069 float stall_check = 0;
00070 float tuning_val = 1;
00071 
00072 int numInterrupts = 0;
00073 
00074 float pulsewidth = 0.15f;
00075 
00076 // Hardware periods
00077 float motorPeriod = .0025;
00078 float servoPeriod = .0025;
00079 
00080 Timer t;
00081 Timer servoTimer;
00082 Timer printTimer; //after printTimer reaches a certain value the main loop will terminate and print the frames
00083 const float printTime = 5.0f;
00084 
00085 //Observed average speeds for each duty cycle
00086 const float DESIRED_SPEED = 1;
00087 const float TUNING_CONSTANT_10 = 1.90;
00088 const float TUNING_CONSTANT_20 = 3.00;
00089 const float TUNING_CONSTANT_30 = 4.30;
00090 const float TUNING_CONSTANT_50 = 6.880;
00091 const float PI = 3.14159;
00092 const float WHEEL_CIRCUMFERENCE = .05*PI;
00093 
00094 //Velocity Control Tuning Constants
00095 const float TUNE_THRESH = 0.5f;
00096 const float TUNE_AMT = 0.1f;
00097 
00098 //Parameters specifying sample sizes and delays for small and large average speed samples
00099 float num_samples_small = 3.0f;
00100 float delay_small = 0.05f;
00101 float num_samples_large = 100.0f;
00102 float delay_large = 0.1f;
00103 
00104 //Large and small arrays used to get average velocity values
00105 float large_avg_speed_list [100];
00106 float small_avg_speed_list [10];
00107 
00108 //End of Encoder and Motor Driver Variables ----------------------
00109 
00110 //Line Tracker Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
00111 
00112 int find_track(float line[]){
00113     int track_location = -1;
00114     float slope_threshold = .05;
00115     bool downslope_indices [128] = {false};
00116     bool upslope_indices [128] = {false};
00117     for(int i=10; i<118; i++){
00118         if(line[i+1] - line[i] < -slope_threshold && line[i+2] - line[i+1] < -slope_threshold){
00119             downslope_indices[i] = true;
00120         }
00121         if(line[i+1] - line[i] > slope_threshold && line[i+2] - line[i+1] > slope_threshold){
00122             upslope_indices[i] = true;
00123         }
00124     }
00125     int numDownslopes = 0;
00126     int numUpslopes = 0;
00127     for(int i=0; i<128; i++){
00128         if(downslope_indices[i] == true){
00129             numDownslopes ++;
00130         }
00131         if(upslope_indices[i] == true){
00132             numUpslopes ++;
00133         }
00134     }
00135     int downslope_locs [numDownslopes];
00136     int upslope_locs [numUpslopes];
00137     int dsctr = 0;
00138     int usctr = 0;
00139     
00140     for (int i=0; i<128; i++){
00141         if(downslope_indices[i] == true){
00142             downslope_locs[dsctr] = i;
00143             dsctr++;
00144         }
00145         if(upslope_indices[i] == true){
00146             upslope_locs[usctr] = i;
00147             usctr++;
00148         }
00149     }
00150     
00151     for(int i=0; i<numDownslopes; i++){
00152         for(int j=0; j<numUpslopes; j++){
00153             if(upslope_locs[j] - downslope_locs[i] >=4 && upslope_locs[j] - downslope_locs[i] <=5){
00154                 track_location = downslope_locs[i] + 2 ;
00155             }
00156         }
00157     }
00158 
00159     return track_location;
00160 }
00161 
00162 //Function for speeding up KL25Z ADC
00163 void initADC(void){
00164  
00165     ADC0->CFG1 = ADC0->CFG1 & (
00166         ~(
00167           0x80 // LDLPC = 0 ; no low-power mode
00168         | 0x60 // ADIV = 1
00169         | 0x10 // Sample time short
00170         | 0x03 // input clock = BUS CLK
00171         )
00172     ) ; // clkdiv <= 1
00173     ADC0->CFG2 = ADC0->CFG2 
00174         | 0x03 ; // Logsample Time 11 = 2 extra ADCK
00175     ADC0->SC3 = ADC0->SC3 
00176         & (~(0x03)) ; // hardware avarage off
00177 }
00178 
00179 
00180 // Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
00181 
00182 float get_speed(){
00183     float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
00184     float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
00185     return linearSpeed;
00186 }
00187 
00188 float get_avg_speed(float num_samples, float delay) {
00189     
00190     float avg_avg_speed = 0;
00191     
00192     for (int c = 0; c < num_samples; c++) {
00193         if (num_samples == num_samples_small){
00194             small_avg_speed_list[c] = get_speed();
00195         } else if (num_samples == num_samples_large){
00196             large_avg_speed_list[c] = get_speed();
00197         }
00198         //wait(delay);
00199         }
00200                 
00201         for (int c = 1; c < num_samples; c++) {
00202             if (num_samples == num_samples_small){
00203                 avg_avg_speed += small_avg_speed_list[c];
00204             } else if (num_samples == num_samples_large){
00205                 avg_avg_speed += large_avg_speed_list[c];
00206             }
00207         }
00208     return avg_avg_speed/num_samples;
00209 }
00210 
00211 void velocity_control(float duty_cyc, float tuning_const) {
00212     
00213     avg_speed = get_speed();//get_avg_speed(num_samples_small, delay_small);
00214     
00215     //When determined speed is infinity or 0, set the speed to the last agreeable speed
00216     /*if (avg_speed > 100 || avg_speed == 0){
00217         avg_speed = last_speed;
00218     }*/
00219     
00220     pc.printf("\n\r%f", avg_speed);
00221     if (avg_speed == stall_check && tuning_const != 0 && avg_speed == 0) {
00222         avg_speed = 0;
00223         tuning_val += TUNE_AMT;
00224     } else if((avg_speed - tuning_const) > TUNE_THRESH){
00225         tuning_val -= TUNE_AMT;
00226         stall_check = avg_speed;
00227 
00228     } else if (tuning_const - avg_speed > TUNE_THRESH && avg_speed != 0){
00229         tuning_val += TUNE_AMT;
00230         stall_check = avg_speed;
00231     } else {
00232         //tuning_val = 1;
00233         stall_check = avg_speed;
00234     }
00235     
00236     if (tuning_val < .5){
00237         tuning_val = .5;
00238     }
00239     pc.printf("\n\rTuning Val: %f", tuning_val);
00240     motor1.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
00241     motor2.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
00242     
00243     if (avg_speed != 0){
00244         last_speed = avg_speed;
00245     }
00246 
00247 }
00248 
00249 // Interrupt Functions for Encoder ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~``
00250 void fallInterrupt(){
00251     
00252     int time = t.read_us();
00253     interval1 = time - lastchange2;
00254     interval2 = lastchange1-lastchange3;
00255     interval3 = lastchange2 - lastchange4;
00256     avg_interval = (interval1 + interval2 + interval3)/3;
00257     
00258     lastchange4 = lastchange3;
00259     lastchange3 = lastchange2;
00260     lastchange2 = lastchange1;
00261     lastchange1 = time;
00262     
00263     numInterrupts++;
00264 }
00265 
00266 void riseInterrupt(){
00267     int time = t.read_us();
00268     interval1 = time - lastchange2;
00269     interval2 = lastchange1-lastchange3;
00270     interval3 = lastchange2 - lastchange4;
00271     avg_interval = (interval1 + interval2 + interval3)/3;
00272     
00273     lastchange4 = lastchange3;
00274     lastchange3 = lastchange2;
00275     lastchange2 = lastchange1;
00276     lastchange1 = time;
00277     
00278     numInterrupts++;
00279 }
00280 
00281 
00282 //End of Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
00283 
00284 int main() {
00285     
00286     //Alter reg values to speed up KL25Z
00287     initADC();
00288     
00289     //Line Tracker Initializations
00290     int integrationCounter = 0;
00291     
00292     //Initial values for directions
00293     currDir = 0;
00294     prevDir = 0;
00295     
00296     // Motor Driver Initializations
00297     motor1.period(motorPeriod);
00298     motor2.period(motorPeriod);
00299 
00300     // Servo Initialization
00301     servo.period(servoPeriod);
00302     
00303     wait(3);
00304 
00305     motor1.pulsewidth(motorPeriod*pulsewidth);
00306     motor2.pulsewidth(motorPeriod*pulsewidth);
00307     break1 = 0;
00308     break2 = 0;
00309     
00310     int loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime = 0;
00311     t.start();
00312     printTimer.start();
00313     //wait(5);
00314 
00315     while(1) {
00316         //break out of main loop if enough time has passed;    
00317         if(loopCtr >= numData){
00318             break;
00319         }
00320         if(integrationCounter % 151== 0){
00321             //Disable interrupts
00322             interrupt.fall(NULL);
00323             interrupt.rise(NULL);
00324           
00325             /*
00326             // copy frame into the vector to be printed  
00327             std::vector<int> temp;
00328             temp.reserve(128); //performance
00329             for(int i=0; i<128; i++){
00330                 temp.push_back(ADCdata[i]);
00331             }
00332             frames.push_back(temp);
00333             */
00334 
00335             //Send start of integration signal
00336             si = 1;
00337             clk = 1;
00338 
00339             si = 0;
00340             clk = 0;
00341             
00342             //Reset timing counter for integration
00343             integrationCounter = 0;
00344             
00345             //Reset line tracking variables
00346             maxAccum = 0;
00347             maxCount = 0;
00348             approxPos = 0;
00349             
00350             space = false;
00351 
00352         }
00353         else if (integrationCounter > 129){
00354             //Start Timer
00355             //t.start();
00356             
00357             //Enable interrupts
00358             //interrupt.fall(&fallInterrupt);
00359             //interrupt.rise(&riseInterrupt);
00360             
00361             maxVal = ADCdata[10];
00362             for (int c = 11; c < 118; c++) {
00363                 if (ADCdata[c] > maxVal){
00364                     maxVal = ADCdata[c];
00365                     maxLoc = c;
00366                 }
00367             }
00368             
00369             for (int c = 10; c < 118; c++) {
00370                 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.07f && ADCdata[c] > 0.1f){
00371                     maxAccum += c;
00372                     maxCount++;
00373                     if (c > prevTrackLoc + spaceThresh){
00374                         space = true;
00375                     }
00376                     prevTrackLoc = c;
00377                 }
00378             }
00379             
00380             //Line Crossing Checks
00381             if (maxCount >= 15 || space) {
00382                 currDir = prevDir;
00383             } else {
00384                 approxPos = (float)maxAccum/(float)maxCount;
00385 
00386                 if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%3==0){
00387                     lineCenters[loopCtr] = approxPos;
00388                     times[loopCtr] = printTimer.read_ms();
00389                     loopCtr++;
00390                 }
00391                 currDir = hardLeft + approxPos/((float) 127)*(hardRight-hardLeft);
00392                 
00393 
00394             }
00395             
00396             //Change speed when turning at different angles
00397             /*if(approxPos > 0 && approxPos <= 45){
00398                     motor1.pulsewidth(motorPeriod*(pulsewidth/2));
00399                     motor2.pulsewidth(motorPeriod*(pulsewidth/2));
00400             } else if (approxPos > 45 && approxPos <= 95){
00401                     motor1.pulsewidth(motorPeriod*pulsewidth);
00402                     motor2.pulsewidth(motorPeriod*pulsewidth);
00403             } else {
00404                 motor1.pulsewidth(motorPeriod*(pulsewidth/2));
00405                 motor2.pulsewidth(motorPeriod*(pulsewidth/2));
00406             }*/
00407             
00408             servo.pulsewidth(currDir);
00409             
00410             //Start Velocity control after requisite number of encoder signals have been collected
00411             //if(numInterrupts >= 4){
00412                 //velocity_control(0.1f, TUNING_CONSTANT_10);
00413             //}        
00414             
00415             //Save current direction as previous direction
00416             prevDir = currDir;
00417             
00418             //Prepare to start collecting more data
00419             integrationCounter = 150;
00420             
00421             //Disable interrupts
00422             //interrupt.fall(NULL);
00423             //interrupt.rise(NULL);
00424             
00425             //Stop timer
00426             //t.stop();                
00427         }
00428         else{
00429             clk = 1;
00430             wait_us(10);
00431             ADCdata[integrationCounter - 1] = camData;
00432             clk = 0;
00433         }
00434 
00435         //clk = 0;
00436         integrationCounter++;
00437         loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime++;
00438         //camData.
00439         
00440     }
00441     
00442     //print frame data
00443     pc.printf("printing frame data\n\r");
00444     //int frameSize = frames.size();
00445     //pc.printf("%i",frameSize);
00446     pc.printf("[");
00447     for(int i=0; i<numData; i++){
00448         if(lineCenters > 0){
00449             pc.printf("%i %i,",lineCenters[i], times[i]);
00450         }
00451     }
00452     pc.printf("]\n\r");
00453 }
00454