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@8:008a7bf80fa0, 2016-10-26 (annotated)

Committer:
willem_hoitzing
Date:
Wed Oct 26 07:59:23 2016 +0000
Revision:
8:008a7bf80fa0
Parent:
7:1444604f10d4
Child:
9:334b1596637b
PID Controller biquad vorm

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 7:1444604f10d4 15
willem_hoitzing 7:1444604f10d4 16 DigitalOut ledg (LED_GREEN);
willem_hoitzing 7:1444604f10d4 17 DigitalOut ledr (LED_RED);
willem_hoitzing 7:1444604f10d4 18 DigitalOut ledb (LED_BLUE);
willem_hoitzing 7:1444604f10d4 19 InterruptIn knop_biceps(SW2);
willem_hoitzing 7:1444604f10d4 20 InterruptIn knop_triceps(SW3);
willem_hoitzing 7:1444604f10d4 21 InterruptIn knop_switch(D9);
willem_hoitzing 0:26ce65a63616 22
willem_hoitzing 2:0a976fb06ff8 23 volatile double q1 = 0;
willem_hoitzing 2:0a976fb06ff8 24 volatile double q2 = 0;
willem_hoitzing 7:1444604f10d4 25 volatile double q1_begin;
willem_hoitzing 7:1444604f10d4 26 volatile double q2_begin;
willem_hoitzing 7:1444604f10d4 27 volatile double l1 = 0.3626;
willem_hoitzing 7:1444604f10d4 28 volatile double l2 = 0.420;
willem_hoitzing 7:1444604f10d4 29 volatile double q1_v;
willem_hoitzing 7:1444604f10d4 30 volatile double q2_v;
willem_hoitzing 6:4d254faf2428 31 volatile double q1_ref;
willem_hoitzing 6:4d254faf2428 32 volatile double q2_ref;
willem_hoitzing 7:1444604f10d4 33 volatile double ctrlOutput_M1 = 0;
willem_hoitzing 7:1444604f10d4 34 volatile double ctrlOutput_M2 = 0;
willem_hoitzing 7:1444604f10d4 35 volatile double vx;
willem_hoitzing 7:1444604f10d4 36 volatile double vy;
willem_hoitzing 7:1444604f10d4 37 volatile bool translatie_richting = true; //true is verticaal, false is horizontaal
willem_hoitzing 0:26ce65a63616 38
willem_hoitzing 3:6ba52d1ae499 39 const double TS = 0.02;
willem_hoitzing 8:008a7bf80fa0 40 const double M1_Kp = 0.5, M1_Ki = 0.00, M1_Kd = 0; // Waardes vinden?
willem_hoitzing 8:008a7bf80fa0 41 const double M2_Kp = 0.5, M2_Ki = 0.00, M2_Kd = 0;
willem_hoitzing 7:1444604f10d4 42 const double N = 0.1;
willem_hoitzing 3:6ba52d1ae499 43
willem_hoitzing 7:1444604f10d4 44 Ticker begin_waarde_q;
willem_hoitzing 7:1444604f10d4 45 int n = 0;
willem_hoitzing 7:1444604f10d4 46 void begin_waarde()
willem_hoitzing 7:1444604f10d4 47 {
willem_hoitzing 7:1444604f10d4 48 n++;
willem_hoitzing 7:1444604f10d4 49 if(n == 2)
willem_hoitzing 7:1444604f10d4 50 {
willem_hoitzing 7:1444604f10d4 51 q1_begin = wheel_M1.getPulses() / (1334.355/2);
willem_hoitzing 7:1444604f10d4 52 q2_begin = wheel_M2.getPulses() / (1334.355/2);
willem_hoitzing 7:1444604f10d4 53 begin_waarde_q.detach();
willem_hoitzing 7:1444604f10d4 54 }
willem_hoitzing 7:1444604f10d4 55 }
willem_hoitzing 3:6ba52d1ae499 56
willem_hoitzing 4:a5f3e1838e3e 57 Ticker update_encoder_ticker;
willem_hoitzing 5:0251fde34cdc 58 volatile bool go_flag_update_encoder = false;
willem_hoitzing 5:0251fde34cdc 59 void flag_update_encoder()
willem_hoitzing 5:0251fde34cdc 60 {
willem_hoitzing 5:0251fde34cdc 61 go_flag_update_encoder = true;
willem_hoitzing 5:0251fde34cdc 62 }
willem_hoitzing 5:0251fde34cdc 63
willem_hoitzing 4:a5f3e1838e3e 64 void update_encoder()
willem_hoitzing 2:0a976fb06ff8 65 {
willem_hoitzing 5:0251fde34cdc 66 q1 = wheel_M1.getPulses()/(1334.355/2);
willem_hoitzing 5:0251fde34cdc 67 q2 = wheel_M2.getPulses()/(1334.355/2);
willem_hoitzing 7:1444604f10d4 68 pc.printf("q1 = %f \tq1_ref = %f \tPID1 = %f \tq2 = %f \tq2_ref = %f \tPID2 = %f \tq1_v=%f \tq2_v=%f\n\r",q1, q1_ref, ctrlOutput_M1,q2,q2_ref,ctrlOutput_M2,q1_v,q2_v);
willem_hoitzing 7:1444604f10d4 69 }
willem_hoitzing 7:1444604f10d4 70
willem_hoitzing 7:1444604f10d4 71 volatile bool go_flag_initialize = false;
willem_hoitzing 7:1444604f10d4 72
willem_hoitzing 7:1444604f10d4 73 void flag_initialize()
willem_hoitzing 7:1444604f10d4 74 {
willem_hoitzing 7:1444604f10d4 75 go_flag_initialize = true;
willem_hoitzing 7:1444604f10d4 76 }
willem_hoitzing 7:1444604f10d4 77
willem_hoitzing 7:1444604f10d4 78 void initialize()
willem_hoitzing 7:1444604f10d4 79 {
willem_hoitzing 7:1444604f10d4 80 q1_ref = 15*2*3.1415/360;
willem_hoitzing 7:1444604f10d4 81 q2_ref = -30*2*3.1415/360;
willem_hoitzing 7:1444604f10d4 82 }
willem_hoitzing 7:1444604f10d4 83
willem_hoitzing 7:1444604f10d4 84 void biceps()
willem_hoitzing 7:1444604f10d4 85 {
willem_hoitzing 7:1444604f10d4 86 if (translatie_richting) { // verticaal / up
willem_hoitzing 7:1444604f10d4 87 vx = 0;
willem_hoitzing 7:1444604f10d4 88 vy = 0.02;
willem_hoitzing 7:1444604f10d4 89 } else { // horizontaal / right
willem_hoitzing 7:1444604f10d4 90 vx = 0.02;
willem_hoitzing 7:1444604f10d4 91 vy = 0;
willem_hoitzing 7:1444604f10d4 92 }
willem_hoitzing 7:1444604f10d4 93 }
willem_hoitzing 7:1444604f10d4 94
willem_hoitzing 7:1444604f10d4 95 void triceps()
willem_hoitzing 7:1444604f10d4 96 {
willem_hoitzing 7:1444604f10d4 97 if (translatie_richting) { // verticaal / down
willem_hoitzing 7:1444604f10d4 98 vx = 0;
willem_hoitzing 7:1444604f10d4 99 vy = -0.02;
willem_hoitzing 7:1444604f10d4 100 } else { // horizontaal / left
willem_hoitzing 7:1444604f10d4 101 vx = -0.02;
willem_hoitzing 7:1444604f10d4 102 vy = 0;
willem_hoitzing 7:1444604f10d4 103 }
willem_hoitzing 7:1444604f10d4 104
willem_hoitzing 7:1444604f10d4 105 }
willem_hoitzing 7:1444604f10d4 106
willem_hoitzing 7:1444604f10d4 107 void switcher()
willem_hoitzing 7:1444604f10d4 108 {
willem_hoitzing 7:1444604f10d4 109 if ( (vx == 0) && (vy == 0) ) {
willem_hoitzing 7:1444604f10d4 110 translatie_richting = !translatie_richting;
willem_hoitzing 7:1444604f10d4 111 } else {
willem_hoitzing 7:1444604f10d4 112 vx = 0;
willem_hoitzing 7:1444604f10d4 113 vy = 0;
willem_hoitzing 7:1444604f10d4 114 }
willem_hoitzing 7:1444604f10d4 115
willem_hoitzing 7:1444604f10d4 116 if (translatie_richting == 1) {
willem_hoitzing 7:1444604f10d4 117 ledr = 1; // blauw - verticaal
willem_hoitzing 7:1444604f10d4 118 ledg = 1;
willem_hoitzing 7:1444604f10d4 119 ledb = 0;
willem_hoitzing 7:1444604f10d4 120 } else {
willem_hoitzing 7:1444604f10d4 121 ledr = 0; // rood - horizontaal
willem_hoitzing 7:1444604f10d4 122 ledg = 1;
willem_hoitzing 7:1444604f10d4 123 ledb = 1;
willem_hoitzing 7:1444604f10d4 124 }
willem_hoitzing 7:1444604f10d4 125 }
willem_hoitzing 7:1444604f10d4 126
willem_hoitzing 7:1444604f10d4 127 Ticker update_ref_ticker;
willem_hoitzing 7:1444604f10d4 128 volatile double J_1;
willem_hoitzing 7:1444604f10d4 129 volatile double J_2;
willem_hoitzing 7:1444604f10d4 130 volatile double J_3;
willem_hoitzing 7:1444604f10d4 131 volatile double J_4;
willem_hoitzing 7:1444604f10d4 132 volatile bool go_flag_update_ref = false;
willem_hoitzing 7:1444604f10d4 133 void flag_update_ref()
willem_hoitzing 7:1444604f10d4 134 {
willem_hoitzing 7:1444604f10d4 135 go_flag_update_ref = true;
willem_hoitzing 7:1444604f10d4 136 }
willem_hoitzing 7:1444604f10d4 137
willem_hoitzing 7:1444604f10d4 138 void update_ref()
willem_hoitzing 7:1444604f10d4 139 {
willem_hoitzing 7:1444604f10d4 140 q1 = wheel_M1.getPulses() / (1334.355/2); // rad
willem_hoitzing 7:1444604f10d4 141 q2 = wheel_M2.getPulses() / (1334.355/2);
willem_hoitzing 7:1444604f10d4 142
willem_hoitzing 7:1444604f10d4 143 J_1 = -(l2*sin(q1 + q2))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
willem_hoitzing 7:1444604f10d4 144 J_2 = (l2*cos(q1 + q2))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
willem_hoitzing 7:1444604f10d4 145 J_3 = (l2*sin(q1 + q2) + l1*sin(q1))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
willem_hoitzing 7:1444604f10d4 146 J_4 = -(l2*cos(q1 + q2) + l1*cos(q1))/(l2*sin(q1 + q2)*(l2*cos(q1 + q2) + l1*cos(q1)) - l2*cos(q1 + q2)*(l2*sin(q1 + q2) + l1*sin(q1)));
willem_hoitzing 7:1444604f10d4 147
willem_hoitzing 7:1444604f10d4 148 q1_v = J_1 * vx + J_2 * vy;
willem_hoitzing 7:1444604f10d4 149 q2_v = J_3 * vx + J_4 * vy;
willem_hoitzing 7:1444604f10d4 150
willem_hoitzing 8:008a7bf80fa0 151 q1_ref = q1 + q1_v*TS;
willem_hoitzing 8:008a7bf80fa0 152 q2_ref = q2 + q2_v*TS;
willem_hoitzing 8:008a7bf80fa0 153
willem_hoitzing 7:1444604f10d4 154 if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) { // WAARDES VINDEN 0.8726 (50 graden)
willem_hoitzing 7:1444604f10d4 155 q1_v = 0;
willem_hoitzing 7:1444604f10d4 156 q2_v = 0;
willem_hoitzing 7:1444604f10d4 157 } else if ( (q1 < -(90*2*3.1415/360)) && (q1_v < 0) ) {
willem_hoitzing 7:1444604f10d4 158 q1_v = 0;
willem_hoitzing 7:1444604f10d4 159 q2_v = 0;
willem_hoitzing 7:1444604f10d4 160 } else if ( (q2 < (-140*2*3.1415/360)) && (q2_v < 0) ) { // WAARDES VINDEN -2.4434 (-140 graden) --> werkelijke max -2.672452
willem_hoitzing 7:1444604f10d4 161 q1_v = 0;
willem_hoitzing 7:1444604f10d4 162 q2_v = 0;
willem_hoitzing 7:1444604f10d4 163 } else if ( (q2 > 0) && (q2_v > 0) ) {
willem_hoitzing 7:1444604f10d4 164 q1_v = 0;
willem_hoitzing 7:1444604f10d4 165 q2_v = 0;
willem_hoitzing 7:1444604f10d4 166 }
willem_hoitzing 2:0a976fb06ff8 167 }
willem_hoitzing 2:0a976fb06ff8 168
willem_hoitzing 3:6ba52d1ae499 169 BiQuad pidf_M1;
willem_hoitzing 3:6ba52d1ae499 170 BiQuad pidf_M2;
willem_hoitzing 3:6ba52d1ae499 171 Ticker PIDcontrol_M1;
willem_hoitzing 3:6ba52d1ae499 172 Ticker PIDcontrol_M2;
willem_hoitzing 5:0251fde34cdc 173 volatile bool go_flag_M1_controller = false;
willem_hoitzing 5:0251fde34cdc 174 volatile bool go_flag_M2_controller = false;
willem_hoitzing 5:0251fde34cdc 175
willem_hoitzing 5:0251fde34cdc 176 void flag_M1_controller()
willem_hoitzing 5:0251fde34cdc 177 {
willem_hoitzing 5:0251fde34cdc 178 go_flag_M1_controller = true;
willem_hoitzing 5:0251fde34cdc 179 }
willem_hoitzing 5:0251fde34cdc 180
willem_hoitzing 5:0251fde34cdc 181 void flag_M2_controller()
willem_hoitzing 5:0251fde34cdc 182 {
willem_hoitzing 5:0251fde34cdc 183 go_flag_M2_controller = true;
willem_hoitzing 5:0251fde34cdc 184 }
willem_hoitzing 0:26ce65a63616 185
willem_hoitzing 2:0a976fb06ff8 186 void M1_controller()
willem_hoitzing 2:0a976fb06ff8 187 {
willem_hoitzing 6:4d254faf2428 188 ctrlOutput_M1 = pidf_M1.step(q1_ref-q1);
willem_hoitzing 5:0251fde34cdc 189
willem_hoitzing 5:0251fde34cdc 190 if (ctrlOutput_M1 < 0) {
willem_hoitzing 3:6ba52d1ae499 191 dir_M1 = 1;
willem_hoitzing 5:0251fde34cdc 192 } else {
willem_hoitzing 2:0a976fb06ff8 193 dir_M1 = 0;
willem_hoitzing 2:0a976fb06ff8 194 }
willem_hoitzing 6:4d254faf2428 195 pwm_M1 = abs(ctrlOutput_M1);
willem_hoitzing 8:008a7bf80fa0 196 if (pwm_M1 < 0) {
willem_hoitzing 8:008a7bf80fa0 197 pwm_M1 = 0;
willem_hoitzing 8:008a7bf80fa0 198 //} else {
willem_hoitzing 8:008a7bf80fa0 199 // pwm_M1 = pwm_M1 + 0.1;
willem_hoitzing 8:008a7bf80fa0 200 }
willem_hoitzing 3:6ba52d1ae499 201 }
willem_hoitzing 3:6ba52d1ae499 202
willem_hoitzing 3:6ba52d1ae499 203 void M2_controller()
willem_hoitzing 3:6ba52d1ae499 204 {
willem_hoitzing 6:4d254faf2428 205 ctrlOutput_M2 = pidf_M2.step(q2_ref-q2);
willem_hoitzing 5:0251fde34cdc 206
willem_hoitzing 5:0251fde34cdc 207 if (ctrlOutput_M2 < 0) {
willem_hoitzing 3:6ba52d1ae499 208 dir_M2 = 1;
willem_hoitzing 5:0251fde34cdc 209 } else {
willem_hoitzing 3:6ba52d1ae499 210 dir_M2 = 0;
willem_hoitzing 2:0a976fb06ff8 211 }
willem_hoitzing 6:4d254faf2428 212 pwm_M2 = abs(ctrlOutput_M2);
willem_hoitzing 8:008a7bf80fa0 213 if (pwm_M2 < 0) {
willem_hoitzing 8:008a7bf80fa0 214 pwm_M2 = 0;
willem_hoitzing 8:008a7bf80fa0 215 //} else {
willem_hoitzing 8:008a7bf80fa0 216 // pwm_M2 = pwm_M2 + 0.1;
willem_hoitzing 8:008a7bf80fa0 217 }
willem_hoitzing 0:26ce65a63616 218 }
willem_hoitzing 0:26ce65a63616 219
willem_hoitzing 0:26ce65a63616 220 int main()
willem_hoitzing 0:26ce65a63616 221 {
willem_hoitzing 0:26ce65a63616 222 pc.baud(115200);
willem_hoitzing 5:0251fde34cdc 223 wheel_M1.reset();
willem_hoitzing 5:0251fde34cdc 224 wheel_M2.reset();
willem_hoitzing 3:6ba52d1ae499 225 pidf_M1.PIDF(M1_Kp,M1_Ki,M1_Kd,N,TS);
willem_hoitzing 3:6ba52d1ae499 226 pidf_M2.PIDF(M2_Kp,M2_Ki,M2_Kd,N,TS);
willem_hoitzing 7:1444604f10d4 227 knop_biceps.rise(&biceps);
willem_hoitzing 7:1444604f10d4 228 knop_triceps.rise(&triceps);
willem_hoitzing 7:1444604f10d4 229 knop_switch.rise(&switcher);
willem_hoitzing 7:1444604f10d4 230
willem_hoitzing 5:0251fde34cdc 231 // flag functions/tickers
willem_hoitzing 7:1444604f10d4 232 update_encoder_ticker.attach(&flag_update_encoder, TS);
willem_hoitzing 7:1444604f10d4 233 update_ref_ticker.attach(&flag_update_ref, TS);
willem_hoitzing 5:0251fde34cdc 234 PIDcontrol_M1.attach(&flag_M1_controller, TS);
willem_hoitzing 5:0251fde34cdc 235 PIDcontrol_M2.attach(&flag_M2_controller, TS);
willem_hoitzing 7:1444604f10d4 236 begin_waarde_q.attach(&begin_waarde, 3);
willem_hoitzing 5:0251fde34cdc 237
willem_hoitzing 8:008a7bf80fa0 238 pc.printf("pidf_M1 is %s", pidf_M1.stable() ? "stable" : "unstable");
willem_hoitzing 8:008a7bf80fa0 239 pc.printf("pidf_M2 is %s", pidf_M2.stable() ? "stable" : "unstable");
willem_hoitzing 8:008a7bf80fa0 240
willem_hoitzing 5:0251fde34cdc 241 while(1) {
willem_hoitzing 7:1444604f10d4 242
willem_hoitzing 7:1444604f10d4 243 // initialize function
willem_hoitzing 7:1444604f10d4 244 initialize();
willem_hoitzing 7:1444604f10d4 245 if (go_flag_initialize == true) {
willem_hoitzing 7:1444604f10d4 246 go_flag_initialize = false;
willem_hoitzing 7:1444604f10d4 247 initialize();
willem_hoitzing 7:1444604f10d4 248 }
willem_hoitzing 5:0251fde34cdc 249 // update encoder
willem_hoitzing 5:0251fde34cdc 250 if (go_flag_update_encoder == true) {
willem_hoitzing 5:0251fde34cdc 251 go_flag_update_encoder = false;
willem_hoitzing 5:0251fde34cdc 252 update_encoder();
willem_hoitzing 5:0251fde34cdc 253 }
willem_hoitzing 7:1444604f10d4 254 // update joint positions/velocities
willem_hoitzing 7:1444604f10d4 255 if (go_flag_update_ref == true) {
willem_hoitzing 7:1444604f10d4 256 go_flag_update_ref = false;
willem_hoitzing 7:1444604f10d4 257 update_ref();
willem_hoitzing 7:1444604f10d4 258 }
willem_hoitzing 5:0251fde34cdc 259 // controller M1
willem_hoitzing 5:0251fde34cdc 260 if (go_flag_M1_controller == true) {
willem_hoitzing 5:0251fde34cdc 261 go_flag_M1_controller = false;
willem_hoitzing 5:0251fde34cdc 262 M1_controller();
willem_hoitzing 7:1444604f10d4 263 }
willem_hoitzing 5:0251fde34cdc 264 // controller M2
willem_hoitzing 5:0251fde34cdc 265 if (go_flag_M2_controller == true) {
willem_hoitzing 5:0251fde34cdc 266 go_flag_M2_controller = false;
willem_hoitzing 5:0251fde34cdc 267 M2_controller();
willem_hoitzing 5:0251fde34cdc 268 }
willem_hoitzing 5:0251fde34cdc 269 }
willem_hoitzing 0:26ce65a63616 270 }