E=MC
/
e=mc
.
main.cpp@6:78a2c2a7f39e, 2015-03-02 (annotated)
- Committer:
- ericoneill
- Date:
- Mon Mar 02 19:38:38 2015 +0000
- Revision:
- 6:78a2c2a7f39e
- Parent:
- 5:61a0a21134f7
- Child:
- 7:ea395616348c
Stall checking
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 | |
ericoneill | 4:263bddc51c0f | 11 | int interval1=0; |
ericoneill | 4:263bddc51c0f | 12 | int interval2=0; |
ericoneill | 4:263bddc51c0f | 13 | int interval3=0; |
ericoneill | 4:263bddc51c0f | 14 | int avg_interval=0; |
ericoneill | 4:263bddc51c0f | 15 | int lastchange1 = 0; |
ericoneill | 4:263bddc51c0f | 16 | int lastchange2 = 0; |
ericoneill | 4:263bddc51c0f | 17 | int lastchange3 = 0; |
ericoneill | 4:263bddc51c0f | 18 | int lastchange4 = 0; |
mawk2311 | 5:61a0a21134f7 | 19 | |
mawk2311 | 5:61a0a21134f7 | 20 | float avg_speed_list [100]; |
mawk2311 | 5:61a0a21134f7 | 21 | float small_avg_speed_list [10]; |
mawk2311 | 5:61a0a21134f7 | 22 | |
ericoneill | 1:8e5821dec0f7 | 23 | Timer t; |
ericoneill | 4:263bddc51c0f | 24 | //constant *(PWM-expected) |
mawk2311 | 5:61a0a21134f7 | 25 | const float TUNING_CONSTANT_20 = 3.697; |
mawk2311 | 5:61a0a21134f7 | 26 | const float TUNING_CONSTANT_30 = 5.607; |
mawk2311 | 5:61a0a21134f7 | 27 | const float TUNING_CONSTANT_50 = 6.880; |
ericoneill | 4:263bddc51c0f | 28 | const float PI = 3.14159; |
ericoneill | 4:263bddc51c0f | 29 | const float WHEEL_CIRCUMFERENCE = .05*PI; |
ericoneill | 0:d328ecb3fbb1 | 30 | |
ericoneill | 4:263bddc51c0f | 31 | float get_speed(){ |
ericoneill | 4:263bddc51c0f | 32 | float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f; |
ericoneill | 4:263bddc51c0f | 33 | float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE; |
ericoneill | 4:263bddc51c0f | 34 | return linearSpeed; |
ericoneill | 4:263bddc51c0f | 35 | |
ericoneill | 4:263bddc51c0f | 36 | } |
mawk2311 | 5:61a0a21134f7 | 37 | |
mawk2311 | 5:61a0a21134f7 | 38 | float get_avg_speed() { |
mawk2311 | 5:61a0a21134f7 | 39 | |
mawk2311 | 5:61a0a21134f7 | 40 | float avg_avg_speed = 0; |
mawk2311 | 5:61a0a21134f7 | 41 | |
mawk2311 | 5:61a0a21134f7 | 42 | for (int c = 0; c < 10; c++) { |
mawk2311 | 5:61a0a21134f7 | 43 | small_avg_speed_list[c] = get_speed(); |
mawk2311 | 5:61a0a21134f7 | 44 | wait(.05); |
mawk2311 | 5:61a0a21134f7 | 45 | } |
mawk2311 | 5:61a0a21134f7 | 46 | |
mawk2311 | 5:61a0a21134f7 | 47 | for (int c = 0; c < 10; c++) { |
mawk2311 | 5:61a0a21134f7 | 48 | avg_avg_speed += small_avg_speed_list[c]; |
mawk2311 | 5:61a0a21134f7 | 49 | } |
mawk2311 | 5:61a0a21134f7 | 50 | |
mawk2311 | 5:61a0a21134f7 | 51 | return avg_avg_speed/10.0f; |
mawk2311 | 5:61a0a21134f7 | 52 | } |
mawk2311 | 5:61a0a21134f7 | 53 | |
ericoneill | 0:d328ecb3fbb1 | 54 | void servo_sweep(){ |
ericoneill | 0:d328ecb3fbb1 | 55 | for(float p = 0.001; p<0.002; p+=0.0001){ |
ericoneill | 0:d328ecb3fbb1 | 56 | servo.pulsewidth(p); |
ericoneill | 0:d328ecb3fbb1 | 57 | wait(0.5); |
ericoneill | 0:d328ecb3fbb1 | 58 | } |
ericoneill | 0:d328ecb3fbb1 | 59 | } |
ericoneill | 3:7eaf505f811e | 60 | void fallInterrupt(){ |
ericoneill | 4:263bddc51c0f | 61 | |
ericoneill | 4:263bddc51c0f | 62 | int time = t.read_us(); |
ericoneill | 4:263bddc51c0f | 63 | interval1 = time - lastchange2; |
ericoneill | 4:263bddc51c0f | 64 | interval2 = lastchange1-lastchange3; |
ericoneill | 4:263bddc51c0f | 65 | interval3 = lastchange2 - lastchange4; |
ericoneill | 4:263bddc51c0f | 66 | avg_interval = (interval1 + interval2 + interval3)/3; |
ericoneill | 4:263bddc51c0f | 67 | |
ericoneill | 4:263bddc51c0f | 68 | lastchange4 = lastchange3; |
ericoneill | 4:263bddc51c0f | 69 | lastchange3 = lastchange2; |
ericoneill | 4:263bddc51c0f | 70 | lastchange2 = lastchange1; |
ericoneill | 4:263bddc51c0f | 71 | lastchange1 = time; |
ericoneill | 4:263bddc51c0f | 72 | //pc.printf("dark to light time : %d\n\r", interval); |
ericoneill | 4:263bddc51c0f | 73 | //pc.printf("fall"); |
ericoneill | 3:7eaf505f811e | 74 | } |
ericoneill | 3:7eaf505f811e | 75 | void riseInterrupt(){ |
ericoneill | 4:263bddc51c0f | 76 | int time = t.read_us(); |
ericoneill | 4:263bddc51c0f | 77 | interval1 = time - lastchange2; |
ericoneill | 4:263bddc51c0f | 78 | interval2 = lastchange1-lastchange3; |
ericoneill | 4:263bddc51c0f | 79 | interval3 = lastchange2 - lastchange4; |
ericoneill | 4:263bddc51c0f | 80 | avg_interval = (interval1 + interval2 + interval3)/3; |
ericoneill | 4:263bddc51c0f | 81 | |
ericoneill | 4:263bddc51c0f | 82 | lastchange4 = lastchange3; |
ericoneill | 4:263bddc51c0f | 83 | lastchange3 = lastchange2; |
ericoneill | 4:263bddc51c0f | 84 | lastchange2 = lastchange1; |
ericoneill | 4:263bddc51c0f | 85 | lastchange1 = time; |
ericoneill | 4:263bddc51c0f | 86 | //pc.printf("light to dark time : %d\n\r", interval); |
ericoneill | 4:263bddc51c0f | 87 | //pc.printf("rise"); |
ericoneill | 3:7eaf505f811e | 88 | } |
ericoneill | 3:7eaf505f811e | 89 | |
ericoneill | 0:d328ecb3fbb1 | 90 | int main() { |
ericoneill | 0:d328ecb3fbb1 | 91 | servo.period(0.005); |
ericoneill | 0:d328ecb3fbb1 | 92 | motor.period(.0025); |
ericoneill | 3:7eaf505f811e | 93 | interrupt.fall(&fallInterrupt); |
ericoneill | 3:7eaf505f811e | 94 | interrupt.rise(&riseInterrupt); |
ericoneill | 3:7eaf505f811e | 95 | |
ericoneill | 1:8e5821dec0f7 | 96 | |
ericoneill | 1:8e5821dec0f7 | 97 | t.start(); |
ericoneill | 4:263bddc51c0f | 98 | while(1){ |
ericoneill | 2:30ebae0d3e17 | 99 | |
ericoneill | 6:78a2c2a7f39e | 100 | float avg_speed; |
ericoneill | 6:78a2c2a7f39e | 101 | float stall_check; |
mawk2311 | 5:61a0a21134f7 | 102 | |
ericoneill | 2:30ebae0d3e17 | 103 | char choice = pc.getc(); |
mawk2311 | 5:61a0a21134f7 | 104 | pc.putc(choice); |
mawk2311 | 5:61a0a21134f7 | 105 | |
ericoneill | 2:30ebae0d3e17 | 106 | switch(choice){ |
ericoneill | 0:d328ecb3fbb1 | 107 | case '0': |
ericoneill | 0:d328ecb3fbb1 | 108 | motor.pulsewidth(0.0); |
ericoneill | 2:30ebae0d3e17 | 109 | pc.printf("0% \n\r"); |
ericoneill | 4:263bddc51c0f | 110 | |
ericoneill | 0:d328ecb3fbb1 | 111 | break; |
ericoneill | 0:d328ecb3fbb1 | 112 | case '1': |
ericoneill | 0:d328ecb3fbb1 | 113 | motor.pulsewidth(.0025); |
ericoneill | 2:30ebae0d3e17 | 114 | pc.printf("100% \n\r"); |
ericoneill | 4:263bddc51c0f | 115 | wait(.5); |
ericoneill | 4:263bddc51c0f | 116 | pc.printf("speed: %f",get_speed()); |
mawk2311 | 5:61a0a21134f7 | 117 | |
ericoneill | 4:263bddc51c0f | 118 | break; |
ericoneill | 4:263bddc51c0f | 119 | case '2': |
ericoneill | 4:263bddc51c0f | 120 | motor.pulsewidth(.0025*.2); |
ericoneill | 4:263bddc51c0f | 121 | pc.printf("20% \n\r"); |
mawk2311 | 5:61a0a21134f7 | 122 | wait(.5); |
mawk2311 | 5:61a0a21134f7 | 123 | float tuning_val = 1; |
mawk2311 | 5:61a0a21134f7 | 124 | pc.printf("speed: %f\n\rtuning val: %f\n\r", get_avg_speed()); |
mawk2311 | 5:61a0a21134f7 | 125 | |
mawk2311 | 5:61a0a21134f7 | 126 | while(1){ |
mawk2311 | 5:61a0a21134f7 | 127 | |
mawk2311 | 5:61a0a21134f7 | 128 | avg_speed = get_avg_speed(); |
ericoneill | 6:78a2c2a7f39e | 129 | if (avg_speed == stall_check) { |
ericoneill | 6:78a2c2a7f39e | 130 | avg_speed = 0; |
ericoneill | 6:78a2c2a7f39e | 131 | tuning_val += .1; |
ericoneill | 6:78a2c2a7f39e | 132 | } else if((avg_speed - TUNING_CONSTANT_20) > 0.5f){ |
mawk2311 | 5:61a0a21134f7 | 133 | tuning_val -= .1; |
ericoneill | 6:78a2c2a7f39e | 134 | stall_check = avg_speed; |
mawk2311 | 5:61a0a21134f7 | 135 | } else if (avg_speed - TUNING_CONSTANT_20 < -0.5f){ |
mawk2311 | 5:61a0a21134f7 | 136 | tuning_val += .1; |
ericoneill | 6:78a2c2a7f39e | 137 | stall_check = avg_speed; |
mawk2311 | 5:61a0a21134f7 | 138 | } else { |
mawk2311 | 5:61a0a21134f7 | 139 | tuning_val = 1; |
ericoneill | 6:78a2c2a7f39e | 140 | stall_check = avg_speed; |
mawk2311 | 5:61a0a21134f7 | 141 | } |
mawk2311 | 5:61a0a21134f7 | 142 | motor.pulsewidth(.0025 * .2 * tuning_val); |
ericoneill | 6:78a2c2a7f39e | 143 | |
ericoneill | 6:78a2c2a7f39e | 144 | pc.printf("speed: %f\n\rtuning val: %f\n\r PWM : ", avg_speed, tuning_val); |
mawk2311 | 5:61a0a21134f7 | 145 | wait(.5); |
mawk2311 | 5:61a0a21134f7 | 146 | } |
mawk2311 | 5:61a0a21134f7 | 147 | |
mawk2311 | 5:61a0a21134f7 | 148 | //break; |
mawk2311 | 5:61a0a21134f7 | 149 | |
ericoneill | 0:d328ecb3fbb1 | 150 | case '3': |
ericoneill | 0:d328ecb3fbb1 | 151 | motor.pulsewidth(.0025*.3); |
ericoneill | 2:30ebae0d3e17 | 152 | pc.printf("30% \n\r"); |
ericoneill | 4:263bddc51c0f | 153 | wait(.5); |
mawk2311 | 5:61a0a21134f7 | 154 | pc.printf("speed: %f",get_avg_speed()); |
mawk2311 | 5:61a0a21134f7 | 155 | |
mawk2311 | 5:61a0a21134f7 | 156 | while(1){ |
mawk2311 | 5:61a0a21134f7 | 157 | |
mawk2311 | 5:61a0a21134f7 | 158 | avg_speed = get_avg_speed(); |
mawk2311 | 5:61a0a21134f7 | 159 | |
mawk2311 | 5:61a0a21134f7 | 160 | if((avg_speed - TUNING_CONSTANT_30) > 0.5f){ |
mawk2311 | 5:61a0a21134f7 | 161 | tuning_val -= .1; |
mawk2311 | 5:61a0a21134f7 | 162 | } else if (avg_speed - TUNING_CONSTANT_30 < -0.5f){ |
mawk2311 | 5:61a0a21134f7 | 163 | tuning_val += .1; |
mawk2311 | 5:61a0a21134f7 | 164 | } else { |
mawk2311 | 5:61a0a21134f7 | 165 | tuning_val = 1; |
mawk2311 | 5:61a0a21134f7 | 166 | } |
mawk2311 | 5:61a0a21134f7 | 167 | motor.pulsewidth(.0025 * .3 * tuning_val); |
mawk2311 | 5:61a0a21134f7 | 168 | pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val); |
mawk2311 | 5:61a0a21134f7 | 169 | wait(.5); |
mawk2311 | 5:61a0a21134f7 | 170 | } |
mawk2311 | 5:61a0a21134f7 | 171 | |
mawk2311 | 5:61a0a21134f7 | 172 | //break; |
ericoneill | 0:d328ecb3fbb1 | 173 | case '5': |
ericoneill | 0:d328ecb3fbb1 | 174 | motor.pulsewidth(.0025*.5); |
ericoneill | 2:30ebae0d3e17 | 175 | pc.printf("50% \n\r"); |
ericoneill | 4:263bddc51c0f | 176 | wait(.5); |
mawk2311 | 5:61a0a21134f7 | 177 | pc.printf("speed: %f",get_avg_speed()); |
mawk2311 | 5:61a0a21134f7 | 178 | |
mawk2311 | 5:61a0a21134f7 | 179 | while(1){ |
mawk2311 | 5:61a0a21134f7 | 180 | |
mawk2311 | 5:61a0a21134f7 | 181 | avg_speed = get_avg_speed(); |
mawk2311 | 5:61a0a21134f7 | 182 | |
mawk2311 | 5:61a0a21134f7 | 183 | if((avg_speed - TUNING_CONSTANT_50) > 0.5f){ |
mawk2311 | 5:61a0a21134f7 | 184 | tuning_val -= .1; |
mawk2311 | 5:61a0a21134f7 | 185 | } else if (avg_speed - TUNING_CONSTANT_50 < -0.5f){ |
mawk2311 | 5:61a0a21134f7 | 186 | tuning_val += .1; |
mawk2311 | 5:61a0a21134f7 | 187 | } else { |
mawk2311 | 5:61a0a21134f7 | 188 | tuning_val = 1; |
mawk2311 | 5:61a0a21134f7 | 189 | } |
mawk2311 | 5:61a0a21134f7 | 190 | motor.pulsewidth(.0025 * .5 * tuning_val); |
mawk2311 | 5:61a0a21134f7 | 191 | pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val); |
mawk2311 | 5:61a0a21134f7 | 192 | wait(.5); |
mawk2311 | 5:61a0a21134f7 | 193 | } |
mawk2311 | 5:61a0a21134f7 | 194 | |
mawk2311 | 5:61a0a21134f7 | 195 | //break; |
ericoneill | 0:d328ecb3fbb1 | 196 | default: |
ericoneill | 0:d328ecb3fbb1 | 197 | motor.pulsewidth(.0025*.3); |
ericoneill | 2:30ebae0d3e17 | 198 | pc.printf("default\n\r"); |
ericoneill | 0:d328ecb3fbb1 | 199 | break; |
ericoneill | 0:d328ecb3fbb1 | 200 | } |
ericoneill | 3:7eaf505f811e | 201 | |
ericoneill | 0:d328ecb3fbb1 | 202 | //servo_sweep(); |
ericoneill | 0:d328ecb3fbb1 | 203 | } |
ericoneill | 0:d328ecb3fbb1 | 204 | } |