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@6:4d254faf2428, 2016-10-24 (annotated)

Committer:
willem_hoitzing
Date:
Mon Oct 24 10:50:17 2016 +0000
Revision:
6:4d254faf2428
Parent:
5:0251fde34cdc
Child:
7:1444604f10d4
Position control - alleen Kp waarde - werkt?

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 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 6:4d254faf2428 27 volatile double q1_ref;
willem_hoitzing 6:4d254faf2428 28 volatile double q2_ref;
willem_hoitzing 3:6ba52d1ae499 29 void initialize()
willem_hoitzing 3:6ba52d1ae499 30 {
willem_hoitzing 6:4d254faf2428 31 q1_ref = 0.125; //2*3.1415 /(2*3.1415);
willem_hoitzing 6:4d254faf2428 32 q2_ref = 0*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 6:4d254faf2428 36 const double M1_Kp = 1, M1_Ki = 0.00, M1_Kd = 0;
willem_hoitzing 5:0251fde34cdc 37 const double M2_Kp = 0.3, M2_Ki = 0.00, M2_Kd = 0;
willem_hoitzing 5:0251fde34cdc 38 const double N = 0;
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 5:0251fde34cdc 44 volatile bool go_flag_update_encoder = false;
willem_hoitzing 5:0251fde34cdc 45 void flag_update_encoder()
willem_hoitzing 5:0251fde34cdc 46 {
willem_hoitzing 5:0251fde34cdc 47 go_flag_update_encoder = true;
willem_hoitzing 5:0251fde34cdc 48 }
willem_hoitzing 5:0251fde34cdc 49
willem_hoitzing 4:a5f3e1838e3e 50 void update_encoder()
willem_hoitzing 2:0a976fb06ff8 51 {
willem_hoitzing 5:0251fde34cdc 52 q1 = wheel_M1.getPulses()/(1334.355/2);
willem_hoitzing 5:0251fde34cdc 53 q2 = wheel_M2.getPulses()/(1334.355/2);
willem_hoitzing 6:4d254faf2428 54 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 55 }
willem_hoitzing 2:0a976fb06ff8 56
willem_hoitzing 3:6ba52d1ae499 57 BiQuad pidf_M1;
willem_hoitzing 3:6ba52d1ae499 58 BiQuad pidf_M2;
willem_hoitzing 3:6ba52d1ae499 59 Ticker PIDcontrol_M1;
willem_hoitzing 3:6ba52d1ae499 60 Ticker PIDcontrol_M2;
willem_hoitzing 5:0251fde34cdc 61 volatile bool go_flag_M1_controller = false;
willem_hoitzing 5:0251fde34cdc 62 volatile bool go_flag_M2_controller = false;
willem_hoitzing 5:0251fde34cdc 63
willem_hoitzing 5:0251fde34cdc 64 void flag_M1_controller()
willem_hoitzing 5:0251fde34cdc 65 {
willem_hoitzing 5:0251fde34cdc 66 go_flag_M1_controller = true;
willem_hoitzing 5:0251fde34cdc 67 }
willem_hoitzing 5:0251fde34cdc 68
willem_hoitzing 5:0251fde34cdc 69 void flag_M2_controller()
willem_hoitzing 5:0251fde34cdc 70 {
willem_hoitzing 5:0251fde34cdc 71 go_flag_M2_controller = true;
willem_hoitzing 5:0251fde34cdc 72 }
willem_hoitzing 0:26ce65a63616 73
willem_hoitzing 2:0a976fb06ff8 74 void M1_controller()
willem_hoitzing 2:0a976fb06ff8 75 {
willem_hoitzing 6:4d254faf2428 76 ctrlOutput_M1 = pidf_M1.step(q1_ref-q1);
willem_hoitzing 5:0251fde34cdc 77
willem_hoitzing 5:0251fde34cdc 78 if (ctrlOutput_M1 < 0) {
willem_hoitzing 3:6ba52d1ae499 79 dir_M1 = 1;
willem_hoitzing 5:0251fde34cdc 80 } else {
willem_hoitzing 2:0a976fb06ff8 81 dir_M1 = 0;
willem_hoitzing 2:0a976fb06ff8 82 }
willem_hoitzing 6:4d254faf2428 83 pwm_M1 = abs(ctrlOutput_M1);
willem_hoitzing 3:6ba52d1ae499 84 }
willem_hoitzing 3:6ba52d1ae499 85
willem_hoitzing 3:6ba52d1ae499 86 void M2_controller()
willem_hoitzing 3:6ba52d1ae499 87 {
willem_hoitzing 6:4d254faf2428 88 ctrlOutput_M2 = pidf_M2.step(q2_ref-q2);
willem_hoitzing 5:0251fde34cdc 89
willem_hoitzing 5:0251fde34cdc 90 if (ctrlOutput_M2 < 0) {
willem_hoitzing 3:6ba52d1ae499 91 dir_M2 = 1;
willem_hoitzing 5:0251fde34cdc 92 } else {
willem_hoitzing 3:6ba52d1ae499 93 dir_M2 = 0;
willem_hoitzing 2:0a976fb06ff8 94 }
willem_hoitzing 6:4d254faf2428 95 pwm_M2 = abs(ctrlOutput_M2);
willem_hoitzing 0:26ce65a63616 96 }
willem_hoitzing 0:26ce65a63616 97
willem_hoitzing 0:26ce65a63616 98 int main()
willem_hoitzing 0:26ce65a63616 99 {
willem_hoitzing 0:26ce65a63616 100 pc.baud(115200);
willem_hoitzing 5:0251fde34cdc 101 wheel_M1.reset();
willem_hoitzing 5:0251fde34cdc 102 wheel_M2.reset();
willem_hoitzing 3:6ba52d1ae499 103 pidf_M1.PIDF(M1_Kp,M1_Ki,M1_Kd,N,TS);
willem_hoitzing 3:6ba52d1ae499 104 pidf_M2.PIDF(M2_Kp,M2_Ki,M2_Kd,N,TS);
willem_hoitzing 5:0251fde34cdc 105 // flag functions/tickers
willem_hoitzing 5:0251fde34cdc 106 update_encoder_ticker.attach(&flag_update_encoder, 0.02f);
willem_hoitzing 5:0251fde34cdc 107 PIDcontrol_M1.attach(&flag_M1_controller, TS);
willem_hoitzing 5:0251fde34cdc 108 PIDcontrol_M2.attach(&flag_M2_controller, TS);
willem_hoitzing 5:0251fde34cdc 109
willem_hoitzing 2:0a976fb06ff8 110 // initialize function
willem_hoitzing 6:4d254faf2428 111 knop.fall(&flag_initialize);
willem_hoitzing 5:0251fde34cdc 112 if (go_flag_initialize == true) {
willem_hoitzing 3:6ba52d1ae499 113 go_flag_initialize = false;
willem_hoitzing 3:6ba52d1ae499 114 initialize();
willem_hoitzing 2:0a976fb06ff8 115 }
willem_hoitzing 5:0251fde34cdc 116
willem_hoitzing 5:0251fde34cdc 117 while(1) {
willem_hoitzing 5:0251fde34cdc 118 // update encoder
willem_hoitzing 5:0251fde34cdc 119 if (go_flag_update_encoder == true) {
willem_hoitzing 5:0251fde34cdc 120 go_flag_update_encoder = false;
willem_hoitzing 5:0251fde34cdc 121 update_encoder();
willem_hoitzing 5:0251fde34cdc 122 }
willem_hoitzing 5:0251fde34cdc 123 // controller M1
willem_hoitzing 5:0251fde34cdc 124 if (go_flag_M1_controller == true) {
willem_hoitzing 5:0251fde34cdc 125 go_flag_M1_controller = false;
willem_hoitzing 5:0251fde34cdc 126 M1_controller();
willem_hoitzing 5:0251fde34cdc 127 }
willem_hoitzing 5:0251fde34cdc 128 // controller M2
willem_hoitzing 5:0251fde34cdc 129 if (go_flag_M2_controller == true) {
willem_hoitzing 5:0251fde34cdc 130 go_flag_M2_controller = false;
willem_hoitzing 5:0251fde34cdc 131 M2_controller();
willem_hoitzing 5:0251fde34cdc 132 }
willem_hoitzing 5:0251fde34cdc 133 }
willem_hoitzing 0:26ce65a63616 134 }