Start van regelaar
Dependencies: Encoder MODSERIAL mbed
Dependents: K9motoraansturing_copy lichtpoortjes
Diff: main.cpp
- Revision:
- 1:9d05c0236c7e
- Parent:
- 0:7bc93f851767
- Child:
- 2:3d39bce54dfe
--- a/main.cpp Tue Oct 08 21:57:18 2013 +0000 +++ b/main.cpp Wed Oct 09 06:49:17 2013 +0000 @@ -2,12 +2,11 @@ #include "encoder.h" #include "MODSERIAL.h" -/** Coerce -> float in, and coerce if less than min, or larger than max **/ -void coerce(float * in, float min, float max); -AnalogIn potmeter(PTC2); +/** 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; - void setlooptimerflag(void) { looptimerflag = true; @@ -15,31 +14,33 @@ int main() { - Encoder motor1(PTD0,PTC9); + //LOCAL VARIABLES + AnalogIn potmeter(PTC2); + Encoder motor1(PTD0,PTC9);//first pin on PTAx or PTDx MODSERIAL pc(USBTX,USBRX); PwmOut pwm_motor(PTA12); DigitalOut motordir(PTD3); float setpoint; - float new_pwm; - pc.baud(115200); + float pwm_to_motor; + //START OF CODE + pc.baud(230400); Ticker looptimer; looptimer.attach(setlooptimerflag,0.01); - pc.printf("bla"); + pc.printf("bla"); + //INFINITE LOOP while(1) { - while(!looptimerflag); + while(looptimerflag != true); looptimerflag = false; - setpoint = (potmeter.read())*2000; + setpoint = (potmeter.read()-0.5)*2000; pc.printf("s: %f, %d \n\r", setpoint, motor1.getPosition()); - if(motor1.getPosition() > setpoint) - new_pwm = 0.5; - else - new_pwm = -0.5; - - if(new_pwm > 0) + pwm_to_motor = (setpoint - motor1.getPosition())*.001; + keep_in_range(&pwm_to_motor, -1,1); + if(pwm_to_motor > 0) motordir = 0; else - motordir = 1; - pwm_motor.write(abs(new_pwm)); + motordir = 1; + //WRITE VALUE TO MOTOR + pwm_motor.write(abs(pwm_to_motor)); } } @@ -47,7 +48,7 @@ //coerces value 'in' to min or max when exceeding those values //if you'd like to understand the statement below take a google for //'ternary operators'. -void coerce(float * in, float min, float max) +void keep_in_range(float * in, float min, float max) { *in > min ? *in < max? : *in = max: *in = min; }