E=MC
/
e=mc
.
Diff: main.cpp
- Revision:
- 4:263bddc51c0f
- Parent:
- 3:7eaf505f811e
- Child:
- 5:61a0a21134f7
diff -r 7eaf505f811e -r 263bddc51c0f main.cpp --- a/main.cpp Fri Feb 27 02:18:17 2015 +0000 +++ b/main.cpp Fri Feb 27 04:19:03 2015 +0000 @@ -7,10 +7,27 @@ // encoder setup and variables InterruptIn interrupt(PTA13); -int previous_val = -1; + +int interval1=0; +int interval2=0; +int interval3=0; +int avg_interval=0; +int lastchange1 = 0; +int lastchange2 = 0; +int lastchange3 = 0; +int lastchange4 = 0; Timer t; +//constant *(PWM-expected) +const float TUNING_CONSTANT = .5; +const float PI = 3.14159; +const float WHEEL_CIRCUMFERENCE = .05*PI; - +float get_speed(){ + float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f; + float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE; + return linearSpeed; + +} void servo_sweep(){ for(float p = 0.001; p<0.002; p+=0.0001){ servo.pulsewidth(p); @@ -18,10 +35,33 @@ } } void fallInterrupt(){ - pc.printf("fall"); + + int time = t.read_us(); + interval1 = time - lastchange2; + interval2 = lastchange1-lastchange3; + interval3 = lastchange2 - lastchange4; + avg_interval = (interval1 + interval2 + interval3)/3; + + lastchange4 = lastchange3; + lastchange3 = lastchange2; + lastchange2 = lastchange1; + lastchange1 = time; + //pc.printf("dark to light time : %d\n\r", interval); + //pc.printf("fall"); } void riseInterrupt(){ - pc.printf("rise"); + int time = t.read_us(); + interval1 = time - lastchange2; + interval2 = lastchange1-lastchange3; + interval3 = lastchange2 - lastchange4; + avg_interval = (interval1 + interval2 + interval3)/3; + + lastchange4 = lastchange3; + lastchange3 = lastchange2; + lastchange2 = lastchange1; + lastchange1 = time; + //pc.printf("light to dark time : %d\n\r", interval); + //pc.printf("rise"); } int main() { @@ -32,31 +72,7 @@ t.start(); - int lastchange = 0; - while(1){/* - if(din) { - if(previous_val != 1){ - int current_time = t.read_ms(); - int interval = current_time - lastchange; - lastchange = current_time; - pc.printf("light to dark time : %d\n\r", interval); - previous_val = 1; - } - myled = 1; - //pc.printf("dark"); - } else { - if(previous_val != 0){ - int current_time = t.read_ms(); - int interval = current_time - lastchange; - lastchange = current_time; - pc.printf("dark to light time : %d\n\r", interval); - previous_val = 0; - } - myled = 0; - //pc.printf("light"); - } - //wait(.2f); - */ + while(1){ char choice = pc.getc(); pc.putc(choice); @@ -64,18 +80,31 @@ case '0': motor.pulsewidth(0.0); pc.printf("0% \n\r"); + break; case '1': motor.pulsewidth(.0025); pc.printf("100% \n\r"); + wait(.5); + pc.printf("speed: %f",get_speed()); + break; + case '2': + motor.pulsewidth(.0025*.2); + pc.printf("20% \n\r"); + wait(.5); + pc.printf("speed: %f",get_speed()); break; case '3': motor.pulsewidth(.0025*.3); pc.printf("30% \n\r"); + wait(.5); + pc.printf("speed: %f",get_speed()); break; case '5': motor.pulsewidth(.0025*.5); pc.printf("50% \n\r"); + wait(.5); + pc.printf("speed: %f",get_speed()); break; default: motor.pulsewidth(.0025*.3); @@ -84,7 +113,5 @@ } //servo_sweep(); - //motor_sweep(); - //motor.pulsewidth(.0025); } }