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@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?

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