voor thomas
Dependencies: BMT-K9-Regelaar MODSERIAL mbed
Fork of BMT-K9-Regelaar by
Diff: main.cpp
- Revision:
- 2:2d32a0543c63
- Parent:
- 1:9d05c0236c7e
--- a/main.cpp Wed Oct 09 06:49:17 2013 +0000 +++ b/main.cpp Thu Oct 31 15:22:46 2013 +0000 @@ -1,46 +1,74 @@ #include "mbed.h" -#include "encoder.h" #include "MODSERIAL.h" + /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/ void keep_in_range(float * in, float min, float max); volatile bool looptimerflag; +float sluis10; +int sluis11; +float y; +float y1; +float y2; +float z; +float z1; +float z2; +float numl1; +float numl2; +float numl3; +float denl1; +float denl2; +float denl3; void setlooptimerflag(void) { looptimerflag = true; } - + AnalogIn sluis1(PTC2); + int main() { - //LOCAL VARIABLES - AnalogIn potmeter(PTC2); - Encoder motor1(PTD0,PTC9);//first pin on PTAx or PTDx + //START OF CODE MODSERIAL pc(USBTX,USBRX); - PwmOut pwm_motor(PTA12); - DigitalOut motordir(PTD3); - float setpoint; - float pwm_to_motor; - //START OF CODE - pc.baud(230400); + pc.baud(115200); Ticker looptimer; - looptimer.attach(setlooptimerflag,0.01); - pc.printf("bla"); + looptimer.attach(setlooptimerflag,0.004); + y=0; + y1=0; + y2=0; + z1=0; + z2=0; + + //Low pass, 2 Hz, 2e orde, 1 ms. + numl1=0.003621681514929; + numl2=0.007243363029857; + numl3=0.003621681514929; + //denl1=1; + denl2=-1.822694925196308; + denl3=0.837181651256023; + //INFINITE LOOP while(1) { while(looptimerflag != true); looptimerflag = false; - setpoint = (potmeter.read()-0.5)*2000; - pc.printf("s: %f, %d \n\r", setpoint, motor1.getPosition()); - pwm_to_motor = (setpoint - motor1.getPosition())*.001; - keep_in_range(&pwm_to_motor, -1,1); - if(pwm_to_motor > 0) - motordir = 0; + y = sluis1.read(); + z=y*numl1+y1*numl2+y2*numl3-z1*denl2-z2*denl3; + + y1=y; + y2=y1; + z1=z; + z2=z1; + + if(z > 0.85) + sluis11 = 1; else - motordir = 1; - //WRITE VALUE TO MOTOR - pwm_motor.write(abs(pwm_to_motor)); + sluis11 = 0; + //pc.printf("%f, %i \n\r", sluis10, sluis11); + //pc.printf("%f \n\r", z); + pc.printf("%i \n\r", sluis11); + + } }