E=MC
/
e=mc
.
main.cpp@4:263bddc51c0f, 2015-02-27 (annotated)
- Committer:
- ericoneill
- Date:
- Fri Feb 27 04:19:03 2015 +0000
- Revision:
- 4:263bddc51c0f
- Parent:
- 3:7eaf505f811e
- Child:
- 5:61a0a21134f7
linear speed calculated, need to do control
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; |
ericoneill | 1:8e5821dec0f7 | 19 | Timer t; |
ericoneill | 4:263bddc51c0f | 20 | //constant *(PWM-expected) |
ericoneill | 4:263bddc51c0f | 21 | const float TUNING_CONSTANT = .5; |
ericoneill | 4:263bddc51c0f | 22 | const float PI = 3.14159; |
ericoneill | 4:263bddc51c0f | 23 | const float WHEEL_CIRCUMFERENCE = .05*PI; |
ericoneill | 0:d328ecb3fbb1 | 24 | |
ericoneill | 4:263bddc51c0f | 25 | float get_speed(){ |
ericoneill | 4:263bddc51c0f | 26 | float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f; |
ericoneill | 4:263bddc51c0f | 27 | float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE; |
ericoneill | 4:263bddc51c0f | 28 | return linearSpeed; |
ericoneill | 4:263bddc51c0f | 29 | |
ericoneill | 4:263bddc51c0f | 30 | } |
ericoneill | 0:d328ecb3fbb1 | 31 | void servo_sweep(){ |
ericoneill | 0:d328ecb3fbb1 | 32 | for(float p = 0.001; p<0.002; p+=0.0001){ |
ericoneill | 0:d328ecb3fbb1 | 33 | servo.pulsewidth(p); |
ericoneill | 0:d328ecb3fbb1 | 34 | wait(0.5); |
ericoneill | 0:d328ecb3fbb1 | 35 | } |
ericoneill | 0:d328ecb3fbb1 | 36 | } |
ericoneill | 3:7eaf505f811e | 37 | void fallInterrupt(){ |
ericoneill | 4:263bddc51c0f | 38 | |
ericoneill | 4:263bddc51c0f | 39 | int time = t.read_us(); |
ericoneill | 4:263bddc51c0f | 40 | interval1 = time - lastchange2; |
ericoneill | 4:263bddc51c0f | 41 | interval2 = lastchange1-lastchange3; |
ericoneill | 4:263bddc51c0f | 42 | interval3 = lastchange2 - lastchange4; |
ericoneill | 4:263bddc51c0f | 43 | avg_interval = (interval1 + interval2 + interval3)/3; |
ericoneill | 4:263bddc51c0f | 44 | |
ericoneill | 4:263bddc51c0f | 45 | lastchange4 = lastchange3; |
ericoneill | 4:263bddc51c0f | 46 | lastchange3 = lastchange2; |
ericoneill | 4:263bddc51c0f | 47 | lastchange2 = lastchange1; |
ericoneill | 4:263bddc51c0f | 48 | lastchange1 = time; |
ericoneill | 4:263bddc51c0f | 49 | //pc.printf("dark to light time : %d\n\r", interval); |
ericoneill | 4:263bddc51c0f | 50 | //pc.printf("fall"); |
ericoneill | 3:7eaf505f811e | 51 | } |
ericoneill | 3:7eaf505f811e | 52 | void riseInterrupt(){ |
ericoneill | 4:263bddc51c0f | 53 | int time = t.read_us(); |
ericoneill | 4:263bddc51c0f | 54 | interval1 = time - lastchange2; |
ericoneill | 4:263bddc51c0f | 55 | interval2 = lastchange1-lastchange3; |
ericoneill | 4:263bddc51c0f | 56 | interval3 = lastchange2 - lastchange4; |
ericoneill | 4:263bddc51c0f | 57 | avg_interval = (interval1 + interval2 + interval3)/3; |
ericoneill | 4:263bddc51c0f | 58 | |
ericoneill | 4:263bddc51c0f | 59 | lastchange4 = lastchange3; |
ericoneill | 4:263bddc51c0f | 60 | lastchange3 = lastchange2; |
ericoneill | 4:263bddc51c0f | 61 | lastchange2 = lastchange1; |
ericoneill | 4:263bddc51c0f | 62 | lastchange1 = time; |
ericoneill | 4:263bddc51c0f | 63 | //pc.printf("light to dark time : %d\n\r", interval); |
ericoneill | 4:263bddc51c0f | 64 | //pc.printf("rise"); |
ericoneill | 3:7eaf505f811e | 65 | } |
ericoneill | 3:7eaf505f811e | 66 | |
ericoneill | 0:d328ecb3fbb1 | 67 | int main() { |
ericoneill | 0:d328ecb3fbb1 | 68 | servo.period(0.005); |
ericoneill | 0:d328ecb3fbb1 | 69 | motor.period(.0025); |
ericoneill | 3:7eaf505f811e | 70 | interrupt.fall(&fallInterrupt); |
ericoneill | 3:7eaf505f811e | 71 | interrupt.rise(&riseInterrupt); |
ericoneill | 3:7eaf505f811e | 72 | |
ericoneill | 1:8e5821dec0f7 | 73 | |
ericoneill | 1:8e5821dec0f7 | 74 | t.start(); |
ericoneill | 4:263bddc51c0f | 75 | while(1){ |
ericoneill | 2:30ebae0d3e17 | 76 | |
ericoneill | 2:30ebae0d3e17 | 77 | char choice = pc.getc(); |
ericoneill | 2:30ebae0d3e17 | 78 | pc.putc(choice); |
ericoneill | 2:30ebae0d3e17 | 79 | switch(choice){ |
ericoneill | 0:d328ecb3fbb1 | 80 | case '0': |
ericoneill | 0:d328ecb3fbb1 | 81 | motor.pulsewidth(0.0); |
ericoneill | 2:30ebae0d3e17 | 82 | pc.printf("0% \n\r"); |
ericoneill | 4:263bddc51c0f | 83 | |
ericoneill | 0:d328ecb3fbb1 | 84 | break; |
ericoneill | 0:d328ecb3fbb1 | 85 | case '1': |
ericoneill | 0:d328ecb3fbb1 | 86 | motor.pulsewidth(.0025); |
ericoneill | 2:30ebae0d3e17 | 87 | pc.printf("100% \n\r"); |
ericoneill | 4:263bddc51c0f | 88 | wait(.5); |
ericoneill | 4:263bddc51c0f | 89 | pc.printf("speed: %f",get_speed()); |
ericoneill | 4:263bddc51c0f | 90 | break; |
ericoneill | 4:263bddc51c0f | 91 | case '2': |
ericoneill | 4:263bddc51c0f | 92 | motor.pulsewidth(.0025*.2); |
ericoneill | 4:263bddc51c0f | 93 | pc.printf("20% \n\r"); |
ericoneill | 4:263bddc51c0f | 94 | wait(.5); |
ericoneill | 4:263bddc51c0f | 95 | pc.printf("speed: %f",get_speed()); |
ericoneill | 0:d328ecb3fbb1 | 96 | break; |
ericoneill | 0:d328ecb3fbb1 | 97 | case '3': |
ericoneill | 0:d328ecb3fbb1 | 98 | motor.pulsewidth(.0025*.3); |
ericoneill | 2:30ebae0d3e17 | 99 | pc.printf("30% \n\r"); |
ericoneill | 4:263bddc51c0f | 100 | wait(.5); |
ericoneill | 4:263bddc51c0f | 101 | pc.printf("speed: %f",get_speed()); |
ericoneill | 0:d328ecb3fbb1 | 102 | break; |
ericoneill | 0:d328ecb3fbb1 | 103 | case '5': |
ericoneill | 0:d328ecb3fbb1 | 104 | motor.pulsewidth(.0025*.5); |
ericoneill | 2:30ebae0d3e17 | 105 | pc.printf("50% \n\r"); |
ericoneill | 4:263bddc51c0f | 106 | wait(.5); |
ericoneill | 4:263bddc51c0f | 107 | pc.printf("speed: %f",get_speed()); |
ericoneill | 0:d328ecb3fbb1 | 108 | break; |
ericoneill | 0:d328ecb3fbb1 | 109 | default: |
ericoneill | 0:d328ecb3fbb1 | 110 | motor.pulsewidth(.0025*.3); |
ericoneill | 2:30ebae0d3e17 | 111 | pc.printf("default\n\r"); |
ericoneill | 0:d328ecb3fbb1 | 112 | break; |
ericoneill | 0:d328ecb3fbb1 | 113 | } |
ericoneill | 3:7eaf505f811e | 114 | |
ericoneill | 0:d328ecb3fbb1 | 115 | //servo_sweep(); |
ericoneill | 0:d328ecb3fbb1 | 116 | } |
ericoneill | 0:d328ecb3fbb1 | 117 | } |