Framework for doing the figure 8 track.

Dependencies:   mbed

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 
00004 //Outputs
00005 DigitalOut led1(LED1);
00006 DigitalOut clk(PTD5);
00007 DigitalOut si(PTD4);
00008 PwmOut motor1(PTA4);
00009 PwmOut motor2(PTA12);
00010 DigitalOut break1(PTC12);
00011 DigitalOut break2(PTC13);
00012 PwmOut servo(PTA5);
00013 
00014 //Inputs
00015 AnalogIn camData(PTC2);
00016 
00017 //Encoder setup and variables
00018 InterruptIn interrupt(PTA13);
00019 
00020 //Line Tracking Variables --------------------------------
00021 float ADCdata [128];
00022 float minAccum;
00023 float minCount;
00024 float approxPos;
00025 float minVal;
00026 int minLoc;
00027 
00028 //Servo turning parameters
00029 float straight = 0.00155f;
00030 float hardLeft = 0.0013f;
00031 float slightLeft = 0.00145f;
00032 float hardRight = 0.0018f;
00033 float slightRight = 0.00165f;
00034 
00035 //End of Line Tracking Variables -------------------------
00036 
00037 //Encoder and Motor Driver Variables ---------------------
00038 
00039 //Intervals used during encoder data collection to measure velocity
00040 int interval1=0;
00041 int interval2=0;
00042 int interval3=0;
00043 int avg_interval=0;
00044 int lastchange1 = 0;
00045 int lastchange2 = 0;
00046 int lastchange3 = 0;
00047 int lastchange4 = 0;
00048 
00049 //Variables used to for velocity control
00050 float avg_speed = 0;
00051 float stall_check = 0;
00052 float tuning_val = 1;
00053 
00054 Timer t;
00055 Timer servoTimer;
00056 
00057 //Observed average speeds for each duty cycle
00058 const float DESIRED_SPEED = 0.5;
00059 const float TUNING_CONSTANT_10 = 1.10;
00060 const float TUNING_CONSTANT_20 = 3.00;
00061 const float TUNING_CONSTANT_30 = 4.30;
00062 const float TUNING_CONSTANT_50 = 6.880;
00063 const float PI = 3.14159;
00064 const float WHEEL_CIRCUMFERENCE = .05*PI;
00065 
00066 //Velocity Control Tuning Constants
00067 const float TUNE_THRESH = 0.5f;
00068 const float TUNE_AMT = 0.1f;
00069 
00070 //Parameters specifying sample sizes and delays for small and large average speed samples
00071 float num_samples_small = 3.0f;
00072 float delay_small = 0.05f;
00073 float num_samples_large = 100.0f;
00074 float delay_large = 0.1f;
00075 
00076 //Large and small arrays used to get average velocity values
00077 float large_avg_speed_list [100];
00078 float small_avg_speed_list [10];
00079 
00080 //End of Encoder and Motor Driver Variables ----------------------
00081 
00082 //Line Tracker Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
00083 
00084 int find_track(float line[]){
00085     int track_location = -1;
00086     float slope_threshold = .05;
00087     bool downslope_indices [128] = {false};
00088     bool upslope_indices [128] = {false};
00089     for(int i=10; i<118; i++){
00090         if(line[i+1] - line[i] < -slope_threshold && line[i+2] - line[i+1] < -slope_threshold){
00091             downslope_indices[i] = true;
00092         }
00093         if(line[i+1] - line[i] > slope_threshold && line[i+2] - line[i+1] > slope_threshold){
00094             upslope_indices[i] = true;
00095         }
00096     }
00097     int numDownslopes = 0;
00098     int numUpslopes = 0;
00099     for(int i=0; i<128; i++){
00100         if(downslope_indices[i] == true){
00101             numDownslopes ++;
00102         }
00103         if(upslope_indices[i] == true){
00104             numUpslopes ++;
00105         }
00106     }
00107     int downslope_locs [numDownslopes];
00108     int upslope_locs [numUpslopes];
00109     int dsctr = 0;
00110     int usctr = 0;
00111     
00112     for (int i=0; i<128; i++){
00113         if(downslope_indices[i] == true){
00114             downslope_locs[dsctr] = i;
00115             dsctr++;
00116         }
00117         if(upslope_indices[i] == true){
00118             upslope_locs[usctr] = i;
00119             usctr++;
00120         }
00121     }
00122     
00123     for(int i=0; i<numDownslopes; i++){
00124         for(int j=0; j<numUpslopes; j++){
00125             if(upslope_locs[j] - downslope_locs[i] >=4 && upslope_locs[j] - downslope_locs[i] <=5){
00126                 track_location = downslope_locs[i] + 2 ;
00127             }
00128         }
00129     }
00130 
00131     return track_location;
00132 }
00133 
00134 
00135 // Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
00136 
00137 float get_speed(){
00138     float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
00139     float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
00140     return linearSpeed;
00141 }
00142 
00143 float get_avg_speed(float num_samples, float delay) {
00144     
00145     float avg_avg_speed = 0;
00146     
00147     for (int c = 0; c < num_samples; c++) {
00148         if (num_samples == num_samples_small){
00149             small_avg_speed_list[c] = get_speed();
00150         } else if (num_samples == num_samples_large){
00151             large_avg_speed_list[c] = get_speed();
00152         }
00153         //wait(delay);
00154         }
00155                 
00156         for (int c = 1; c < num_samples; c++) {
00157             if (num_samples == num_samples_small){
00158                 avg_avg_speed += small_avg_speed_list[c];
00159             } else if (num_samples == num_samples_large){
00160                 avg_avg_speed += large_avg_speed_list[c];
00161             }
00162         }
00163     return avg_avg_speed/num_samples;
00164 }
00165 
00166 void velocity_control(float duty_cyc, float tuning_const) {
00167     
00168     avg_speed = get_speed();//get_avg_speed(num_samples_small, delay_small);
00169     
00170     if (avg_speed == stall_check) {
00171         avg_speed = 0;
00172         tuning_val += TUNE_AMT;
00173     } else if((avg_speed - tuning_const) > TUNE_THRESH){
00174         tuning_val -= TUNE_AMT;
00175         stall_check = avg_speed;
00176     } else if (tuning_const - avg_speed > TUNE_THRESH){
00177         //tuning_val += TUNE_AMT;
00178         stall_check = avg_speed;
00179     } else {
00180         //tuning_val = 1;
00181         stall_check = avg_speed;
00182     }
00183     motor1.pulsewidth(.0025 * duty_cyc * tuning_val);
00184     motor2.pulsewidth(.0025 * duty_cyc * tuning_val);
00185     
00186 }
00187 
00188 void fallInterrupt(){
00189     
00190     int time = t.read_us();
00191     interval1 = time - lastchange2;
00192     interval2 = lastchange1-lastchange3;
00193     interval3 = lastchange2 - lastchange4;
00194     avg_interval = (interval1 + interval2 + interval3)/3;
00195     
00196     lastchange4 = lastchange3;
00197     lastchange3 = lastchange2;
00198     lastchange2 = lastchange1;
00199     lastchange1 = time;
00200 }
00201 
00202 void riseInterrupt(){
00203     int time = t.read_us();
00204     interval1 = time - lastchange2;
00205     interval2 = lastchange1-lastchange3;
00206     interval3 = lastchange2 - lastchange4;
00207     avg_interval = (interval1 + interval2 + interval3)/3;
00208     
00209     lastchange4 = lastchange3;
00210     lastchange3 = lastchange2;
00211     lastchange2 = lastchange1;
00212     lastchange1 = time;
00213 }
00214 
00215 
00216 //End of Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
00217 
00218 int main() {
00219     //Line Tracker Initializations
00220     int integrationCounter = 0;
00221     
00222     // Motor Driver Initializations
00223     motor1.period(.0025);
00224     motor2.period(.0025);
00225     //interrupt.fall(&fallInterrupt);
00226     //interrupt.rise(&riseInterrupt);
00227 
00228     wait(5);
00229 
00230     motor1.pulsewidth(.0025*.105);
00231     motor2.pulsewidth(.0025*.105);
00232     break1 = 0;
00233     break2 = 0;
00234     
00235     //t.start();
00236     
00237     //wait(5);
00238     
00239     while(1) {
00240             
00241         if(integrationCounter % 151== 0){
00242             //Disable interrupts
00243             //__disable_irq();
00244             //interrupt.fall(NULL);
00245             //interrupt.rise(NULL);
00246             
00247             //Send start of integration signal
00248             si = 1;
00249             clk = 1;
00250 
00251             si = 0;
00252             clk = 0;
00253             
00254             //Reset timing counter for integration
00255             integrationCounter = 0;
00256             
00257             //Reset line tracking variables
00258             minAccum = 0;
00259             minCount = 0;
00260             approxPos = 0;
00261             
00262             //Reset Encoder and Motor Driver Variables
00263             interval1=0;
00264             interval2=0;
00265             interval3=0;
00266             avg_interval=0;
00267             lastchange1 = 0;
00268             lastchange2 = 0;
00269             lastchange3 = 0;
00270             lastchange4 = 0;
00271             
00272             t.reset();
00273                 
00274         }
00275         else if (integrationCounter > 129){
00276             //Enable interrupts
00277             //__enable_irq();
00278             //interrupt.fall(&fallInterrupt);
00279             //interrupt.rise(&riseInterrupt);
00280             
00281             minVal = ADCdata[15];
00282             for (int c = 10; c < 118; c++) {
00283                 if (ADCdata[c] > minVal){
00284                     minVal = ADCdata[c];
00285                     minLoc = c;
00286                 }
00287             }
00288             
00289             for (int c = 10; c < 118; c++) {
00290                 if (ADCdata[c] <= minVal && minVal - ADCdata[c] < 0.07f && ADCdata[c] > 0.1f){
00291                     minAccum += c;
00292                     minCount++;
00293                 }
00294             }
00295             
00296             approxPos = (float)minAccum/(float)minCount;
00297             
00298             
00299             //approxPos = find_track(ADCdata);
00300             
00301             if(approxPos > 0 && approxPos <= 25){
00302                     servo.pulsewidth(hardLeft);
00303                     motor1.pulsewidth(.0025*.1);
00304                     motor2.pulsewidth(.0025*.1);
00305             } else if (approxPos > 25 && approxPos <= 40){
00306                     servo.pulsewidth(slightLeft);
00307                     motor1.pulsewidth(.0025*.105);
00308                     motor2.pulsewidth(.0025*.105);
00309             } else if (approxPos > 40 && approxPos <= 90){
00310                 servo.pulsewidth(straight);
00311                     motor1.pulsewidth(.0025*.11);
00312                     motor2.pulsewidth(.0025*.11);
00313             } else if (approxPos > 90 && approxPos <= 105){
00314                     servo.pulsewidth(slightRight);
00315                     motor1.pulsewidth(.0025*.105);
00316                     motor2.pulsewidth(.0025*.105);
00317             } else if (approxPos > 105 && approxPos <= 128){
00318                     servo.pulsewidth(hardRight);
00319                     motor1.pulsewidth(.0025*.1);
00320                     motor2.pulsewidth(.0025*.1);
00321             }
00322             /*
00323             if(approxPos > 0 && approxPos <= 45){
00324                     servo.pulsewidth(hardLeft);
00325             } else if (approxPos > 45 && approxPos <= 95){
00326                     servo.pulsewidth(straight);
00327             } else {
00328                 servo.pulsewidth(hardRight);
00329             }*/
00330             //velocity_control(0.05f, DESIRED_SPEED);
00331             
00332             integrationCounter = 150;
00333         }
00334         else{
00335             clk = 1;
00336             wait_us(220);
00337             ADCdata[integrationCounter - 1] = camData;
00338             clk = 0;
00339         }
00340 
00341         //clk = 0;
00342         integrationCounter++;
00343         //camData.
00344         
00345     }
00346 }