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@11:03d979f1517f, 2016-10-28 (annotated)

Committer:
willem_hoitzing
Date:
Fri Oct 28 10:36:46 2016 +0000
Revision:
11:03d979f1517f
Parent:
10:f60f9849980a
Child:
12:ff7947a79149
initialize-snelheid gefixt; nog niet heel rechte lijnen, en sloom

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