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