encoder.cpp

00001 #include <encoder.h>
00002 
00003 //Observed average speeds for each duty cycle
00004 const float TUNING_CONSTANT_20 = 3.00;
00005 const float TUNING_CONSTANT_30 = 4.30;
00006 const float TUNING_CONSTANT_50 = 6.880;
00007 const float PI = 3.14159;
00008 const float WHEEL_CIRCUMFERENCE = .05*PI;
00009 
00010 //Velocity Control Tuning Constants
00011 const float TUNE_THRESH = 0.5f;
00012 const float TUNE_AMT = 0.1f;
00013 
00014 
00015 Encoder::Encoder(Timer t1){
00016     
00017     //set timer
00018     t = t1;
00019     //Initialize Intervals used during encoder data collection to measure velocity
00020     interval1=0;
00021     interval2=0;
00022     interval3=0;
00023     avg_interval=0;
00024     lastchange1 = 0;
00025     lastchange2 = 0;
00026     lastchange3 = 0;
00027     lastchange4 = 0;
00028     
00029     //Initialize Variables used to for velocity control
00030     avg_speed = 0;
00031     stall_check = 0;
00032     tuning_val = 1;
00033     
00034     // Servo parameters
00035     lastTurnTime = 0.0f;
00036     servoLeft = true;
00037     
00038     //Parameters specifying sample sizes and delays for small and large average speed samples
00039     num_samples_small = 10.0f;
00040     delay_small = 0.05f;
00041     num_samples_large = 100.0f;
00042     delay_large = 0.1f;
00043     
00044 }
00045 
00046 float Encoder::get_speed(){
00047     float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
00048     float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
00049     return linearSpeed;
00050 }
00051 
00052 
00053 float Encoder::get_avg_speed(float num_samples, float delay) {
00054     
00055     float avg_avg_speed = 0;
00056     
00057     for (int c = 0; c < num_samples; c++) {
00058         if (num_samples == num_samples_small){
00059             small_avg_speed_list[c] = get_speed();
00060         } else if (num_samples == num_samples_large){
00061             large_avg_speed_list[c] = get_speed();
00062             //pc.printf("\n\rworking: %f", large_avg_speed_list[c]);
00063         }
00064         wait(delay);
00065         }
00066                 
00067         for (int c = 1; c < num_samples; c++) {
00068             if (num_samples == num_samples_small){
00069                 avg_avg_speed += small_avg_speed_list[c];
00070             } else if (num_samples == num_samples_large){
00071                 avg_avg_speed += large_avg_speed_list[c];
00072                 //pc.printf("\n\rworking: %f", large_avg_speed_list[c]);
00073             }
00074         }
00075     return avg_avg_speed/num_samples;
00076 }
00077 
00078 float Encoder::velocity_control(float duty_cyc, float tuning_const) {
00079     
00080     avg_speed = get_avg_speed(num_samples_small, delay_small);
00081     
00082     if (avg_speed == stall_check) {
00083         avg_speed = 0;
00084         tuning_val += TUNE_AMT;
00085     } else if((avg_speed -  tuning_const) > TUNE_THRESH){
00086         tuning_val -= TUNE_AMT;
00087         stall_check = avg_speed;
00088     } else if (avg_speed - tuning_const < -1*TUNE_THRESH){
00089         tuning_val += TUNE_AMT;
00090         stall_check = avg_speed;
00091     } else {
00092         tuning_val = 1;
00093         stall_check = avg_speed;
00094     }
00095     return .0025 * duty_cyc * tuning_val;
00096     
00097 }
00098 void Encoder::fallInterrupt(){
00099     
00100     int time = t.read_us();
00101     interval1 = time - lastchange2;
00102     interval2 = lastchange1-lastchange3;
00103     interval3 = lastchange2 - lastchange4;
00104     avg_interval = (interval1 + interval2 + interval3)/3;
00105     
00106     lastchange4 = lastchange3;
00107     lastchange3 = lastchange2;
00108     lastchange2 = lastchange1;
00109     lastchange1 = time;
00110     //pc.printf("dark to light time : %d\n\r", interval);
00111     //pc.printf("fall");
00112 }
00113 void Encoder::riseInterrupt(){
00114     int time = t.read_us();
00115     interval1 = time - lastchange2;
00116     interval2 = lastchange1-lastchange3;
00117     interval3 = lastchange2 - lastchange4;
00118     avg_interval = (interval1 + interval2 + interval3)/3;
00119     
00120     lastchange4 = lastchange3;
00121     lastchange3 = lastchange2;
00122     lastchange2 = lastchange1;
00123     lastchange1 = time;
00124     //pc.printf("light to dark time : %d\n\r", interval);
00125     //pc.printf("rise");
00126 }
00127