Willem Hoitzing / Mbed 2 deprecated PID_controller

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?

UserRevisionLine numberNew 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 }