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

Committer:
willem_hoitzing
Date:
Wed Oct 26 15:27:21 2016 +0000
Revision:
10:f60f9849980a
Parent:
9:334b1596637b
Child:
11:03d979f1517f
Inverse Kinematics, PID controller, stoppen werkt, heel kleine fout blijft aanwezig

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