pid+sineinput
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp@6:bbc9ce35e0a4, 2018-10-30 (annotated)
- Committer:
- cmaas
- Date:
- Tue Oct 30 06:35:29 2018 +0000
- Revision:
- 6:bbc9ce35e0a4
- Parent:
- 5:ea9f9f11d7c1
Correct Kp, Ki, Kd values added
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cmaas | 0:447a347725bb | 1 | |
cmaas | 0:447a347725bb | 2 | // ------------- INITIALIZING ----------------- |
cmaas | 0:447a347725bb | 3 | |
cmaas | 0:447a347725bb | 4 | #include "mbed.h" |
cmaas | 0:447a347725bb | 5 | #include "BiQuad.h" |
cmaas | 0:447a347725bb | 6 | #include "QEI.h" |
cmaas | 0:447a347725bb | 7 | #include "MODSERIAL.h" |
cmaas | 0:447a347725bb | 8 | #include "HIDScope.h" |
cmaas | 6:bbc9ce35e0a4 | 9 | //#include "Math.h" |
cmaas | 0:447a347725bb | 10 | |
cmaas | 1:e10ae03926e6 | 11 | AnalogIn button2(A4); |
cmaas | 0:447a347725bb | 12 | PwmOut pwmpin1(D6); |
cmaas | 0:447a347725bb | 13 | DigitalOut directionpin1(D7); |
cmaas | 0:447a347725bb | 14 | QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING); |
cmaas | 0:447a347725bb | 15 | Ticker ref_rot; |
cmaas | 2:2c32825bb327 | 16 | Ticker show_counts; |
cmaas | 0:447a347725bb | 17 | Ticker Scope_Data; |
cmaas | 0:447a347725bb | 18 | MODSERIAL pc(USBTX, USBRX); |
cmaas | 3:fe052c67c840 | 19 | HIDScope scope(2); |
cmaas | 0:447a347725bb | 20 | |
cmaas | 0:447a347725bb | 21 | // INITIALIZING GLOBAl VALUES |
cmaas | 5:ea9f9f11d7c1 | 22 | |
cmaas | 5:ea9f9f11d7c1 | 23 | double PI = 3.141592653589793238462; |
cmaas | 6:bbc9ce35e0a4 | 24 | double Kp = 16; //200 , 50 |
cmaas | 6:bbc9ce35e0a4 | 25 | double Ki = 0; //1, 0.5 |
cmaas | 6:bbc9ce35e0a4 | 26 | double Kd = 4.5; //200, 10 |
cmaas | 0:447a347725bb | 27 | double Ts = 0.1; // Sample time in seconds |
cmaas | 5:ea9f9f11d7c1 | 28 | volatile double reference_rotation = PI; //define as radians |
cmaas | 0:447a347725bb | 29 | double motor_position; |
cmaas | 0:447a347725bb | 30 | bool AlwaysTrue; |
cmaas | 5:ea9f9f11d7c1 | 31 | |
cmaas | 0:447a347725bb | 32 | |
cmaas | 0:447a347725bb | 33 | //----------- FUNCTIONS ---------------------- |
cmaas | 0:447a347725bb | 34 | |
cmaas | 0:447a347725bb | 35 | |
cmaas | 0:447a347725bb | 36 | // PID CONTROLLER FUNCTION |
cmaas | 0:447a347725bb | 37 | double PID_controller(double error) |
cmaas | 0:447a347725bb | 38 | { |
cmaas | 2:2c32825bb327 | 39 | static double error_integral = 0; |
cmaas | 2:2c32825bb327 | 40 | static double error_prev = error; // initialization with this value only done once! |
cmaas | 2:2c32825bb327 | 41 | static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); |
cmaas | 0:447a347725bb | 42 | |
cmaas | 2:2c32825bb327 | 43 | // Proportional part: |
cmaas | 2:2c32825bb327 | 44 | double u_k = Kp * error; |
cmaas | 0:447a347725bb | 45 | |
cmaas | 2:2c32825bb327 | 46 | // Integral part |
cmaas | 2:2c32825bb327 | 47 | error_integral = error_integral + error * Ts; |
cmaas | 2:2c32825bb327 | 48 | double u_i = Ki * error_integral; |
cmaas | 0:447a347725bb | 49 | |
cmaas | 2:2c32825bb327 | 50 | // Derivative part |
cmaas | 2:2c32825bb327 | 51 | double error_derivative = (error - error_prev)/Ts; |
cmaas | 2:2c32825bb327 | 52 | double filtered_error_derivative = LowPassFilter.step(error_derivative); |
cmaas | 2:2c32825bb327 | 53 | double u_d = Kd * filtered_error_derivative; |
cmaas | 2:2c32825bb327 | 54 | error_prev = error; |
cmaas | 0:447a347725bb | 55 | |
cmaas | 2:2c32825bb327 | 56 | // Sum all parts and return it |
cmaas | 2:2c32825bb327 | 57 | return u_k + u_i + u_d; |
cmaas | 0:447a347725bb | 58 | } |
cmaas | 0:447a347725bb | 59 | |
cmaas | 0:447a347725bb | 60 | |
cmaas | 0:447a347725bb | 61 | // DIRECTON AND SPEED CONTROL |
cmaas | 0:447a347725bb | 62 | void moter_control(double u) |
cmaas | 0:447a347725bb | 63 | { |
cmaas | 0:447a347725bb | 64 | directionpin1= u > 0.0f; //eithertrueor false |
cmaas | 2:2c32825bb327 | 65 | pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value |
cmaas | 0:447a347725bb | 66 | } |
cmaas | 0:447a347725bb | 67 | |
cmaas | 0:447a347725bb | 68 | |
cmaas | 0:447a347725bb | 69 | // CONTROLLING THE MOTOR |
cmaas | 0:447a347725bb | 70 | void Motor_mover() |
cmaas | 0:447a347725bb | 71 | { |
cmaas | 1:e10ae03926e6 | 72 | double motor_position = encoder1.getPulses(); //output in counts |
cmaas | 1:e10ae03926e6 | 73 | double error = reference_rotation - motor_position*(2*PI)/8400; |
cmaas | 0:447a347725bb | 74 | double u = PID_controller(error); |
cmaas | 0:447a347725bb | 75 | moter_control(u); |
cmaas | 0:447a347725bb | 76 | } |
cmaas | 0:447a347725bb | 77 | |
cmaas | 0:447a347725bb | 78 | //PRINT TICKER |
cmaas | 0:447a347725bb | 79 | void PrintFlag() |
cmaas | 0:447a347725bb | 80 | { |
cmaas | 0:447a347725bb | 81 | AlwaysTrue = true; |
cmaas | 2:2c32825bb327 | 82 | } |
cmaas | 3:fe052c67c840 | 83 | |
cmaas | 0:447a347725bb | 84 | // HIDSCOPE |
cmaas | 0:447a347725bb | 85 | void ScopeData() |
cmaas | 0:447a347725bb | 86 | { |
cmaas | 0:447a347725bb | 87 | double y = encoder1.getPulses(); |
cmaas | 0:447a347725bb | 88 | scope.set(0, y); |
cmaas | 0:447a347725bb | 89 | scope.send(); |
cmaas | 2:2c32825bb327 | 90 | } |
cmaas | 3:fe052c67c840 | 91 | |
cmaas | 0:447a347725bb | 92 | //------------------ EXECUTION --------------- |
cmaas | 0:447a347725bb | 93 | |
cmaas | 0:447a347725bb | 94 | int main() |
cmaas | 0:447a347725bb | 95 | { |
cmaas | 2:2c32825bb327 | 96 | // INITIALIZING |
cmaas | 2:2c32825bb327 | 97 | |
cmaas | 2:2c32825bb327 | 98 | pwmpin1.period_us(60); |
cmaas | 2:2c32825bb327 | 99 | pc.printf("test"); |
cmaas | 2:2c32825bb327 | 100 | pc.baud(9600); |
cmaas | 2:2c32825bb327 | 101 | //show_counts.attach(PrintFlag, 0.2); |
cmaas | 2:2c32825bb327 | 102 | ref_rot.attach(Motor_mover, 0.01); |
cmaas | 6:bbc9ce35e0a4 | 103 | Scope_Data.attach(ScopeData, 0.01); |
cmaas | 2:2c32825bb327 | 104 | |
cmaas | 2:2c32825bb327 | 105 | // DEFININING VARIABLES |
cmaas | 2:2c32825bb327 | 106 | //float reference; |
cmaas | 2:2c32825bb327 | 107 | float foo[17]; |
cmaas | 2:2c32825bb327 | 108 | float length = 9.0f; |
cmaas | 2:2c32825bb327 | 109 | int a; |
cmaas | 2:2c32825bb327 | 110 | int q; |
cmaas | 2:2c32825bb327 | 111 | |
cmaas | 2:2c32825bb327 | 112 | while (true) { |
cmaas | 2:2c32825bb327 | 113 | |
cmaas | 2:2c32825bb327 | 114 | // EXECTURION IN MAIN |
cmaas | 2:2c32825bb327 | 115 | // SIN ALS INPUT GEVEN |
cmaas | 2:2c32825bb327 | 116 | if (button2 == false){ |
cmaas | 2:2c32825bb327 | 117 | wait (0.2f); |
cmaas | 2:2c32825bb327 | 118 | for (q=0; q<4; q++){ |
cmaas | 1:e10ae03926e6 | 119 | float b = 8.0f; |
cmaas | 2:2c32825bb327 | 120 | for (a=0; a<10; a++){ |
cmaas | 2:2c32825bb327 | 121 | foo[a] = 3.14f/length*(a); |
cmaas | 2:2c32825bb327 | 122 | //printf("%f\n\r", foo[a]); |
cmaas | 2:2c32825bb327 | 123 | reference_rotation = foo[a]; |
cmaas | 6:bbc9ce35e0a4 | 124 | wait(0.2f); |
cmaas | 2:2c32825bb327 | 125 | } |
cmaas | 2:2c32825bb327 | 126 | for(a=10; a<18; a++){ |
cmaas | 2:2c32825bb327 | 127 | foo[a] = 3.14f/length*b; |
cmaas | 2:2c32825bb327 | 128 | //printf("%f\n\r", foo[a]); |
cmaas | 2:2c32825bb327 | 129 | reference_rotation = foo[a]; |
cmaas | 2:2c32825bb327 | 130 | b = b-1.0f; |
cmaas | 6:bbc9ce35e0a4 | 131 | wait(0.2f); |
cmaas | 2:2c32825bb327 | 132 | } |
cmaas | 1:e10ae03926e6 | 133 | } |
cmaas | 2:2c32825bb327 | 134 | |
cmaas | 6:bbc9ce35e0a4 | 135 | reference_rotation = 0; |
cmaas | 2:2c32825bb327 | 136 | |
cmaas | 2:2c32825bb327 | 137 | |
cmaas | 2:2c32825bb327 | 138 | |
cmaas | 2:2c32825bb327 | 139 | /* //IETS PRINTEN OP JE TERMINAL |
cmaas | 0:447a347725bb | 140 | PrintFlag(); |
cmaas | 0:447a347725bb | 141 | while (AlwaysTrue){ |
cmaas | 0:447a347725bb | 142 | float q = encoder1.getPulses(); |
cmaas | 0:447a347725bb | 143 | pc.printf("%f \r\n", q); |
cmaas | 0:447a347725bb | 144 | AlwaysTrue = false; |
cmaas | 0:447a347725bb | 145 | wait(0.2f); |
cmaas | 0:447a347725bb | 146 | } |
cmaas | 0:447a347725bb | 147 | */ |
cmaas | 1:e10ae03926e6 | 148 | |
cmaas | 0:447a347725bb | 149 | |
cmaas | 2:2c32825bb327 | 150 | |
cmaas | 0:447a347725bb | 151 | } |
cmaas | 2:2c32825bb327 | 152 | |
cmaas | 2:2c32825bb327 | 153 | } |
cmaas | 2:2c32825bb327 | 154 | } |