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@4:a5f3e1838e3e, 2016-10-20 (annotated)
- Committer:
- willem_hoitzing
- Date:
- Thu Oct 20 13:51:34 2016 +0000
- Revision:
- 4:a5f3e1838e3e
- Parent:
- 3:6ba52d1ae499
- Child:
- 5:0251fde34cdc
PID control - poging position control - langzaam
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 | 3:6ba52d1ae499 | 19 | |
willem_hoitzing | 3:6ba52d1ae499 | 20 | volatile bool go_flag_initialize = false; |
willem_hoitzing | 0:26ce65a63616 | 21 | |
willem_hoitzing | 3:6ba52d1ae499 | 22 | void flag_initialize() |
willem_hoitzing | 0:26ce65a63616 | 23 | { |
willem_hoitzing | 3:6ba52d1ae499 | 24 | go_flag_initialize = true; |
willem_hoitzing | 3:6ba52d1ae499 | 25 | } |
willem_hoitzing | 3:6ba52d1ae499 | 26 | |
willem_hoitzing | 4:a5f3e1838e3e | 27 | volatile double q1_ref; |
willem_hoitzing | 4:a5f3e1838e3e | 28 | volatile double q2_ref; |
willem_hoitzing | 3:6ba52d1ae499 | 29 | void initialize() |
willem_hoitzing | 3:6ba52d1ae499 | 30 | { |
willem_hoitzing | 4:a5f3e1838e3e | 31 | q1_ref = 2*3.1415 /(2*3.1415); |
willem_hoitzing | 4:a5f3e1838e3e | 32 | q2_ref = 3.1415 /(2*3.1415); |
willem_hoitzing | 2:0a976fb06ff8 | 33 | } |
willem_hoitzing | 0:26ce65a63616 | 34 | |
willem_hoitzing | 3:6ba52d1ae499 | 35 | const double TS = 0.02; |
willem_hoitzing | 4:a5f3e1838e3e | 36 | const double M1_Kp = 0.4086, M1_Ki = 0.0046, M1_Kd = 3.2710; |
willem_hoitzing | 4:a5f3e1838e3e | 37 | const double M2_Kp = 0.4086, M2_Ki = 0.0046, M2_Kd = 3.2710; |
willem_hoitzing | 4:a5f3e1838e3e | 38 | const double N = 3.0624; |
willem_hoitzing | 3:6ba52d1ae499 | 39 | |
willem_hoitzing | 3:6ba52d1ae499 | 40 | volatile double ctrlOutput_M1 = 0; |
willem_hoitzing | 3:6ba52d1ae499 | 41 | volatile double ctrlOutput_M2 = 0; |
willem_hoitzing | 3:6ba52d1ae499 | 42 | |
willem_hoitzing | 4:a5f3e1838e3e | 43 | Ticker update_encoder_ticker; |
willem_hoitzing | 4:a5f3e1838e3e | 44 | void update_encoder() |
willem_hoitzing | 2:0a976fb06ff8 | 45 | { |
willem_hoitzing | 4:a5f3e1838e3e | 46 | q1 = wheel_M1.getPulses()/(1334.355/2) /(2*3.1415); |
willem_hoitzing | 4:a5f3e1838e3e | 47 | q2 = wheel_M2.getPulses()/(1334.355/2) /(2*3.1415); |
willem_hoitzing | 4:a5f3e1838e3e | 48 | pc.printf("q1 = %f \tq1_ref = %f \tPID1 = %f \tq2 = %f \tq2_ref = %f \tPID2 = %f\n\r",q1, q1_ref, ctrlOutput_M1,q2,q2_ref,ctrlOutput_M2); |
willem_hoitzing | 2:0a976fb06ff8 | 49 | } |
willem_hoitzing | 2:0a976fb06ff8 | 50 | |
willem_hoitzing | 3:6ba52d1ae499 | 51 | BiQuad pidf_M1; |
willem_hoitzing | 3:6ba52d1ae499 | 52 | BiQuad pidf_M2; |
willem_hoitzing | 3:6ba52d1ae499 | 53 | Ticker PIDcontrol_M1; |
willem_hoitzing | 3:6ba52d1ae499 | 54 | Ticker PIDcontrol_M2; |
willem_hoitzing | 0:26ce65a63616 | 55 | |
willem_hoitzing | 2:0a976fb06ff8 | 56 | void M1_controller() |
willem_hoitzing | 2:0a976fb06ff8 | 57 | { |
willem_hoitzing | 4:a5f3e1838e3e | 58 | ctrlOutput_M1 = pidf_M1.step(q1_ref-q1); |
willem_hoitzing | 3:6ba52d1ae499 | 59 | |
willem_hoitzing | 3:6ba52d1ae499 | 60 | if (ctrlOutput_M1 < 0) |
willem_hoitzing | 3:6ba52d1ae499 | 61 | { |
willem_hoitzing | 3:6ba52d1ae499 | 62 | dir_M1 = 1; |
willem_hoitzing | 3:6ba52d1ae499 | 63 | } |
willem_hoitzing | 3:6ba52d1ae499 | 64 | else |
willem_hoitzing | 2:0a976fb06ff8 | 65 | { |
willem_hoitzing | 2:0a976fb06ff8 | 66 | dir_M1 = 0; |
willem_hoitzing | 2:0a976fb06ff8 | 67 | } |
willem_hoitzing | 3:6ba52d1ae499 | 68 | pwm_M1 = abs(ctrlOutput_M1); |
willem_hoitzing | 3:6ba52d1ae499 | 69 | } |
willem_hoitzing | 3:6ba52d1ae499 | 70 | |
willem_hoitzing | 3:6ba52d1ae499 | 71 | void M2_controller() |
willem_hoitzing | 3:6ba52d1ae499 | 72 | { |
willem_hoitzing | 4:a5f3e1838e3e | 73 | ctrlOutput_M2 = pidf_M2.step(q2_ref-q2); |
willem_hoitzing | 3:6ba52d1ae499 | 74 | |
willem_hoitzing | 3:6ba52d1ae499 | 75 | if (ctrlOutput_M2 < 0) |
willem_hoitzing | 3:6ba52d1ae499 | 76 | { |
willem_hoitzing | 3:6ba52d1ae499 | 77 | dir_M2 = 1; |
willem_hoitzing | 3:6ba52d1ae499 | 78 | } |
willem_hoitzing | 2:0a976fb06ff8 | 79 | else |
willem_hoitzing | 2:0a976fb06ff8 | 80 | { |
willem_hoitzing | 3:6ba52d1ae499 | 81 | dir_M2 = 0; |
willem_hoitzing | 2:0a976fb06ff8 | 82 | } |
willem_hoitzing | 3:6ba52d1ae499 | 83 | pwm_M2 = abs(ctrlOutput_M2); |
willem_hoitzing | 0:26ce65a63616 | 84 | } |
willem_hoitzing | 0:26ce65a63616 | 85 | |
willem_hoitzing | 0:26ce65a63616 | 86 | int main() |
willem_hoitzing | 0:26ce65a63616 | 87 | { |
willem_hoitzing | 2:0a976fb06ff8 | 88 | ledr = 1; |
willem_hoitzing | 0:26ce65a63616 | 89 | pc.baud(115200); |
willem_hoitzing | 3:6ba52d1ae499 | 90 | pidf_M1.PIDF(M1_Kp,M1_Ki,M1_Kd,N,TS); |
willem_hoitzing | 3:6ba52d1ae499 | 91 | pidf_M2.PIDF(M2_Kp,M2_Ki,M2_Kd,N,TS); |
willem_hoitzing | 4:a5f3e1838e3e | 92 | update_encoder_ticker.attach(&update_encoder, 0.02f); |
willem_hoitzing | 3:6ba52d1ae499 | 93 | PIDcontrol_M1.attach(&M1_controller, TS); |
willem_hoitzing | 3:6ba52d1ae499 | 94 | PIDcontrol_M2.attach(&M2_controller, TS); |
willem_hoitzing | 3:6ba52d1ae499 | 95 | flag_initialize(); |
willem_hoitzing | 2:0a976fb06ff8 | 96 | // initialize function |
willem_hoitzing | 3:6ba52d1ae499 | 97 | if (go_flag_initialize == true) |
willem_hoitzing | 2:0a976fb06ff8 | 98 | { |
willem_hoitzing | 3:6ba52d1ae499 | 99 | go_flag_initialize = false; |
willem_hoitzing | 3:6ba52d1ae499 | 100 | initialize(); |
willem_hoitzing | 2:0a976fb06ff8 | 101 | } |
willem_hoitzing | 3:6ba52d1ae499 | 102 | while(1){} |
willem_hoitzing | 0:26ce65a63616 | 103 | } |