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