E=MC / Mbed 2 deprecated figure_eight

Code for the car to drive in a figure eight motion

Dependencies:   mbed-rtos mbed MODSERIAL mbed-dsp telemetry

encoder.cpp@23:d8cacd996cce, 2015-03-20 (annotated)

Committer:
cheryl_he
Date:
Fri Mar 20 09:49:57 2015 +0000
Revision:
23:d8cacd996cce
Parent:
13:97708869a4ba 
telemetry stuff is compiling, needs to be debugged

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ericoneill 5:764c2ffb523a 1 #include <encoder.h>
ericoneill 9:d3909d9325e4 2
ericoneill 9:d3909d9325e4 3 //Observed average speeds for each duty cycle
ericoneill 9:d3909d9325e4 4 const float TUNING_CONSTANT_20 = 3.00;
ericoneill 9:d3909d9325e4 5 const float TUNING_CONSTANT_30 = 4.30;
ericoneill 9:d3909d9325e4 6 const float TUNING_CONSTANT_50 = 6.880;
ericoneill 9:d3909d9325e4 7 const float PI = 3.14159;
ericoneill 9:d3909d9325e4 8 const float WHEEL_CIRCUMFERENCE = .05*PI;
ericoneill 9:d3909d9325e4 9
ericoneill 9:d3909d9325e4 10 //Velocity Control Tuning Constants
ericoneill 9:d3909d9325e4 11 const float TUNE_THRESH = 0.5f;
ericoneill 9:d3909d9325e4 12 const float TUNE_AMT = 0.1f;
ericoneill 9:d3909d9325e4 13
ericoneill 9:d3909d9325e4 14
ericoneill 13:97708869a4ba 15 Encoder::Encoder(Timer t1){
ericoneill 11:f8aa39c19477 16
ericoneill 13:97708869a4ba 17 //set timer
ericoneill 13:97708869a4ba 18 t = t1;
ericoneill 11:f8aa39c19477 19 //Initialize Intervals used during encoder data collection to measure velocity
ericoneill 11:f8aa39c19477 20 interval1=0;
ericoneill 11:f8aa39c19477 21 interval2=0;
ericoneill 11:f8aa39c19477 22 interval3=0;
ericoneill 11:f8aa39c19477 23 avg_interval=0;
ericoneill 11:f8aa39c19477 24 lastchange1 = 0;
ericoneill 11:f8aa39c19477 25 lastchange2 = 0;
ericoneill 11:f8aa39c19477 26 lastchange3 = 0;
ericoneill 11:f8aa39c19477 27 lastchange4 = 0;
ericoneill 11:f8aa39c19477 28
ericoneill 11:f8aa39c19477 29 //Initialize Variables used to for velocity control
ericoneill 11:f8aa39c19477 30 avg_speed = 0;
ericoneill 11:f8aa39c19477 31 stall_check = 0;
ericoneill 11:f8aa39c19477 32 tuning_val = 1;
ericoneill 11:f8aa39c19477 33
ericoneill 11:f8aa39c19477 34 // Servo parameters
ericoneill 11:f8aa39c19477 35 lastTurnTime = 0.0f;
ericoneill 11:f8aa39c19477 36 servoLeft = true;
ericoneill 11:f8aa39c19477 37
ericoneill 11:f8aa39c19477 38 //Parameters specifying sample sizes and delays for small and large average speed samples
ericoneill 11:f8aa39c19477 39 num_samples_small = 10.0f;
ericoneill 11:f8aa39c19477 40 delay_small = 0.05f;
ericoneill 11:f8aa39c19477 41 num_samples_large = 100.0f;
ericoneill 11:f8aa39c19477 42 delay_large = 0.1f;
ericoneill 11:f8aa39c19477 43
ericoneill 11:f8aa39c19477 44 }
ericoneill 9:d3909d9325e4 45
ericoneill 9:d3909d9325e4 46 float Encoder::get_speed(){
ericoneill 9:d3909d9325e4 47 float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
ericoneill 9:d3909d9325e4 48 float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
ericoneill 9:d3909d9325e4 49 return linearSpeed;
ericoneill 11:f8aa39c19477 50 }
ericoneill 11:f8aa39c19477 51
ericoneill 11:f8aa39c19477 52
ericoneill 11:f8aa39c19477 53 float Encoder::get_avg_speed(float num_samples, float delay) {
ericoneill 11:f8aa39c19477 54
ericoneill 11:f8aa39c19477 55 float avg_avg_speed = 0;
ericoneill 11:f8aa39c19477 56
ericoneill 11:f8aa39c19477 57 for (int c = 0; c < num_samples; c++) {
ericoneill 11:f8aa39c19477 58 if (num_samples == num_samples_small){
ericoneill 11:f8aa39c19477 59 small_avg_speed_list[c] = get_speed();
ericoneill 11:f8aa39c19477 60 } else if (num_samples == num_samples_large){
ericoneill 11:f8aa39c19477 61 large_avg_speed_list[c] = get_speed();
ericoneill 11:f8aa39c19477 62 //pc.printf("\n\rworking: %f", large_avg_speed_list[c]);
ericoneill 11:f8aa39c19477 63 }
ericoneill 11:f8aa39c19477 64 wait(delay);
ericoneill 11:f8aa39c19477 65 }
ericoneill 11:f8aa39c19477 66
ericoneill 11:f8aa39c19477 67 for (int c = 1; c < num_samples; c++) {
ericoneill 11:f8aa39c19477 68 if (num_samples == num_samples_small){
ericoneill 11:f8aa39c19477 69 avg_avg_speed += small_avg_speed_list[c];
ericoneill 11:f8aa39c19477 70 } else if (num_samples == num_samples_large){
ericoneill 11:f8aa39c19477 71 avg_avg_speed += large_avg_speed_list[c];
ericoneill 11:f8aa39c19477 72 //pc.printf("\n\rworking: %f", large_avg_speed_list[c]);
ericoneill 11:f8aa39c19477 73 }
ericoneill 11:f8aa39c19477 74 }
ericoneill 11:f8aa39c19477 75 return avg_avg_speed/num_samples;
ericoneill 11:f8aa39c19477 76 }
ericoneill 11:f8aa39c19477 77
ericoneill 13:97708869a4ba 78 float Encoder::velocity_control(float duty_cyc, float tuning_const) {
ericoneill 11:f8aa39c19477 79
ericoneill 11:f8aa39c19477 80 avg_speed = get_avg_speed(num_samples_small, delay_small);
ericoneill 11:f8aa39c19477 81
ericoneill 11:f8aa39c19477 82 if (avg_speed == stall_check) {
ericoneill 11:f8aa39c19477 83 avg_speed = 0;
ericoneill 11:f8aa39c19477 84 tuning_val += TUNE_AMT;
ericoneill 11:f8aa39c19477 85 } else if((avg_speed - tuning_const) > TUNE_THRESH){
ericoneill 11:f8aa39c19477 86 tuning_val -= TUNE_AMT;
ericoneill 11:f8aa39c19477 87 stall_check = avg_speed;
ericoneill 11:f8aa39c19477 88 } else if (avg_speed - tuning_const < -1*TUNE_THRESH){
ericoneill 11:f8aa39c19477 89 tuning_val += TUNE_AMT;
ericoneill 11:f8aa39c19477 90 stall_check = avg_speed;
ericoneill 11:f8aa39c19477 91 } else {
ericoneill 11:f8aa39c19477 92 tuning_val = 1;
ericoneill 11:f8aa39c19477 93 stall_check = avg_speed;
ericoneill 11:f8aa39c19477 94 }
ericoneill 13:97708869a4ba 95 return .0025 * duty_cyc * tuning_val;
ericoneill 11:f8aa39c19477 96
ericoneill 11:f8aa39c19477 97 }
ericoneill 11:f8aa39c19477 98 void Encoder::fallInterrupt(){
ericoneill 11:f8aa39c19477 99
ericoneill 11:f8aa39c19477 100 int time = t.read_us();
ericoneill 11:f8aa39c19477 101 interval1 = time - lastchange2;
ericoneill 11:f8aa39c19477 102 interval2 = lastchange1-lastchange3;
ericoneill 11:f8aa39c19477 103 interval3 = lastchange2 - lastchange4;
ericoneill 11:f8aa39c19477 104 avg_interval = (interval1 + interval2 + interval3)/3;
ericoneill 11:f8aa39c19477 105
ericoneill 11:f8aa39c19477 106 lastchange4 = lastchange3;
ericoneill 11:f8aa39c19477 107 lastchange3 = lastchange2;
ericoneill 11:f8aa39c19477 108 lastchange2 = lastchange1;
ericoneill 11:f8aa39c19477 109 lastchange1 = time;
ericoneill 11:f8aa39c19477 110 //pc.printf("dark to light time : %d\n\r", interval);
ericoneill 11:f8aa39c19477 111 //pc.printf("fall");
ericoneill 11:f8aa39c19477 112 }
ericoneill 11:f8aa39c19477 113 void Encoder::riseInterrupt(){
ericoneill 11:f8aa39c19477 114 int time = t.read_us();
ericoneill 11:f8aa39c19477 115 interval1 = time - lastchange2;
ericoneill 11:f8aa39c19477 116 interval2 = lastchange1-lastchange3;
ericoneill 11:f8aa39c19477 117 interval3 = lastchange2 - lastchange4;
ericoneill 11:f8aa39c19477 118 avg_interval = (interval1 + interval2 + interval3)/3;
ericoneill 11:f8aa39c19477 119
ericoneill 11:f8aa39c19477 120 lastchange4 = lastchange3;
ericoneill 11:f8aa39c19477 121 lastchange3 = lastchange2;
ericoneill 11:f8aa39c19477 122 lastchange2 = lastchange1;
ericoneill 11:f8aa39c19477 123 lastchange1 = time;
ericoneill 11:f8aa39c19477 124 //pc.printf("light to dark time : %d\n\r", interval);
ericoneill 11:f8aa39c19477 125 //pc.printf("rise");
ericoneill 11:f8aa39c19477 126 }
ericoneill 11:f8aa39c19477 127