E=MC
/
e=mc
.
main.cpp
- Committer:
- ericoneill
- Date:
- 2015-02-27
- Revision:
- 4:263bddc51c0f
- Parent:
- 3:7eaf505f811e
- Child:
- 5:61a0a21134f7
File content as of revision 4:263bddc51c0f:
#include "mbed.h" DigitalOut myled(LED1); PwmOut servo(PTA5); PwmOut motor(PTA4); Serial pc(USBTX, USBRX); // tx, rx // encoder setup and variables InterruptIn interrupt(PTA13); 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); wait(0.5); } } void fallInterrupt(){ 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(){ 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() { servo.period(0.005); motor.period(.0025); interrupt.fall(&fallInterrupt); interrupt.rise(&riseInterrupt); t.start(); while(1){ char choice = pc.getc(); pc.putc(choice); switch(choice){ 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); pc.printf("default\n\r"); break; } //servo_sweep(); } }