PIDcontroller_without_biquadfilter
Dependencies: MODSERIAL QEI biquadFilter mbed
Fork of 20170910_PID_V1_0 by
main.cpp
- Committer:
- Arnoud113
- Date:
- 2017-10-09
- Revision:
- 0:85ca550760e9
- Child:
- 1:b66e14435f70
File content as of revision 0:85ca550760e9:
#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); } }