PID controller voor 1 motor die een hoek van 20 graden draait, niet werkend.
Dependencies: MODSERIAL QEI mbed biquadFilter
Inverse Kinematics + PID Controller
main.cpp@5:0251fde34cdc, 2016-10-24 (annotated)
- Committer:
- willem_hoitzing
- Date:
- Mon Oct 24 10:45:05 2016 +0000
- Revision:
- 5:0251fde34cdc
- Parent:
- 4:a5f3e1838e3e
- Child:
- 6:4d254faf2428
velocity control - alleen Kp waarde - werkt niet
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
willem_hoitzing | 0:26ce65a63616 | 1 | #include "stdio.h" |
willem_hoitzing | 0:26ce65a63616 | 2 | #include "math.h" |
willem_hoitzing | 0:26ce65a63616 | 3 | #include "mbed.h" |
willem_hoitzing | 0:26ce65a63616 | 4 | #include "QEI.h" |
willem_hoitzing | 0:26ce65a63616 | 5 | #include "MODSERIAL.h" |
willem_hoitzing | 3:6ba52d1ae499 | 6 | #include "BiQuad.h" |
willem_hoitzing | 0:26ce65a63616 | 7 | |
willem_hoitzing | 0:26ce65a63616 | 8 | MODSERIAL pc(USBTX, USBRX); |
willem_hoitzing | 2:0a976fb06ff8 | 9 | QEI wheel_M1 (D13, D12, NC, 32); |
willem_hoitzing | 2:0a976fb06ff8 | 10 | QEI wheel_M2 (D10, D11, NC, 32); |
willem_hoitzing | 2:0a976fb06ff8 | 11 | PwmOut pwm_M1 (D6); |
willem_hoitzing | 2:0a976fb06ff8 | 12 | PwmOut pwm_M2 (D5); |
willem_hoitzing | 2:0a976fb06ff8 | 13 | DigitalOut dir_M1 (D7); |
willem_hoitzing | 2:0a976fb06ff8 | 14 | DigitalOut dir_M2 (D4); |
willem_hoitzing | 5:0251fde34cdc | 15 | InterruptIn knop(SW3); |
willem_hoitzing | 0:26ce65a63616 | 16 | |
willem_hoitzing | 2:0a976fb06ff8 | 17 | volatile double q1 = 0; |
willem_hoitzing | 2:0a976fb06ff8 | 18 | volatile double q2 = 0; |
willem_hoitzing | 5:0251fde34cdc | 19 | volatile double q1_prev = 0; |
willem_hoitzing | 5:0251fde34cdc | 20 | volatile double q2_prev = 0; |
willem_hoitzing | 5:0251fde34cdc | 21 | volatile double q1_v; |
willem_hoitzing | 5:0251fde34cdc | 22 | volatile double q2_v; |
willem_hoitzing | 3:6ba52d1ae499 | 23 | |
willem_hoitzing | 3:6ba52d1ae499 | 24 | volatile bool go_flag_initialize = false; |
willem_hoitzing | 0:26ce65a63616 | 25 | |
willem_hoitzing | 3:6ba52d1ae499 | 26 | void flag_initialize() |
willem_hoitzing | 0:26ce65a63616 | 27 | { |
willem_hoitzing | 3:6ba52d1ae499 | 28 | go_flag_initialize = true; |
willem_hoitzing | 3:6ba52d1ae499 | 29 | } |
willem_hoitzing | 3:6ba52d1ae499 | 30 | |
willem_hoitzing | 5:0251fde34cdc | 31 | volatile double q1_v_ref; |
willem_hoitzing | 5:0251fde34cdc | 32 | volatile double q2_v_ref; |
willem_hoitzing | 3:6ba52d1ae499 | 33 | void initialize() |
willem_hoitzing | 3:6ba52d1ae499 | 34 | { |
willem_hoitzing | 5:0251fde34cdc | 35 | q1_v_ref = 0.4; //2*3.1415 /(2*3.1415); |
willem_hoitzing | 5:0251fde34cdc | 36 | q2_v_ref = 0*3.1415 /(2*3.1415); |
willem_hoitzing | 2:0a976fb06ff8 | 37 | } |
willem_hoitzing | 0:26ce65a63616 | 38 | |
willem_hoitzing | 3:6ba52d1ae499 | 39 | const double TS = 0.02; |
willem_hoitzing | 5:0251fde34cdc | 40 | const double M1_Kp = 0.05, M1_Ki = 0.00, M1_Kd = 0; |
willem_hoitzing | 5:0251fde34cdc | 41 | const double M2_Kp = 0.3, M2_Ki = 0.00, M2_Kd = 0; |
willem_hoitzing | 5:0251fde34cdc | 42 | const double N = 0; |
willem_hoitzing | 3:6ba52d1ae499 | 43 | |
willem_hoitzing | 3:6ba52d1ae499 | 44 | volatile double ctrlOutput_M1 = 0; |
willem_hoitzing | 3:6ba52d1ae499 | 45 | volatile double ctrlOutput_M2 = 0; |
willem_hoitzing | 3:6ba52d1ae499 | 46 | |
willem_hoitzing | 4:a5f3e1838e3e | 47 | Ticker update_encoder_ticker; |
willem_hoitzing | 5:0251fde34cdc | 48 | volatile bool go_flag_update_encoder = false; |
willem_hoitzing | 5:0251fde34cdc | 49 | void flag_update_encoder() |
willem_hoitzing | 5:0251fde34cdc | 50 | { |
willem_hoitzing | 5:0251fde34cdc | 51 | go_flag_update_encoder = true; |
willem_hoitzing | 5:0251fde34cdc | 52 | } |
willem_hoitzing | 5:0251fde34cdc | 53 | |
willem_hoitzing | 4:a5f3e1838e3e | 54 | void update_encoder() |
willem_hoitzing | 2:0a976fb06ff8 | 55 | { |
willem_hoitzing | 5:0251fde34cdc | 56 | q1 = wheel_M1.getPulses()/(1334.355/2); |
willem_hoitzing | 5:0251fde34cdc | 57 | q2 = wheel_M2.getPulses()/(1334.355/2); |
willem_hoitzing | 5:0251fde34cdc | 58 | q1_v = (q1-q1_prev)/0.02/0.3; |
willem_hoitzing | 5:0251fde34cdc | 59 | q2_v = (q2-q2_prev)/0.02/0.3; |
willem_hoitzing | 5:0251fde34cdc | 60 | q1_prev = q1; |
willem_hoitzing | 5:0251fde34cdc | 61 | q2_prev = q2; |
willem_hoitzing | 5:0251fde34cdc | 62 | pc.printf("q1_v = %f \tq1_v_ref = %f \tPID1 = %f \tq2_v = %f \tq2_v_ref = %f \tPID2 = %f\n\r",q1_v, q1_v_ref, ctrlOutput_M1,q2_v,q2_v_ref,ctrlOutput_M2); |
willem_hoitzing | 2:0a976fb06ff8 | 63 | } |
willem_hoitzing | 2:0a976fb06ff8 | 64 | |
willem_hoitzing | 3:6ba52d1ae499 | 65 | BiQuad pidf_M1; |
willem_hoitzing | 3:6ba52d1ae499 | 66 | BiQuad pidf_M2; |
willem_hoitzing | 3:6ba52d1ae499 | 67 | Ticker PIDcontrol_M1; |
willem_hoitzing | 3:6ba52d1ae499 | 68 | Ticker PIDcontrol_M2; |
willem_hoitzing | 5:0251fde34cdc | 69 | volatile bool go_flag_M1_controller = false; |
willem_hoitzing | 5:0251fde34cdc | 70 | volatile bool go_flag_M2_controller = false; |
willem_hoitzing | 5:0251fde34cdc | 71 | |
willem_hoitzing | 5:0251fde34cdc | 72 | void flag_M1_controller() |
willem_hoitzing | 5:0251fde34cdc | 73 | { |
willem_hoitzing | 5:0251fde34cdc | 74 | go_flag_M1_controller = true; |
willem_hoitzing | 5:0251fde34cdc | 75 | } |
willem_hoitzing | 5:0251fde34cdc | 76 | |
willem_hoitzing | 5:0251fde34cdc | 77 | void flag_M2_controller() |
willem_hoitzing | 5:0251fde34cdc | 78 | { |
willem_hoitzing | 5:0251fde34cdc | 79 | go_flag_M2_controller = true; |
willem_hoitzing | 5:0251fde34cdc | 80 | } |
willem_hoitzing | 0:26ce65a63616 | 81 | |
willem_hoitzing | 2:0a976fb06ff8 | 82 | void M1_controller() |
willem_hoitzing | 2:0a976fb06ff8 | 83 | { |
willem_hoitzing | 5:0251fde34cdc | 84 | ctrlOutput_M1 = pidf_M1.step(q1_v_ref-q1_v); |
willem_hoitzing | 5:0251fde34cdc | 85 | |
willem_hoitzing | 5:0251fde34cdc | 86 | if (ctrlOutput_M1 < 0) { |
willem_hoitzing | 3:6ba52d1ae499 | 87 | dir_M1 = 1; |
willem_hoitzing | 5:0251fde34cdc | 88 | } else { |
willem_hoitzing | 2:0a976fb06ff8 | 89 | dir_M1 = 0; |
willem_hoitzing | 2:0a976fb06ff8 | 90 | } |
willem_hoitzing | 5:0251fde34cdc | 91 | if (q1>0.5*3.1415) { |
willem_hoitzing | 5:0251fde34cdc | 92 | pwm_M1 = 0; |
willem_hoitzing | 5:0251fde34cdc | 93 | } else if (q1<-0.5*3.1415) { |
willem_hoitzing | 5:0251fde34cdc | 94 | pwm_M1 = 0; |
willem_hoitzing | 5:0251fde34cdc | 95 | } else { |
willem_hoitzing | 5:0251fde34cdc | 96 | pwm_M1 = abs(ctrlOutput_M1) + q1_v; |
willem_hoitzing | 5:0251fde34cdc | 97 | } |
willem_hoitzing | 3:6ba52d1ae499 | 98 | } |
willem_hoitzing | 3:6ba52d1ae499 | 99 | |
willem_hoitzing | 3:6ba52d1ae499 | 100 | void M2_controller() |
willem_hoitzing | 3:6ba52d1ae499 | 101 | { |
willem_hoitzing | 5:0251fde34cdc | 102 | ctrlOutput_M2 = pidf_M2.step(q2_v_ref-q2_v); |
willem_hoitzing | 5:0251fde34cdc | 103 | |
willem_hoitzing | 5:0251fde34cdc | 104 | if (ctrlOutput_M2 < 0) { |
willem_hoitzing | 3:6ba52d1ae499 | 105 | dir_M2 = 1; |
willem_hoitzing | 5:0251fde34cdc | 106 | } else { |
willem_hoitzing | 3:6ba52d1ae499 | 107 | dir_M2 = 0; |
willem_hoitzing | 2:0a976fb06ff8 | 108 | } |
willem_hoitzing | 5:0251fde34cdc | 109 | pwm_M2 = abs(ctrlOutput_M2) + q2_v; |
willem_hoitzing | 0:26ce65a63616 | 110 | } |
willem_hoitzing | 0:26ce65a63616 | 111 | |
willem_hoitzing | 0:26ce65a63616 | 112 | int main() |
willem_hoitzing | 0:26ce65a63616 | 113 | { |
willem_hoitzing | 0:26ce65a63616 | 114 | pc.baud(115200); |
willem_hoitzing | 5:0251fde34cdc | 115 | wheel_M1.reset(); |
willem_hoitzing | 5:0251fde34cdc | 116 | wheel_M2.reset(); |
willem_hoitzing | 3:6ba52d1ae499 | 117 | pidf_M1.PIDF(M1_Kp,M1_Ki,M1_Kd,N,TS); |
willem_hoitzing | 3:6ba52d1ae499 | 118 | pidf_M2.PIDF(M2_Kp,M2_Ki,M2_Kd,N,TS); |
willem_hoitzing | 5:0251fde34cdc | 119 | // flag functions/tickers |
willem_hoitzing | 5:0251fde34cdc | 120 | update_encoder_ticker.attach(&flag_update_encoder, 0.02f); |
willem_hoitzing | 5:0251fde34cdc | 121 | PIDcontrol_M1.attach(&flag_M1_controller, TS); |
willem_hoitzing | 5:0251fde34cdc | 122 | PIDcontrol_M2.attach(&flag_M2_controller, TS); |
willem_hoitzing | 5:0251fde34cdc | 123 | |
willem_hoitzing | 2:0a976fb06ff8 | 124 | // initialize function |
willem_hoitzing | 5:0251fde34cdc | 125 | knop.fall(&initialize); |
willem_hoitzing | 5:0251fde34cdc | 126 | if (go_flag_initialize == true) { |
willem_hoitzing | 3:6ba52d1ae499 | 127 | go_flag_initialize = false; |
willem_hoitzing | 3:6ba52d1ae499 | 128 | initialize(); |
willem_hoitzing | 2:0a976fb06ff8 | 129 | } |
willem_hoitzing | 5:0251fde34cdc | 130 | |
willem_hoitzing | 5:0251fde34cdc | 131 | while(1) { |
willem_hoitzing | 5:0251fde34cdc | 132 | // update encoder |
willem_hoitzing | 5:0251fde34cdc | 133 | if (go_flag_update_encoder == true) { |
willem_hoitzing | 5:0251fde34cdc | 134 | go_flag_update_encoder = false; |
willem_hoitzing | 5:0251fde34cdc | 135 | update_encoder(); |
willem_hoitzing | 5:0251fde34cdc | 136 | } |
willem_hoitzing | 5:0251fde34cdc | 137 | // controller M1 |
willem_hoitzing | 5:0251fde34cdc | 138 | if (go_flag_M1_controller == true) { |
willem_hoitzing | 5:0251fde34cdc | 139 | go_flag_M1_controller = false; |
willem_hoitzing | 5:0251fde34cdc | 140 | M1_controller(); |
willem_hoitzing | 5:0251fde34cdc | 141 | } |
willem_hoitzing | 5:0251fde34cdc | 142 | // controller M2 |
willem_hoitzing | 5:0251fde34cdc | 143 | if (go_flag_M2_controller == true) { |
willem_hoitzing | 5:0251fde34cdc | 144 | go_flag_M2_controller = false; |
willem_hoitzing | 5:0251fde34cdc | 145 | M2_controller(); |
willem_hoitzing | 5:0251fde34cdc | 146 | } |
willem_hoitzing | 5:0251fde34cdc | 147 | } |
willem_hoitzing | 0:26ce65a63616 | 148 | } |