pid+sineinput
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp
- Committer:
- cmaas
- Date:
- 2018-10-26
- Revision:
- 0:447a347725bb
- Child:
- 1:e10ae03926e6
File content as of revision 0:447a347725bb:
// ------------- INITIALIZING ----------------- #include "mbed.h" #include "BiQuad.h" #include "QEI.h" #include "MODSERIAL.h" #include "HIDScope.h" PwmOut pwmpin1(D6); DigitalOut directionpin1(D7); QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING); Ticker ref_rot; Ticker show_counts; Ticker Scope_Data; MODSERIAL pc(USBTX, USBRX); HIDScope scope(2); // INITIALIZING GLOBAl VALUES double Kp = 50; //200 double Ki = 0.5; //1 double Kd = 10; //200 double Ts = 0.1; // Sample time in seconds volatile double reference_rotation = 8400; double motor_position; bool AlwaysTrue; //----------- FUNCTIONS ---------------------- // PID CONTROLLER FUNCTION double PID_controller(double error) { static double error_integral = 0; static double error_prev = error; // initialization with this value only done once! static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); // Proportional part: double u_k = Kp * error; // Integral part error_integral = error_integral + error * Ts; double u_i = Ki * error_integral; // Derivative part double error_derivative = (error - error_prev)/Ts; double filtered_error_derivative = LowPassFilter.step(error_derivative); double u_d = Kd * filtered_error_derivative; error_prev = error; // Sum all parts and return it return u_k + u_i + u_d; } // DIRECTON AND SPEED CONTROL void moter_control(double u) { directionpin1= u > 0.0f; //eithertrueor false pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value } // CONTROLLING THE MOTOR void Motor_mover() { double motor_position = encoder1.getPulses(); double error = reference_rotation - motor_position; double u = PID_controller(error); moter_control(u); } //PRINT TICKER void PrintFlag() { AlwaysTrue = true; } // HIDSCOPE void ScopeData() { double y = encoder1.getPulses(); scope.set(0, y); scope.send(); } //------------------ EXECUTION --------------- int main() { pwmpin1.period_us(60); pc.printf("test"); pc.baud(9600); //show_counts.attach(PrintFlag, 0.2); ref_rot.attach(Motor_mover, 0.01); Scope_Data.attach(ScopeData, 0.1); while (true) { /* PrintFlag(); while (AlwaysTrue){ float q = encoder1.getPulses(); pc.printf("%f \r\n", q); AlwaysTrue = false; wait(0.2f); } */ } }