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

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