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@9:334b1596637b, 2016-10-26 (annotated)

Committer:
willem_hoitzing
Date:
Wed Oct 26 14:00:22 2016 +0000
Revision:
9:334b1596637b
Parent:
8:008a7bf80fa0
Child:
10:f60f9849980a
Inverse Kinematics, PID controller zonder biquad, uitgeschreven.; Werkt nog 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 9:334b1596637b 23 volatile float q1 = 0;
willem_hoitzing 9:334b1596637b 24 volatile float q2 = 0;
willem_hoitzing 9:334b1596637b 25 volatile float q1_begin;
willem_hoitzing 9:334b1596637b 26 volatile float q2_begin;
willem_hoitzing 9:334b1596637b 27 volatile float l1 = 0.3626;
willem_hoitzing 9:334b1596637b 28 volatile float l2 = 0.420;
willem_hoitzing 9:334b1596637b 29 volatile float q1_v;
willem_hoitzing 9:334b1596637b 30 volatile float q2_v;
willem_hoitzing 9:334b1596637b 31 float q1_ref = 0;
willem_hoitzing 9:334b1596637b 32 float q2_ref = 0;
willem_hoitzing 9:334b1596637b 33 float q1_error = 0;
willem_hoitzing 9:334b1596637b 34 float q2_error = 0;
willem_hoitzing 9:334b1596637b 35 float q1_error_prev = 0;
willem_hoitzing 9:334b1596637b 36 float q2_error_prev = 0;
willem_hoitzing 9:334b1596637b 37 float q1DerivativeError = 0;
willem_hoitzing 9:334b1596637b 38 float q2DerivativeError = 0;
willem_hoitzing 9:334b1596637b 39 float q1IntError = 0;
willem_hoitzing 9:334b1596637b 40 float q2IntError = 0;
willem_hoitzing 9:334b1596637b 41 float TotalError1= 0;
willem_hoitzing 9:334b1596637b 42 float TotalError2= 0;
willem_hoitzing 9:334b1596637b 43 float motorValue1Out = 0.0;
willem_hoitzing 9:334b1596637b 44 float motorValue2Out = 0.0;
willem_hoitzing 9:334b1596637b 45 volatile float ctrlOutput_M1 = 0;
willem_hoitzing 9:334b1596637b 46 volatile float ctrlOutput_M2 = 0;
willem_hoitzing 9:334b1596637b 47 volatile float vx;
willem_hoitzing 9:334b1596637b 48 volatile float vy;
willem_hoitzing 7:1444604f10d4 49 volatile bool translatie_richting = true; //true is verticaal, false is horizontaal
willem_hoitzing 0:26ce65a63616 50
willem_hoitzing 9:334b1596637b 51 const float TS = 0.02;
willem_hoitzing 9:334b1596637b 52 const float MotorGain = 8.4; // bij pwm = 1 draait (losse) motor met 8.4 rad/s
willem_hoitzing 3:6ba52d1ae499 53
willem_hoitzing 4:a5f3e1838e3e 54 Ticker update_encoder_ticker;
willem_hoitzing 5:0251fde34cdc 55 volatile bool go_flag_update_encoder = false;
willem_hoitzing 5:0251fde34cdc 56 void flag_update_encoder()
willem_hoitzing 5:0251fde34cdc 57 {
willem_hoitzing 5:0251fde34cdc 58 go_flag_update_encoder = true;
willem_hoitzing 5:0251fde34cdc 59 }
willem_hoitzing 5:0251fde34cdc 60
willem_hoitzing 4:a5f3e1838e3e 61 void update_encoder()
willem_hoitzing 2:0a976fb06ff8 62 {
willem_hoitzing 5:0251fde34cdc 63 q1 = wheel_M1.getPulses()/(1334.355/2);
willem_hoitzing 5:0251fde34cdc 64 q2 = wheel_M2.getPulses()/(1334.355/2);
willem_hoitzing 9:334b1596637b 65 //pc.printf("q1 = %f \tq1_ref = %f \tPID1 = %f \tq2 = %f \tq2_ref = %f \tPID2 = %f \ttotalerror1 = %f \ttotalerror2 = %f\n\r",q1, q1_ref, ctrlOutput_M1,q2,q2_ref,ctrlOutput_M2,TotalError1,TotalError2);
willem_hoitzing 9:334b1596637b 66 pc.printf("vx = %f \tvy = %f \tq1_v = %f \tq2_v = %f\n\r",vx,vy,q1_v,q2_v);
willem_hoitzing 7:1444604f10d4 67 }
willem_hoitzing 7:1444604f10d4 68
willem_hoitzing 7:1444604f10d4 69 volatile bool go_flag_initialize = false;
willem_hoitzing 7:1444604f10d4 70
willem_hoitzing 7:1444604f10d4 71 void flag_initialize()
willem_hoitzing 7:1444604f10d4 72 {
willem_hoitzing 7:1444604f10d4 73 go_flag_initialize = true;
willem_hoitzing 7:1444604f10d4 74 }
willem_hoitzing 7:1444604f10d4 75
willem_hoitzing 9:334b1596637b 76 Ticker PIDcontrol;
willem_hoitzing 9:334b1596637b 77 volatile bool go_flag_controller = false;
willem_hoitzing 9:334b1596637b 78
willem_hoitzing 9:334b1596637b 79 void flag_controller()
willem_hoitzing 9:334b1596637b 80 {
willem_hoitzing 9:334b1596637b 81 go_flag_controller = true;
willem_hoitzing 9:334b1596637b 82 }
willem_hoitzing 9:334b1596637b 83
willem_hoitzing 9:334b1596637b 84 volatile bool active_PID_ticker = false;
willem_hoitzing 9:334b1596637b 85
willem_hoitzing 9:334b1596637b 86 void begin_hoeken()
willem_hoitzing 9:334b1596637b 87 {
willem_hoitzing 9:334b1596637b 88 wait(3);
willem_hoitzing 9:334b1596637b 89 float q1_ref = wheel_M1.getPulses()/(1334.355/2);
willem_hoitzing 9:334b1596637b 90 float q2_ref = wheel_M2.getPulses()/(1334.355/2);
willem_hoitzing 9:334b1596637b 91 active_PID_ticker = true;
willem_hoitzing 9:334b1596637b 92 }
willem_hoitzing 9:334b1596637b 93
willem_hoitzing 9:334b1596637b 94
willem_hoitzing 7:1444604f10d4 95 void initialize()
willem_hoitzing 7:1444604f10d4 96 {
willem_hoitzing 9:334b1596637b 97 dir_M1 = 0; //ccw
willem_hoitzing 9:334b1596637b 98 dir_M2 = 1; //cw
willem_hoitzing 9:334b1596637b 99 while (q1 < 20*2*3.1415/360) {
willem_hoitzing 9:334b1596637b 100 q1 = wheel_M1.getPulses()/(1334.355/2);
willem_hoitzing 9:334b1596637b 101 pwm_M1 = 0.01;
willem_hoitzing 9:334b1596637b 102 }
willem_hoitzing 9:334b1596637b 103 pwm_M1 = 0;
willem_hoitzing 9:334b1596637b 104
willem_hoitzing 9:334b1596637b 105 while (q2 > -45*2*3.1415/360) {
willem_hoitzing 9:334b1596637b 106 q2 = wheel_M2.getPulses()/(1334.355/2);
willem_hoitzing 9:334b1596637b 107 pwm_M2 = 0.01;
willem_hoitzing 9:334b1596637b 108 }
willem_hoitzing 9:334b1596637b 109 pwm_M2 = 0;
willem_hoitzing 9:334b1596637b 110 ledg = !ledg;
willem_hoitzing 9:334b1596637b 111 begin_hoeken();
willem_hoitzing 7:1444604f10d4 112 }
willem_hoitzing 7:1444604f10d4 113
willem_hoitzing 9:334b1596637b 114
willem_hoitzing 7:1444604f10d4 115 void biceps()
willem_hoitzing 7:1444604f10d4 116 {
willem_hoitzing 7:1444604f10d4 117 if (translatie_richting) { // verticaal / up
willem_hoitzing 7:1444604f10d4 118 vx = 0;
willem_hoitzing 7:1444604f10d4 119 vy = 0.02;
willem_hoitzing 7:1444604f10d4 120 } else { // horizontaal / right
willem_hoitzing 7:1444604f10d4 121 vx = 0.02;
willem_hoitzing 7:1444604f10d4 122 vy = 0;
willem_hoitzing 7:1444604f10d4 123 }
willem_hoitzing 7:1444604f10d4 124 }
willem_hoitzing 7:1444604f10d4 125
willem_hoitzing 7:1444604f10d4 126 void triceps()
willem_hoitzing 7:1444604f10d4 127 {
willem_hoitzing 7:1444604f10d4 128 if (translatie_richting) { // verticaal / down
willem_hoitzing 7:1444604f10d4 129 vx = 0;
willem_hoitzing 7:1444604f10d4 130 vy = -0.02;
willem_hoitzing 7:1444604f10d4 131 } else { // horizontaal / left
willem_hoitzing 7:1444604f10d4 132 vx = -0.02;
willem_hoitzing 7:1444604f10d4 133 vy = 0;
willem_hoitzing 7:1444604f10d4 134 }
willem_hoitzing 7:1444604f10d4 135
willem_hoitzing 7:1444604f10d4 136 }
willem_hoitzing 7:1444604f10d4 137
willem_hoitzing 7:1444604f10d4 138 void switcher()
willem_hoitzing 7:1444604f10d4 139 {
willem_hoitzing 7:1444604f10d4 140 if ( (vx == 0) && (vy == 0) ) {
willem_hoitzing 7:1444604f10d4 141 translatie_richting = !translatie_richting;
willem_hoitzing 7:1444604f10d4 142 } else {
willem_hoitzing 7:1444604f10d4 143 vx = 0;
willem_hoitzing 7:1444604f10d4 144 vy = 0;
willem_hoitzing 7:1444604f10d4 145 }
willem_hoitzing 7:1444604f10d4 146
willem_hoitzing 7:1444604f10d4 147 if (translatie_richting == 1) {
willem_hoitzing 7:1444604f10d4 148 ledr = 1; // blauw - verticaal
willem_hoitzing 7:1444604f10d4 149 ledg = 1;
willem_hoitzing 7:1444604f10d4 150 ledb = 0;
willem_hoitzing 7:1444604f10d4 151 } else {
willem_hoitzing 7:1444604f10d4 152 ledr = 0; // rood - horizontaal
willem_hoitzing 7:1444604f10d4 153 ledg = 1;
willem_hoitzing 7:1444604f10d4 154 ledb = 1;
willem_hoitzing 7:1444604f10d4 155 }
willem_hoitzing 7:1444604f10d4 156 }
willem_hoitzing 7:1444604f10d4 157
willem_hoitzing 7:1444604f10d4 158 Ticker update_ref_ticker;
willem_hoitzing 9:334b1596637b 159 volatile float J_1;
willem_hoitzing 9:334b1596637b 160 volatile float J_2;
willem_hoitzing 9:334b1596637b 161 volatile float J_3;
willem_hoitzing 9:334b1596637b 162 volatile float J_4;
willem_hoitzing 7:1444604f10d4 163 volatile bool go_flag_update_ref = false;
willem_hoitzing 7:1444604f10d4 164 void flag_update_ref()
willem_hoitzing 7:1444604f10d4 165 {
willem_hoitzing 7:1444604f10d4 166 go_flag_update_ref = true;
willem_hoitzing 7:1444604f10d4 167 }
willem_hoitzing 7:1444604f10d4 168
willem_hoitzing 7:1444604f10d4 169 void update_ref()
willem_hoitzing 7:1444604f10d4 170 {
willem_hoitzing 7:1444604f10d4 171 q1 = wheel_M1.getPulses() / (1334.355/2); // rad
willem_hoitzing 7:1444604f10d4 172 q2 = wheel_M2.getPulses() / (1334.355/2);
willem_hoitzing 7:1444604f10d4 173
willem_hoitzing 7:1444604f10d4 174 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 175 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 176 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 177 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 178
willem_hoitzing 7:1444604f10d4 179 q1_v = J_1 * vx + J_2 * vy;
willem_hoitzing 7:1444604f10d4 180 q2_v = J_3 * vx + J_4 * vy;
willem_hoitzing 7:1444604f10d4 181
willem_hoitzing 7:1444604f10d4 182 if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) { // WAARDES VINDEN 0.8726 (50 graden)
willem_hoitzing 7:1444604f10d4 183 q1_v = 0;
willem_hoitzing 7:1444604f10d4 184 q2_v = 0;
willem_hoitzing 7:1444604f10d4 185 } else if ( (q1 < -(90*2*3.1415/360)) && (q1_v < 0) ) {
willem_hoitzing 7:1444604f10d4 186 q1_v = 0;
willem_hoitzing 7:1444604f10d4 187 q2_v = 0;
willem_hoitzing 7:1444604f10d4 188 } 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 189 q1_v = 0;
willem_hoitzing 7:1444604f10d4 190 q2_v = 0;
willem_hoitzing 7:1444604f10d4 191 } else if ( (q2 > 0) && (q2_v > 0) ) {
willem_hoitzing 7:1444604f10d4 192 q1_v = 0;
willem_hoitzing 7:1444604f10d4 193 q2_v = 0;
willem_hoitzing 7:1444604f10d4 194 }
willem_hoitzing 9:334b1596637b 195
willem_hoitzing 9:334b1596637b 196 q1_ref = q1 + q1_v*TS;
willem_hoitzing 9:334b1596637b 197 q2_ref = q2 + q2_v*TS;
willem_hoitzing 2:0a976fb06ff8 198 }
willem_hoitzing 2:0a976fb06ff8 199
willem_hoitzing 9:334b1596637b 200 void PID(float q1,float q1_ref,float q2,float q2_ref,float TS,float &motorValue1Out, float &motorValue2Out)
willem_hoitzing 9:334b1596637b 201 {
willem_hoitzing 9:334b1596637b 202 // linear feedback control
willem_hoitzing 9:334b1596637b 203 float q1_error = q1_ref - q1; //referencePosition1 - Position1; // proportional angular error in radians
willem_hoitzing 9:334b1596637b 204 float q2_error = q2_ref - q2; //referencePosition1 - Position1; // proportional angular error in radians
willem_hoitzing 9:334b1596637b 205 float Kp = 1;
willem_hoitzing 5:0251fde34cdc 206
willem_hoitzing 9:334b1596637b 207 float q1IntError = q1IntError + q1_error*TS; // integrated error in radians
willem_hoitzing 9:334b1596637b 208 float q2IntError = q2IntError + q2_error*TS; // integrated error in radians
willem_hoitzing 9:334b1596637b 209 float Ki = 0.1;
willem_hoitzing 9:334b1596637b 210
willem_hoitzing 9:334b1596637b 211 float q1DerivativeError = (q1_error - q1_error_prev)/TS; // derivative of error in radians
willem_hoitzing 9:334b1596637b 212 float q2DerivativeError = (q2_error - q2_error_prev)/TS; // derivative of error in radians
willem_hoitzing 9:334b1596637b 213 float Kd = 0.0;
willem_hoitzing 9:334b1596637b 214
willem_hoitzing 9:334b1596637b 215 TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd; //total controller output = motor input
willem_hoitzing 9:334b1596637b 216 TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd; //total controller output = motor input
willem_hoitzing 9:334b1596637b 217
willem_hoitzing 9:334b1596637b 218 if (TotalError1 < 0) {
willem_hoitzing 9:334b1596637b 219 TotalError1=0;
willem_hoitzing 9:334b1596637b 220 }
willem_hoitzing 9:334b1596637b 221 else {
willem_hoitzing 9:334b1596637b 222 TotalError1=TotalError1;
willem_hoitzing 9:334b1596637b 223 }
willem_hoitzing 9:334b1596637b 224 if (TotalError2 < 0) {
willem_hoitzing 9:334b1596637b 225 TotalError2=0;
willem_hoitzing 9:334b1596637b 226 }
willem_hoitzing 9:334b1596637b 227 else {
willem_hoitzing 9:334b1596637b 228 TotalError2=TotalError2;
willem_hoitzing 9:334b1596637b 229 }
willem_hoitzing 9:334b1596637b 230
willem_hoitzing 9:334b1596637b 231 motorValue1Out = TotalError1/MotorGain;
willem_hoitzing 9:334b1596637b 232 motorValue2Out = TotalError2/MotorGain;
willem_hoitzing 9:334b1596637b 233
willem_hoitzing 9:334b1596637b 234 q1_error_prev = q1_error;
willem_hoitzing 9:334b1596637b 235 q2_error_prev = q2_error;
willem_hoitzing 5:0251fde34cdc 236 }
willem_hoitzing 5:0251fde34cdc 237
willem_hoitzing 9:334b1596637b 238 void Controller()
willem_hoitzing 5:0251fde34cdc 239 {
willem_hoitzing 9:334b1596637b 240 PID(q1,q1_ref,q2,q2_ref,TS,motorValue1Out,motorValue2Out);
willem_hoitzing 9:334b1596637b 241 ctrlOutput_M1 = motorValue1Out;
willem_hoitzing 9:334b1596637b 242 ctrlOutput_M2 = motorValue2Out;
willem_hoitzing 9:334b1596637b 243
willem_hoitzing 5:0251fde34cdc 244 if (ctrlOutput_M1 < 0) {
willem_hoitzing 3:6ba52d1ae499 245 dir_M1 = 1;
willem_hoitzing 5:0251fde34cdc 246 } else {
willem_hoitzing 2:0a976fb06ff8 247 dir_M1 = 0;
willem_hoitzing 2:0a976fb06ff8 248 }
willem_hoitzing 6:4d254faf2428 249 pwm_M1 = abs(ctrlOutput_M1);
willem_hoitzing 9:334b1596637b 250 if (pwm_M1 <= 0) {
willem_hoitzing 8:008a7bf80fa0 251 pwm_M1 = 0;
willem_hoitzing 9:334b1596637b 252 } else {
willem_hoitzing 9:334b1596637b 253 pwm_M1 = pwm_M1 + 0.05;
willem_hoitzing 8:008a7bf80fa0 254 }
willem_hoitzing 5:0251fde34cdc 255
willem_hoitzing 5:0251fde34cdc 256 if (ctrlOutput_M2 < 0) {
willem_hoitzing 3:6ba52d1ae499 257 dir_M2 = 1;
willem_hoitzing 5:0251fde34cdc 258 } else {
willem_hoitzing 3:6ba52d1ae499 259 dir_M2 = 0;
willem_hoitzing 2:0a976fb06ff8 260 }
willem_hoitzing 6:4d254faf2428 261 pwm_M2 = abs(ctrlOutput_M2);
willem_hoitzing 9:334b1596637b 262 if (pwm_M2 <= 0) {
willem_hoitzing 8:008a7bf80fa0 263 pwm_M2 = 0;
willem_hoitzing 9:334b1596637b 264 } else {
willem_hoitzing 9:334b1596637b 265 pwm_M2 = pwm_M2 + 0.05;
willem_hoitzing 8:008a7bf80fa0 266 }
willem_hoitzing 0:26ce65a63616 267 }
willem_hoitzing 0:26ce65a63616 268
willem_hoitzing 0:26ce65a63616 269 int main()
willem_hoitzing 0:26ce65a63616 270 {
willem_hoitzing 0:26ce65a63616 271 pc.baud(115200);
willem_hoitzing 5:0251fde34cdc 272 wheel_M1.reset();
willem_hoitzing 5:0251fde34cdc 273 wheel_M2.reset();
willem_hoitzing 7:1444604f10d4 274 knop_biceps.rise(&biceps);
willem_hoitzing 7:1444604f10d4 275 knop_triceps.rise(&triceps);
willem_hoitzing 7:1444604f10d4 276 knop_switch.rise(&switcher);
willem_hoitzing 7:1444604f10d4 277
willem_hoitzing 5:0251fde34cdc 278 // flag functions/tickers
willem_hoitzing 7:1444604f10d4 279 update_encoder_ticker.attach(&flag_update_encoder, TS);
willem_hoitzing 7:1444604f10d4 280 update_ref_ticker.attach(&flag_update_ref, TS);
willem_hoitzing 9:334b1596637b 281 // initialize -> beginposities
willem_hoitzing 9:334b1596637b 282 initialize();
willem_hoitzing 5:0251fde34cdc 283
willem_hoitzing 9:334b1596637b 284 if (active_PID_ticker == true) {
willem_hoitzing 9:334b1596637b 285 PIDcontrol.attach(&flag_controller, TS);
willem_hoitzing 9:334b1596637b 286 }
willem_hoitzing 8:008a7bf80fa0 287
willem_hoitzing 5:0251fde34cdc 288 while(1) {
willem_hoitzing 9:334b1596637b 289
willem_hoitzing 5:0251fde34cdc 290 // update encoder
willem_hoitzing 5:0251fde34cdc 291 if (go_flag_update_encoder == true) {
willem_hoitzing 5:0251fde34cdc 292 go_flag_update_encoder = false;
willem_hoitzing 5:0251fde34cdc 293 update_encoder();
willem_hoitzing 5:0251fde34cdc 294 }
willem_hoitzing 7:1444604f10d4 295 // update joint positions/velocities
willem_hoitzing 7:1444604f10d4 296 if (go_flag_update_ref == true) {
willem_hoitzing 7:1444604f10d4 297 go_flag_update_ref = false;
willem_hoitzing 7:1444604f10d4 298 update_ref();
willem_hoitzing 7:1444604f10d4 299 }
willem_hoitzing 9:334b1596637b 300 // controller M1+M2
willem_hoitzing 9:334b1596637b 301 if (go_flag_controller == true) {
willem_hoitzing 9:334b1596637b 302 go_flag_controller = false;
willem_hoitzing 9:334b1596637b 303 Controller();
willem_hoitzing 5:0251fde34cdc 304 }
willem_hoitzing 5:0251fde34cdc 305 }
willem_hoitzing 0:26ce65a63616 306 }