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@7:1444604f10d4, 2016-10-25 (annotated)

Committer:
willem_hoitzing
Date:
Tue Oct 25 13:03:24 2016 +0000
Revision:
7:1444604f10d4
Parent:
6:4d254faf2428
Child:
8:008a7bf80fa0
Inverse Kinematics Controller -> versie waar hout stukje kapot ging, dus 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 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 7:1444604f10d4 40 const double M1_Kp = 0.5, M1_Ki = 0.00, M1_Kd = 0.1; // Waardes vinden?
willem_hoitzing 7:1444604f10d4 41 const double M2_Kp = 0.5, M2_Ki = 0.00, M2_Kd = 0.1;
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 7:1444604f10d4 151 q1_ref = q1_ref + q1_v*TS + q1_begin;
willem_hoitzing 7:1444604f10d4 152 q2_ref = q2_ref + q2_v*TS + q2_begin;
willem_hoitzing 7:1444604f10d4 153 if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) { // WAARDES VINDEN 0.8726 (50 graden)
willem_hoitzing 7:1444604f10d4 154 q1_v = 0;
willem_hoitzing 7:1444604f10d4 155 q2_v = 0;
willem_hoitzing 7:1444604f10d4 156 } else if ( (q1 < -(90*2*3.1415/360)) && (q1_v < 0) ) {
willem_hoitzing 7:1444604f10d4 157 q1_v = 0;
willem_hoitzing 7:1444604f10d4 158 q2_v = 0;
willem_hoitzing 7:1444604f10d4 159 } 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 160 q1_v = 0;
willem_hoitzing 7:1444604f10d4 161 q2_v = 0;
willem_hoitzing 7:1444604f10d4 162 } else if ( (q2 > 0) && (q2_v > 0) ) {
willem_hoitzing 7:1444604f10d4 163 q1_v = 0;
willem_hoitzing 7:1444604f10d4 164 q2_v = 0;
willem_hoitzing 7:1444604f10d4 165 }
willem_hoitzing 2:0a976fb06ff8 166 }
willem_hoitzing 2:0a976fb06ff8 167
willem_hoitzing 3:6ba52d1ae499 168 BiQuad pidf_M1;
willem_hoitzing 3:6ba52d1ae499 169 BiQuad pidf_M2;
willem_hoitzing 3:6ba52d1ae499 170 Ticker PIDcontrol_M1;
willem_hoitzing 3:6ba52d1ae499 171 Ticker PIDcontrol_M2;
willem_hoitzing 5:0251fde34cdc 172 volatile bool go_flag_M1_controller = false;
willem_hoitzing 5:0251fde34cdc 173 volatile bool go_flag_M2_controller = false;
willem_hoitzing 5:0251fde34cdc 174
willem_hoitzing 5:0251fde34cdc 175 void flag_M1_controller()
willem_hoitzing 5:0251fde34cdc 176 {
willem_hoitzing 5:0251fde34cdc 177 go_flag_M1_controller = true;
willem_hoitzing 5:0251fde34cdc 178 }
willem_hoitzing 5:0251fde34cdc 179
willem_hoitzing 5:0251fde34cdc 180 void flag_M2_controller()
willem_hoitzing 5:0251fde34cdc 181 {
willem_hoitzing 5:0251fde34cdc 182 go_flag_M2_controller = true;
willem_hoitzing 5:0251fde34cdc 183 }
willem_hoitzing 0:26ce65a63616 184
willem_hoitzing 2:0a976fb06ff8 185 void M1_controller()
willem_hoitzing 2:0a976fb06ff8 186 {
willem_hoitzing 6:4d254faf2428 187 ctrlOutput_M1 = pidf_M1.step(q1_ref-q1);
willem_hoitzing 5:0251fde34cdc 188
willem_hoitzing 5:0251fde34cdc 189 if (ctrlOutput_M1 < 0) {
willem_hoitzing 3:6ba52d1ae499 190 dir_M1 = 1;
willem_hoitzing 5:0251fde34cdc 191 } else {
willem_hoitzing 2:0a976fb06ff8 192 dir_M1 = 0;
willem_hoitzing 2:0a976fb06ff8 193 }
willem_hoitzing 6:4d254faf2428 194 pwm_M1 = abs(ctrlOutput_M1);
willem_hoitzing 3:6ba52d1ae499 195 }
willem_hoitzing 3:6ba52d1ae499 196
willem_hoitzing 3:6ba52d1ae499 197 void M2_controller()
willem_hoitzing 3:6ba52d1ae499 198 {
willem_hoitzing 6:4d254faf2428 199 ctrlOutput_M2 = pidf_M2.step(q2_ref-q2);
willem_hoitzing 5:0251fde34cdc 200
willem_hoitzing 5:0251fde34cdc 201 if (ctrlOutput_M2 < 0) {
willem_hoitzing 3:6ba52d1ae499 202 dir_M2 = 1;
willem_hoitzing 5:0251fde34cdc 203 } else {
willem_hoitzing 3:6ba52d1ae499 204 dir_M2 = 0;
willem_hoitzing 2:0a976fb06ff8 205 }
willem_hoitzing 6:4d254faf2428 206 pwm_M2 = abs(ctrlOutput_M2);
willem_hoitzing 0:26ce65a63616 207 }
willem_hoitzing 0:26ce65a63616 208
willem_hoitzing 0:26ce65a63616 209 int main()
willem_hoitzing 0:26ce65a63616 210 {
willem_hoitzing 0:26ce65a63616 211 pc.baud(115200);
willem_hoitzing 5:0251fde34cdc 212 wheel_M1.reset();
willem_hoitzing 5:0251fde34cdc 213 wheel_M2.reset();
willem_hoitzing 3:6ba52d1ae499 214 pidf_M1.PIDF(M1_Kp,M1_Ki,M1_Kd,N,TS);
willem_hoitzing 3:6ba52d1ae499 215 pidf_M2.PIDF(M2_Kp,M2_Ki,M2_Kd,N,TS);
willem_hoitzing 7:1444604f10d4 216 knop_biceps.rise(&biceps);
willem_hoitzing 7:1444604f10d4 217 knop_triceps.rise(&triceps);
willem_hoitzing 7:1444604f10d4 218 knop_switch.rise(&switcher);
willem_hoitzing 7:1444604f10d4 219
willem_hoitzing 5:0251fde34cdc 220 // flag functions/tickers
willem_hoitzing 7:1444604f10d4 221 update_encoder_ticker.attach(&flag_update_encoder, TS);
willem_hoitzing 7:1444604f10d4 222 update_ref_ticker.attach(&flag_update_ref, TS);
willem_hoitzing 5:0251fde34cdc 223 PIDcontrol_M1.attach(&flag_M1_controller, TS);
willem_hoitzing 5:0251fde34cdc 224 PIDcontrol_M2.attach(&flag_M2_controller, TS);
willem_hoitzing 7:1444604f10d4 225 begin_waarde_q.attach(&begin_waarde, 3);
willem_hoitzing 5:0251fde34cdc 226
willem_hoitzing 5:0251fde34cdc 227 while(1) {
willem_hoitzing 7:1444604f10d4 228
willem_hoitzing 7:1444604f10d4 229 // initialize function
willem_hoitzing 7:1444604f10d4 230 initialize();
willem_hoitzing 7:1444604f10d4 231 if (go_flag_initialize == true) {
willem_hoitzing 7:1444604f10d4 232 go_flag_initialize = false;
willem_hoitzing 7:1444604f10d4 233 initialize();
willem_hoitzing 7:1444604f10d4 234 }
willem_hoitzing 5:0251fde34cdc 235 // update encoder
willem_hoitzing 5:0251fde34cdc 236 if (go_flag_update_encoder == true) {
willem_hoitzing 5:0251fde34cdc 237 go_flag_update_encoder = false;
willem_hoitzing 5:0251fde34cdc 238 update_encoder();
willem_hoitzing 5:0251fde34cdc 239 }
willem_hoitzing 7:1444604f10d4 240 // update joint positions/velocities
willem_hoitzing 7:1444604f10d4 241 if (go_flag_update_ref == true) {
willem_hoitzing 7:1444604f10d4 242 go_flag_update_ref = false;
willem_hoitzing 7:1444604f10d4 243 update_ref();
willem_hoitzing 7:1444604f10d4 244 }
willem_hoitzing 5:0251fde34cdc 245 // controller M1
willem_hoitzing 5:0251fde34cdc 246 if (go_flag_M1_controller == true) {
willem_hoitzing 5:0251fde34cdc 247 go_flag_M1_controller = false;
willem_hoitzing 5:0251fde34cdc 248 M1_controller();
willem_hoitzing 7:1444604f10d4 249 }
willem_hoitzing 5:0251fde34cdc 250 // controller M2
willem_hoitzing 5:0251fde34cdc 251 if (go_flag_M2_controller == true) {
willem_hoitzing 5:0251fde34cdc 252 go_flag_M2_controller = false;
willem_hoitzing 5:0251fde34cdc 253 M2_controller();
willem_hoitzing 5:0251fde34cdc 254 }
willem_hoitzing 5:0251fde34cdc 255 }
willem_hoitzing 0:26ce65a63616 256 }