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@3:6ba52d1ae499, 2016-10-20 (annotated)
- Committer:
- willem_hoitzing
- Date:
- Thu Oct 20 13:07:52 2016 +0000
- Revision:
- 3:6ba52d1ae499
- Parent:
- 2:0a976fb06ff8
- Child:
- 4:a5f3e1838e3e
PID controller - velocity control - werkt niet goed (afgeleide voor hoeksnelheid geeft error?); nu/hierna proberen position control
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 | 2:0a976fb06ff8 | 15 | DigitalOut ledr (LED_RED); |
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 | 0:26ce65a63616 | 19 | volatile double q1_prev = 0; |
willem_hoitzing | 0:26ce65a63616 | 20 | volatile double q2_prev = 0; |
willem_hoitzing | 3:6ba52d1ae499 | 21 | volatile double q1_v_ref = 0; |
willem_hoitzing | 3:6ba52d1ae499 | 22 | volatile double q2_v_ref = 0; |
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 | 3:6ba52d1ae499 | 31 | void initialize() |
willem_hoitzing | 3:6ba52d1ae499 | 32 | { |
willem_hoitzing | 3:6ba52d1ae499 | 33 | q1_v_ref = 0.2; // 0.6 max |
willem_hoitzing | 3:6ba52d1ae499 | 34 | q2_v_ref = 0.45; |
willem_hoitzing | 2:0a976fb06ff8 | 35 | } |
willem_hoitzing | 0:26ce65a63616 | 36 | |
willem_hoitzing | 3:6ba52d1ae499 | 37 | const double TS = 0.02; |
willem_hoitzing | 3:6ba52d1ae499 | 38 | const double M1_Kp = 4.348, M1_Ki = 36.632, M1_Kd = 0.126; |
willem_hoitzing | 3:6ba52d1ae499 | 39 | const double M2_Kp = 4.348, M2_Ki = 36.632, M2_Kd = 0.126; |
willem_hoitzing | 3:6ba52d1ae499 | 40 | const double N = 25; |
willem_hoitzing | 3:6ba52d1ae499 | 41 | |
willem_hoitzing | 3:6ba52d1ae499 | 42 | volatile double velocity_M1; |
willem_hoitzing | 3:6ba52d1ae499 | 43 | volatile double velocity_M2; |
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 | 3:6ba52d1ae499 | 47 | Ticker update_velocity_ticker; |
willem_hoitzing | 3:6ba52d1ae499 | 48 | void update_velocity() |
willem_hoitzing | 2:0a976fb06ff8 | 49 | { |
willem_hoitzing | 3:6ba52d1ae499 | 50 | double q1 = wheel_M1.getPulses()/(1334.355/2); |
willem_hoitzing | 3:6ba52d1ae499 | 51 | velocity_M1 = ((q1 - q1_prev) / TS)/5.0265; |
willem_hoitzing | 3:6ba52d1ae499 | 52 | q1_prev = q1; |
willem_hoitzing | 3:6ba52d1ae499 | 53 | double q2 = wheel_M2.getPulses()/(1334.355/2); |
willem_hoitzing | 3:6ba52d1ae499 | 54 | velocity_M2 = ((q2 - q2_prev) / TS)/5.0265; |
willem_hoitzing | 3:6ba52d1ae499 | 55 | q2_prev = q2; |
willem_hoitzing | 3:6ba52d1ae499 | 56 | pc.printf("q1_v = %f \tq1_v_ref = %f \tpwm_M1 = %f \tctrlM1 = %f \tq2_v = %f \tq2_v_ref = %f \tpwm_M2 = %f \tctrlM2 = %f\n\r",velocity_M1,q1_v_ref,pwm_M1.read(),ctrlOutput_M1,velocity_M2,q2_v_ref,pwm_M2.read(),ctrlOutput_M2); |
willem_hoitzing | 2:0a976fb06ff8 | 57 | } |
willem_hoitzing | 2:0a976fb06ff8 | 58 | |
willem_hoitzing | 3:6ba52d1ae499 | 59 | BiQuad pidf_M1; |
willem_hoitzing | 3:6ba52d1ae499 | 60 | BiQuad pidf_M2; |
willem_hoitzing | 3:6ba52d1ae499 | 61 | Ticker PIDcontrol_M1; |
willem_hoitzing | 3:6ba52d1ae499 | 62 | Ticker PIDcontrol_M2; |
willem_hoitzing | 0:26ce65a63616 | 63 | |
willem_hoitzing | 2:0a976fb06ff8 | 64 | void M1_controller() |
willem_hoitzing | 2:0a976fb06ff8 | 65 | { |
willem_hoitzing | 3:6ba52d1ae499 | 66 | ctrlOutput_M1 = pidf_M1.step(q1_v_ref-velocity_M1); |
willem_hoitzing | 3:6ba52d1ae499 | 67 | |
willem_hoitzing | 3:6ba52d1ae499 | 68 | if (ctrlOutput_M1 < 0) |
willem_hoitzing | 3:6ba52d1ae499 | 69 | { |
willem_hoitzing | 3:6ba52d1ae499 | 70 | dir_M1 = 1; |
willem_hoitzing | 3:6ba52d1ae499 | 71 | } |
willem_hoitzing | 3:6ba52d1ae499 | 72 | else |
willem_hoitzing | 2:0a976fb06ff8 | 73 | { |
willem_hoitzing | 2:0a976fb06ff8 | 74 | dir_M1 = 0; |
willem_hoitzing | 2:0a976fb06ff8 | 75 | } |
willem_hoitzing | 3:6ba52d1ae499 | 76 | pwm_M1 = abs(ctrlOutput_M1); |
willem_hoitzing | 3:6ba52d1ae499 | 77 | } |
willem_hoitzing | 3:6ba52d1ae499 | 78 | |
willem_hoitzing | 3:6ba52d1ae499 | 79 | void M2_controller() |
willem_hoitzing | 3:6ba52d1ae499 | 80 | { |
willem_hoitzing | 3:6ba52d1ae499 | 81 | ctrlOutput_M2 = pidf_M2.step(q2_v_ref-velocity_M2); |
willem_hoitzing | 3:6ba52d1ae499 | 82 | |
willem_hoitzing | 3:6ba52d1ae499 | 83 | if (ctrlOutput_M2 < 0) |
willem_hoitzing | 3:6ba52d1ae499 | 84 | { |
willem_hoitzing | 3:6ba52d1ae499 | 85 | dir_M2 = 1; |
willem_hoitzing | 3:6ba52d1ae499 | 86 | } |
willem_hoitzing | 2:0a976fb06ff8 | 87 | else |
willem_hoitzing | 2:0a976fb06ff8 | 88 | { |
willem_hoitzing | 3:6ba52d1ae499 | 89 | dir_M2 = 0; |
willem_hoitzing | 2:0a976fb06ff8 | 90 | } |
willem_hoitzing | 3:6ba52d1ae499 | 91 | pwm_M2 = abs(ctrlOutput_M2); |
willem_hoitzing | 0:26ce65a63616 | 92 | } |
willem_hoitzing | 0:26ce65a63616 | 93 | |
willem_hoitzing | 0:26ce65a63616 | 94 | int main() |
willem_hoitzing | 0:26ce65a63616 | 95 | { |
willem_hoitzing | 2:0a976fb06ff8 | 96 | ledr = 1; |
willem_hoitzing | 0:26ce65a63616 | 97 | pc.baud(115200); |
willem_hoitzing | 3:6ba52d1ae499 | 98 | pidf_M1.PIDF(M1_Kp,M1_Ki,M1_Kd,N,TS); |
willem_hoitzing | 3:6ba52d1ae499 | 99 | pidf_M2.PIDF(M2_Kp,M2_Ki,M2_Kd,N,TS); |
willem_hoitzing | 3:6ba52d1ae499 | 100 | update_velocity_ticker.attach(&update_velocity, 0.02f); |
willem_hoitzing | 3:6ba52d1ae499 | 101 | PIDcontrol_M1.attach(&M1_controller, TS); |
willem_hoitzing | 3:6ba52d1ae499 | 102 | PIDcontrol_M2.attach(&M2_controller, TS); |
willem_hoitzing | 3:6ba52d1ae499 | 103 | flag_initialize(); |
willem_hoitzing | 2:0a976fb06ff8 | 104 | // initialize function |
willem_hoitzing | 3:6ba52d1ae499 | 105 | if (go_flag_initialize == true) |
willem_hoitzing | 2:0a976fb06ff8 | 106 | { |
willem_hoitzing | 3:6ba52d1ae499 | 107 | go_flag_initialize = false; |
willem_hoitzing | 3:6ba52d1ae499 | 108 | initialize(); |
willem_hoitzing | 2:0a976fb06ff8 | 109 | } |
willem_hoitzing | 3:6ba52d1ae499 | 110 | while(1){} |
willem_hoitzing | 0:26ce65a63616 | 111 | } |