E=MC
/
e=mc
.
main.cpp@7:ea395616348c, 2015-03-02 (annotated)
- Committer:
- mawk2311
- Date:
- Mon Mar 02 22:11:35 2015 +0000
- Revision:
- 7:ea395616348c
- Parent:
- 6:78a2c2a7f39e
- Child:
- 8:f3e0b4814888
- Child:
- 9:aecffea74d88
Cleaned up the code and added "case a", which allows us to get the average value of 20, 30, and 50 percent duty cycles.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ericoneill | 0:d328ecb3fbb1 | 1 | #include "mbed.h" |
ericoneill | 0:d328ecb3fbb1 | 2 | |
ericoneill | 0:d328ecb3fbb1 | 3 | DigitalOut myled(LED1); |
ericoneill | 0:d328ecb3fbb1 | 4 | PwmOut servo(PTA5); |
ericoneill | 0:d328ecb3fbb1 | 5 | PwmOut motor(PTA4); |
ericoneill | 0:d328ecb3fbb1 | 6 | Serial pc(USBTX, USBRX); // tx, rx |
ericoneill | 3:7eaf505f811e | 7 | |
ericoneill | 3:7eaf505f811e | 8 | // encoder setup and variables |
ericoneill | 3:7eaf505f811e | 9 | InterruptIn interrupt(PTA13); |
ericoneill | 4:263bddc51c0f | 10 | |
mawk2311 | 7:ea395616348c | 11 | //Intervals used during encoder data collection to measure velocity |
ericoneill | 4:263bddc51c0f | 12 | int interval1=0; |
ericoneill | 4:263bddc51c0f | 13 | int interval2=0; |
ericoneill | 4:263bddc51c0f | 14 | int interval3=0; |
ericoneill | 4:263bddc51c0f | 15 | int avg_interval=0; |
ericoneill | 4:263bddc51c0f | 16 | int lastchange1 = 0; |
ericoneill | 4:263bddc51c0f | 17 | int lastchange2 = 0; |
ericoneill | 4:263bddc51c0f | 18 | int lastchange3 = 0; |
ericoneill | 4:263bddc51c0f | 19 | int lastchange4 = 0; |
mawk2311 | 5:61a0a21134f7 | 20 | |
mawk2311 | 7:ea395616348c | 21 | //Variables used to for velocity control |
mawk2311 | 7:ea395616348c | 22 | float avg_speed = 0; |
mawk2311 | 7:ea395616348c | 23 | float stall_check = 0; |
mawk2311 | 7:ea395616348c | 24 | float tuning_val = 1; |
mawk2311 | 5:61a0a21134f7 | 25 | |
ericoneill | 1:8e5821dec0f7 | 26 | Timer t; |
mawk2311 | 7:ea395616348c | 27 | |
mawk2311 | 7:ea395616348c | 28 | //Observed average speeds for each duty cycle |
mawk2311 | 7:ea395616348c | 29 | const float TUNING_CONSTANT_20 = 3.00; |
mawk2311 | 7:ea395616348c | 30 | const float TUNING_CONSTANT_30 = 4.30; |
mawk2311 | 5:61a0a21134f7 | 31 | const float TUNING_CONSTANT_50 = 6.880; |
ericoneill | 4:263bddc51c0f | 32 | const float PI = 3.14159; |
ericoneill | 4:263bddc51c0f | 33 | const float WHEEL_CIRCUMFERENCE = .05*PI; |
ericoneill | 0:d328ecb3fbb1 | 34 | |
mawk2311 | 7:ea395616348c | 35 | //Velocity Control Tuning Constants |
mawk2311 | 7:ea395616348c | 36 | const float TUNE_THRESH = 0.5f; |
mawk2311 | 7:ea395616348c | 37 | const float TUNE_AMT = 0.1f; |
mawk2311 | 7:ea395616348c | 38 | |
mawk2311 | 7:ea395616348c | 39 | //Parameters specifying sample sizes and delays for small and large average speed samples |
mawk2311 | 7:ea395616348c | 40 | float num_samples_small = 10.0f; |
mawk2311 | 7:ea395616348c | 41 | float delay_small = 0.05f; |
mawk2311 | 7:ea395616348c | 42 | float num_samples_large = 100.0f; |
mawk2311 | 7:ea395616348c | 43 | float delay_large = 0.1f; |
mawk2311 | 7:ea395616348c | 44 | |
mawk2311 | 7:ea395616348c | 45 | // Large and small arrays used to get average velocity values |
mawk2311 | 7:ea395616348c | 46 | float large_avg_speed_list [100]; |
mawk2311 | 7:ea395616348c | 47 | float small_avg_speed_list [10]; |
mawk2311 | 7:ea395616348c | 48 | |
mawk2311 | 7:ea395616348c | 49 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
ericoneill | 4:263bddc51c0f | 50 | float get_speed(){ |
ericoneill | 4:263bddc51c0f | 51 | float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f; |
ericoneill | 4:263bddc51c0f | 52 | float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE; |
ericoneill | 4:263bddc51c0f | 53 | return linearSpeed; |
ericoneill | 4:263bddc51c0f | 54 | } |
mawk2311 | 5:61a0a21134f7 | 55 | |
mawk2311 | 7:ea395616348c | 56 | float get_avg_speed(float num_samples, float delay) { |
mawk2311 | 5:61a0a21134f7 | 57 | |
mawk2311 | 5:61a0a21134f7 | 58 | float avg_avg_speed = 0; |
mawk2311 | 5:61a0a21134f7 | 59 | |
mawk2311 | 7:ea395616348c | 60 | for (int c = 0; c < num_samples; c++) { |
mawk2311 | 7:ea395616348c | 61 | if (num_samples == num_samples_small){ |
mawk2311 | 7:ea395616348c | 62 | small_avg_speed_list[c] = get_speed(); |
mawk2311 | 7:ea395616348c | 63 | } else if (num_samples == num_samples_large){ |
mawk2311 | 7:ea395616348c | 64 | large_avg_speed_list[c] = get_speed(); |
mawk2311 | 7:ea395616348c | 65 | //pc.printf("\n\rworking: %f", large_avg_speed_list[c]); |
mawk2311 | 7:ea395616348c | 66 | } |
mawk2311 | 7:ea395616348c | 67 | wait(delay); |
mawk2311 | 5:61a0a21134f7 | 68 | } |
mawk2311 | 5:61a0a21134f7 | 69 | |
mawk2311 | 7:ea395616348c | 70 | for (int c = 1; c < num_samples; c++) { |
mawk2311 | 7:ea395616348c | 71 | if (num_samples == num_samples_small){ |
mawk2311 | 7:ea395616348c | 72 | avg_avg_speed += small_avg_speed_list[c]; |
mawk2311 | 7:ea395616348c | 73 | } else if (num_samples == num_samples_large){ |
mawk2311 | 7:ea395616348c | 74 | avg_avg_speed += large_avg_speed_list[c]; |
mawk2311 | 7:ea395616348c | 75 | //pc.printf("\n\rworking: %f", large_avg_speed_list[c]); |
mawk2311 | 7:ea395616348c | 76 | } |
mawk2311 | 5:61a0a21134f7 | 77 | } |
mawk2311 | 7:ea395616348c | 78 | return avg_avg_speed/num_samples; |
mawk2311 | 5:61a0a21134f7 | 79 | } |
mawk2311 | 5:61a0a21134f7 | 80 | |
mawk2311 | 7:ea395616348c | 81 | void velocity_control(float duty_cyc, float tuning_const) { |
mawk2311 | 7:ea395616348c | 82 | |
mawk2311 | 7:ea395616348c | 83 | avg_speed = get_avg_speed(num_samples_small, delay_small); |
mawk2311 | 7:ea395616348c | 84 | |
mawk2311 | 7:ea395616348c | 85 | if (avg_speed == stall_check) { |
mawk2311 | 7:ea395616348c | 86 | avg_speed = 0; |
mawk2311 | 7:ea395616348c | 87 | tuning_val += TUNE_AMT; |
mawk2311 | 7:ea395616348c | 88 | } else if((avg_speed - tuning_const) > TUNE_THRESH){ |
mawk2311 | 7:ea395616348c | 89 | tuning_val -= TUNE_AMT; |
mawk2311 | 7:ea395616348c | 90 | stall_check = avg_speed; |
mawk2311 | 7:ea395616348c | 91 | } else if (avg_speed - tuning_const < -1*TUNE_THRESH){ |
mawk2311 | 7:ea395616348c | 92 | tuning_val += TUNE_AMT; |
mawk2311 | 7:ea395616348c | 93 | stall_check = avg_speed; |
mawk2311 | 7:ea395616348c | 94 | } else { |
mawk2311 | 7:ea395616348c | 95 | tuning_val = 1; |
mawk2311 | 7:ea395616348c | 96 | stall_check = avg_speed; |
mawk2311 | 7:ea395616348c | 97 | } |
mawk2311 | 7:ea395616348c | 98 | motor.pulsewidth(.0025 * duty_cyc * tuning_val); |
mawk2311 | 7:ea395616348c | 99 | |
mawk2311 | 7:ea395616348c | 100 | pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val); |
mawk2311 | 7:ea395616348c | 101 | wait(.2); |
mawk2311 | 7:ea395616348c | 102 | } |
mawk2311 | 7:ea395616348c | 103 | |
ericoneill | 0:d328ecb3fbb1 | 104 | void servo_sweep(){ |
ericoneill | 0:d328ecb3fbb1 | 105 | for(float p = 0.001; p<0.002; p+=0.0001){ |
ericoneill | 0:d328ecb3fbb1 | 106 | servo.pulsewidth(p); |
ericoneill | 0:d328ecb3fbb1 | 107 | wait(0.5); |
ericoneill | 0:d328ecb3fbb1 | 108 | } |
ericoneill | 0:d328ecb3fbb1 | 109 | } |
mawk2311 | 7:ea395616348c | 110 | |
ericoneill | 3:7eaf505f811e | 111 | void fallInterrupt(){ |
ericoneill | 4:263bddc51c0f | 112 | |
ericoneill | 4:263bddc51c0f | 113 | int time = t.read_us(); |
ericoneill | 4:263bddc51c0f | 114 | interval1 = time - lastchange2; |
ericoneill | 4:263bddc51c0f | 115 | interval2 = lastchange1-lastchange3; |
ericoneill | 4:263bddc51c0f | 116 | interval3 = lastchange2 - lastchange4; |
ericoneill | 4:263bddc51c0f | 117 | avg_interval = (interval1 + interval2 + interval3)/3; |
ericoneill | 4:263bddc51c0f | 118 | |
ericoneill | 4:263bddc51c0f | 119 | lastchange4 = lastchange3; |
ericoneill | 4:263bddc51c0f | 120 | lastchange3 = lastchange2; |
ericoneill | 4:263bddc51c0f | 121 | lastchange2 = lastchange1; |
ericoneill | 4:263bddc51c0f | 122 | lastchange1 = time; |
ericoneill | 4:263bddc51c0f | 123 | //pc.printf("dark to light time : %d\n\r", interval); |
ericoneill | 4:263bddc51c0f | 124 | //pc.printf("fall"); |
ericoneill | 3:7eaf505f811e | 125 | } |
ericoneill | 3:7eaf505f811e | 126 | void riseInterrupt(){ |
ericoneill | 4:263bddc51c0f | 127 | int time = t.read_us(); |
ericoneill | 4:263bddc51c0f | 128 | interval1 = time - lastchange2; |
ericoneill | 4:263bddc51c0f | 129 | interval2 = lastchange1-lastchange3; |
ericoneill | 4:263bddc51c0f | 130 | interval3 = lastchange2 - lastchange4; |
ericoneill | 4:263bddc51c0f | 131 | avg_interval = (interval1 + interval2 + interval3)/3; |
ericoneill | 4:263bddc51c0f | 132 | |
ericoneill | 4:263bddc51c0f | 133 | lastchange4 = lastchange3; |
ericoneill | 4:263bddc51c0f | 134 | lastchange3 = lastchange2; |
ericoneill | 4:263bddc51c0f | 135 | lastchange2 = lastchange1; |
ericoneill | 4:263bddc51c0f | 136 | lastchange1 = time; |
ericoneill | 4:263bddc51c0f | 137 | //pc.printf("light to dark time : %d\n\r", interval); |
ericoneill | 4:263bddc51c0f | 138 | //pc.printf("rise"); |
ericoneill | 3:7eaf505f811e | 139 | } |
ericoneill | 3:7eaf505f811e | 140 | |
ericoneill | 0:d328ecb3fbb1 | 141 | int main() { |
ericoneill | 0:d328ecb3fbb1 | 142 | servo.period(0.005); |
ericoneill | 0:d328ecb3fbb1 | 143 | motor.period(.0025); |
ericoneill | 3:7eaf505f811e | 144 | interrupt.fall(&fallInterrupt); |
ericoneill | 3:7eaf505f811e | 145 | interrupt.rise(&riseInterrupt); |
ericoneill | 3:7eaf505f811e | 146 | |
ericoneill | 1:8e5821dec0f7 | 147 | t.start(); |
ericoneill | 4:263bddc51c0f | 148 | while(1){ |
ericoneill | 2:30ebae0d3e17 | 149 | |
ericoneill | 2:30ebae0d3e17 | 150 | char choice = pc.getc(); |
mawk2311 | 5:61a0a21134f7 | 151 | pc.putc(choice); |
mawk2311 | 5:61a0a21134f7 | 152 | |
ericoneill | 2:30ebae0d3e17 | 153 | switch(choice){ |
ericoneill | 0:d328ecb3fbb1 | 154 | case '0': |
ericoneill | 0:d328ecb3fbb1 | 155 | motor.pulsewidth(0.0); |
ericoneill | 2:30ebae0d3e17 | 156 | pc.printf("0% \n\r"); |
ericoneill | 4:263bddc51c0f | 157 | |
ericoneill | 0:d328ecb3fbb1 | 158 | break; |
ericoneill | 0:d328ecb3fbb1 | 159 | case '1': |
ericoneill | 0:d328ecb3fbb1 | 160 | motor.pulsewidth(.0025); |
ericoneill | 2:30ebae0d3e17 | 161 | pc.printf("100% \n\r"); |
ericoneill | 4:263bddc51c0f | 162 | wait(.5); |
mawk2311 | 7:ea395616348c | 163 | pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small)); |
mawk2311 | 5:61a0a21134f7 | 164 | |
ericoneill | 4:263bddc51c0f | 165 | break; |
ericoneill | 4:263bddc51c0f | 166 | case '2': |
ericoneill | 4:263bddc51c0f | 167 | motor.pulsewidth(.0025*.2); |
mawk2311 | 7:ea395616348c | 168 | pc.printf("\n\r20% \n\r"); |
mawk2311 | 5:61a0a21134f7 | 169 | wait(.5); |
mawk2311 | 7:ea395616348c | 170 | pc.printf("speed: %f\n\rtuning val: %f\n\r", get_avg_speed(num_samples_small, delay_small)); |
mawk2311 | 5:61a0a21134f7 | 171 | |
mawk2311 | 5:61a0a21134f7 | 172 | while(1){ |
mawk2311 | 7:ea395616348c | 173 | velocity_control(0.2f, TUNING_CONSTANT_20); |
mawk2311 | 5:61a0a21134f7 | 174 | } |
mawk2311 | 5:61a0a21134f7 | 175 | |
mawk2311 | 7:ea395616348c | 176 | //break; |
ericoneill | 0:d328ecb3fbb1 | 177 | case '3': |
ericoneill | 0:d328ecb3fbb1 | 178 | motor.pulsewidth(.0025*.3); |
mawk2311 | 7:ea395616348c | 179 | pc.printf("\n\r30% \n\r"); |
ericoneill | 4:263bddc51c0f | 180 | wait(.5); |
mawk2311 | 7:ea395616348c | 181 | pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small)); |
mawk2311 | 5:61a0a21134f7 | 182 | |
mawk2311 | 5:61a0a21134f7 | 183 | while(1){ |
mawk2311 | 7:ea395616348c | 184 | velocity_control(0.3f, TUNING_CONSTANT_30); |
mawk2311 | 5:61a0a21134f7 | 185 | } |
mawk2311 | 5:61a0a21134f7 | 186 | |
mawk2311 | 5:61a0a21134f7 | 187 | //break; |
ericoneill | 0:d328ecb3fbb1 | 188 | case '5': |
ericoneill | 0:d328ecb3fbb1 | 189 | motor.pulsewidth(.0025*.5); |
mawk2311 | 7:ea395616348c | 190 | pc.printf("\n\r50% \n\r"); |
ericoneill | 4:263bddc51c0f | 191 | wait(.5); |
mawk2311 | 7:ea395616348c | 192 | pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small)); |
mawk2311 | 5:61a0a21134f7 | 193 | |
mawk2311 | 5:61a0a21134f7 | 194 | while(1){ |
mawk2311 | 7:ea395616348c | 195 | velocity_control(0.5f, TUNING_CONSTANT_50); |
mawk2311 | 5:61a0a21134f7 | 196 | } |
mawk2311 | 5:61a0a21134f7 | 197 | |
mawk2311 | 5:61a0a21134f7 | 198 | //break; |
mawk2311 | 7:ea395616348c | 199 | case 'a': |
mawk2311 | 7:ea395616348c | 200 | pc.printf("\n\rGet average velocity of which duty cycle?\n\r"); |
mawk2311 | 7:ea395616348c | 201 | choice = pc.getc(); |
mawk2311 | 7:ea395616348c | 202 | pc.putc(choice); |
mawk2311 | 7:ea395616348c | 203 | |
mawk2311 | 7:ea395616348c | 204 | switch(choice){ |
mawk2311 | 7:ea395616348c | 205 | |
mawk2311 | 7:ea395616348c | 206 | case '2': |
mawk2311 | 7:ea395616348c | 207 | motor.pulsewidth(.0025*0.2f); |
mawk2311 | 7:ea395616348c | 208 | pc.printf("\n\rLongterm average speed at 20 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large)); |
mawk2311 | 7:ea395616348c | 209 | break; |
mawk2311 | 7:ea395616348c | 210 | |
mawk2311 | 7:ea395616348c | 211 | case '3': |
mawk2311 | 7:ea395616348c | 212 | motor.pulsewidth(.0025*0.3f); |
mawk2311 | 7:ea395616348c | 213 | pc.printf("\n\rLongterm average speed at 30 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large)); |
mawk2311 | 7:ea395616348c | 214 | break; |
mawk2311 | 7:ea395616348c | 215 | |
mawk2311 | 7:ea395616348c | 216 | case '5': |
mawk2311 | 7:ea395616348c | 217 | motor.pulsewidth(.0025*0.5f); |
mawk2311 | 7:ea395616348c | 218 | pc.printf("\n\rLongterm average speed at 50 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large)); |
mawk2311 | 7:ea395616348c | 219 | break; |
mawk2311 | 7:ea395616348c | 220 | |
mawk2311 | 7:ea395616348c | 221 | default: |
mawk2311 | 7:ea395616348c | 222 | break; |
mawk2311 | 7:ea395616348c | 223 | } |
mawk2311 | 7:ea395616348c | 224 | break; |
mawk2311 | 7:ea395616348c | 225 | |
ericoneill | 0:d328ecb3fbb1 | 226 | default: |
mawk2311 | 7:ea395616348c | 227 | motor.pulsewidth(.0025*0); |
ericoneill | 2:30ebae0d3e17 | 228 | pc.printf("default\n\r"); |
ericoneill | 0:d328ecb3fbb1 | 229 | break; |
ericoneill | 0:d328ecb3fbb1 | 230 | } |
ericoneill | 3:7eaf505f811e | 231 | |
ericoneill | 0:d328ecb3fbb1 | 232 | //servo_sweep(); |
ericoneill | 0:d328ecb3fbb1 | 233 | } |
ericoneill | 0:d328ecb3fbb1 | 234 | } |