![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
PIDcontroller_without_biquadfilter
Dependencies: MODSERIAL QEI biquadFilter mbed
Fork of 20170910_PID_V1_0 by
Diff: main.cpp
- Revision:
- 0:85ca550760e9
- Child:
- 1:b66e14435f70
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Oct 09 13:52:37 2017 +0000 @@ -0,0 +1,78 @@ +#include "mbed.h" +#include "QEI.h" +#include "MODSERIAL.h" +#include "math.h" + + + +DigitalOut gpo(D0); +DigitalOut ledb(LED_BLUE); +DigitalOut ledr(LED_RED); +DigitalOut ledg(LED_GREEN); +DigitalOut motor1DC(D7); +DigitalOut motor1PWM(D6); +DigitalOut motor2DC(D4); +DigitalOut motor2PWM(D5); + +AnalogIn potMeter1(A0); +AnalogIn potMeter2(A1); +DigitalIn button1(D11); +DigitalIn button2(D12); +QEI Encoder1(D12,D13,NC,4200); + +MODSERIAL pc(USBTX,USBRX); + +Ticker controller; + +const double pi = 3.1415926535897; +const float MOTOR1_KP = 5; +const float RAD_PER_PULSE = (2*pi)/4200; +const float CONTROLLER_TS = 0.01; //TIME INTERVAL/ hZ + +float P(double error, const float Kp){ + return Kp * error; + } + +void motor1_Controller(){ + double reference = 10*potMeter1.read(); + double position = RAD_PER_PULSE*Encoder1.getPulses(); + double motor1 = P(reference-position, MOTOR1_KP); + motor1PWM = motor1; + + if(motor1 > 0.1){ + motor1DC = 1; + + ledr = 1; + ledg = 1; //Blau + ledb = 0; + } + else if (motor1<-0.1) { + motor1DC = 0; + + ledb = 1; + ledr = 1; + ledg = 0; //Groen + + } + else{ + motor1PWM = 0; + + ledb = 1; //Rood + ledr = 0; + ledg = 1; + } + + +} + +int main(){ + pc.baud(115200); + controller.attach(&motor1_Controller, CONTROLLER_TS); + + while(1){ + double reference = 10*potMeter1.read(); + double position = RAD_PER_PULSE*Encoder1.getPulses(); + double motor1 = P(reference-position, MOTOR1_KP); + pc.printf("\r value motor1: %f. reference Pot: %f. Position: %f \n", motor1, reference, position); + } +}