pid+sineinput
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp
- Committer:
- cmaas
- Date:
- 2018-10-30
- Revision:
- 6:bbc9ce35e0a4
- Parent:
- 5:ea9f9f11d7c1
File content as of revision 6:bbc9ce35e0a4:
// ------------- INITIALIZING ----------------- #include "mbed.h" #include "BiQuad.h" #include "QEI.h" #include "MODSERIAL.h" #include "HIDScope.h" //#include "Math.h" AnalogIn button2(A4); 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 PI = 3.141592653589793238462; double Kp = 16; //200 , 50 double Ki = 0; //1, 0.5 double Kd = 4.5; //200, 10 double Ts = 0.1; // Sample time in seconds volatile double reference_rotation = PI; //define as radians 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(); //output in counts double error = reference_rotation - motor_position*(2*PI)/8400; 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() { // INITIALIZING 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.01); // DEFININING VARIABLES //float reference; float foo[17]; float length = 9.0f; int a; int q; while (true) { // EXECTURION IN MAIN // SIN ALS INPUT GEVEN if (button2 == false){ wait (0.2f); for (q=0; q<4; q++){ float b = 8.0f; for (a=0; a<10; a++){ foo[a] = 3.14f/length*(a); //printf("%f\n\r", foo[a]); reference_rotation = foo[a]; wait(0.2f); } for(a=10; a<18; a++){ foo[a] = 3.14f/length*b; //printf("%f\n\r", foo[a]); reference_rotation = foo[a]; b = b-1.0f; wait(0.2f); } } reference_rotation = 0; /* //IETS PRINTEN OP JE TERMINAL PrintFlag(); while (AlwaysTrue){ float q = encoder1.getPulses(); pc.printf("%f \r\n", q); AlwaysTrue = false; wait(0.2f); } */ } } }