Astrid Schut / Mbed 2 deprecated PWMmotor

Code om de PID controller af te stellen aan de hand van een sinus golf

Dependencies:   mbed QEI MODSERIAL FastPWM biquadFilter

main.cpp@20:ad425c7f84b6, 2019-04-20 (annotated)

Committer:
aschut
Date:
Sat Apr 20 19:55:10 2019 +0000
Revision:
20:ad425c7f84b6
Parent:
19:24f5e36c6490 
Met comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aschut 0:a9a42914138c 1 #include "mbed.h"
aschut 2:926d56babb1a 2 #include "MODSERIAL.h"
aschut 4:99f7fdce608e 3 #include "QEI.h"
aschut 6:14a9c4f30c86 4 #include "BiQuad.h"
aschut 13:b7a1a4245f37 5 #include "FastPWM.h"
aschut 20:ad425c7f84b6 6 // Dit script is voor het testen van motor 2, volg instructies om het om te zetten naar motor 1
aschut 20:ad425c7f84b6 7
aschut 20:ad425c7f84b6 8
aschut 4:99f7fdce608e 9 // Algemeen
aschut 17:190c86f4828f 10 Timer t;
aschut 1:58f34947c674 11 DigitalIn button2(SW2);
aschut 1:58f34947c674 12 DigitalIn button3(SW3);
aschut 20:ad425c7f84b6 13 DigitalIn But2(D12); //Buttons op motorschield
aschut 17:190c86f4828f 14 DigitalIn But1(D13);
aschut 2:926d56babb1a 15 DigitalOut led(LED_GREEN);
aschut 2:926d56babb1a 16 DigitalOut led2(LED_RED);
aschut 4:99f7fdce608e 17 DigitalOut led3(LED_BLUE);
aschut 2:926d56babb1a 18 MODSERIAL pc(USBTX, USBRX);
aschut 20:ad425c7f84b6 19
aschut 20:ad425c7f84b6 20 //waardes voor sinus signaal
aschut 17:190c86f4828f 21 float A;
aschut 17:190c86f4828f 22 int i = 0;
aschut 17:190c86f4828f 23 float pi = 3.14159265359;
aschut 17:190c86f4828f 24
aschut 2:926d56babb1a 25
aschut 4:99f7fdce608e 26 //Motoren
aschut 0:a9a42914138c 27 DigitalOut direction1(D4);
aschut 13:b7a1a4245f37 28 FastPWM pwmpin1(D5);
aschut 13:b7a1a4245f37 29 FastPWM pwmpin2(D6);
aschut 0:a9a42914138c 30 DigitalOut direction2(D7);
aschut 9:aa5d6636197b 31 volatile float pwm1;
aschut 4:99f7fdce608e 32 volatile float pwm2;
aschut 4:99f7fdce608e 33
aschut 4:99f7fdce608e 34 //Encoder
aschut 17:190c86f4828f 35 QEI encoder1 (D1, D0, NC, 1200, QEI::X4_ENCODING);
aschut 17:190c86f4828f 36 QEI encoder2 (D3, D2, NC, 4800, QEI::X4_ENCODING);
aschut 9:aa5d6636197b 37 double Pulses1;
aschut 9:aa5d6636197b 38 double motor_position1;
aschut 6:14a9c4f30c86 39 double Pulses2;
aschut 6:14a9c4f30c86 40 double motor_position2;
aschut 9:aa5d6636197b 41 double error1;
aschut 19:24f5e36c6490 42 double error2;
aschut 2:926d56babb1a 43
aschut 2:926d56babb1a 44 //Pot meter
aschut 17:190c86f4828f 45 AnalogIn pot(A5);
aschut 6:14a9c4f30c86 46 AnalogIn pot0(A0);
aschut 4:99f7fdce608e 47 float Pot2;
aschut 6:14a9c4f30c86 48 float Pot1;
aschut 4:99f7fdce608e 49
aschut 4:99f7fdce608e 50 //Ticker
aschut 4:99f7fdce608e 51 Ticker Pwm;
aschut 2:926d56babb1a 52 Ticker PotRead;
aschut 7:9a1007e35bac 53 Ticker Kdc;
aschut 6:14a9c4f30c86 54
aschut 0:a9a42914138c 55
aschut 5:4b25551aeb6e 56 //Kinematica
aschut 9:aa5d6636197b 57 double stap1;
aschut 9:aa5d6636197b 58 double stap2;
aschut 6:14a9c4f30c86 59 double KPot;
aschut 5:4b25551aeb6e 60 float KPotabs;
aschut 9:aa5d6636197b 61
aschut 5:4b25551aeb6e 62 float ElbowReference;
aschut 5:4b25551aeb6e 63 float Ellebooghoek1;
aschut 5:4b25551aeb6e 64 float Ellebooghoek2;
aschut 5:4b25551aeb6e 65 float Ellebooghoek3;
aschut 5:4b25551aeb6e 66 float Ellebooghoek4;
aschut 9:aa5d6636197b 67
aschut 9:aa5d6636197b 68 float PolsReference;
aschut 9:aa5d6636197b 69 float Polshoek1;
aschut 9:aa5d6636197b 70 float Polshoek2;
aschut 9:aa5d6636197b 71 float Polshoek3;
aschut 9:aa5d6636197b 72 float Polshoek4;
aschut 9:aa5d6636197b 73
aschut 9:aa5d6636197b 74 float Hoeknieuw1;
aschut 9:aa5d6636197b 75 float Hoeknieuw2;
aschut 5:4b25551aeb6e 76
aschut 5:4b25551aeb6e 77 //Limiet in graden
aschut 13:b7a1a4245f37 78 float lowerlim1 = -900;
aschut 13:b7a1a4245f37 79 float upperlim1 = 900;
aschut 9:aa5d6636197b 80 float lowerlim2 = 0;
aschut 9:aa5d6636197b 81 float upperlim2 = 748.8; //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor
aschut 2:926d56babb1a 82
aschut 6:14a9c4f30c86 83 // VARIABLES PID CONTROLLER
aschut 14:30d32f51dd9f 84 double Kp1 = 0;
aschut 9:aa5d6636197b 85 double Ki1 = 0;
aschut 9:aa5d6636197b 86 double Kd1 = 1;
aschut 9:aa5d6636197b 87 double Kp2 = 6; // Zonder arm: 6,0,1
aschut 9:aa5d6636197b 88 double Ki2 = 0;
aschut 9:aa5d6636197b 89 double Kd2 = 1;
aschut 11:2375b538b631 90 double Ts = 0.0005; // Sample time in seconds
aschut 6:14a9c4f30c86 91
aschut 9:aa5d6636197b 92 float Kinematics1(float KPot)
aschut 6:14a9c4f30c86 93 {
aschut 6:14a9c4f30c86 94
aschut 6:14a9c4f30c86 95 if (KPot > 0.45f) {
aschut 13:b7a1a4245f37 96 stap1 = KPot*450*Ts;
aschut 9:aa5d6636197b 97 Hoeknieuw1 = PolsReference + stap1;
aschut 9:aa5d6636197b 98 return Hoeknieuw1;
aschut 5:4b25551aeb6e 99 }
aschut 4:99f7fdce608e 100
aschut 6:14a9c4f30c86 101 else if (KPot < -0.45f) {
aschut 13:b7a1a4245f37 102 stap1 = KPot*450*Ts;
aschut 9:aa5d6636197b 103 Hoeknieuw1 = PolsReference + stap1;
aschut 9:aa5d6636197b 104 return Hoeknieuw1;
aschut 9:aa5d6636197b 105 }
aschut 9:aa5d6636197b 106
aschut 9:aa5d6636197b 107 else {
aschut 9:aa5d6636197b 108 return PolsReference;
aschut 9:aa5d6636197b 109 }
aschut 9:aa5d6636197b 110 }
aschut 9:aa5d6636197b 111 float Kinematics2(float KPot)
aschut 9:aa5d6636197b 112 {
aschut 9:aa5d6636197b 113
aschut 9:aa5d6636197b 114 if (KPot > 0.45f) {
aschut 20:ad425c7f84b6 115 stap2 = KPot*150*Ts;
aschut 9:aa5d6636197b 116 Hoeknieuw2 = ElbowReference + stap2;
aschut 9:aa5d6636197b 117 return Hoeknieuw2;
aschut 9:aa5d6636197b 118 }
aschut 9:aa5d6636197b 119
aschut 9:aa5d6636197b 120 else if (KPot < -0.45f) {
aschut 9:aa5d6636197b 121 stap2 = KPot*150*Ts;
aschut 9:aa5d6636197b 122 Hoeknieuw2 = ElbowReference + stap2;
aschut 9:aa5d6636197b 123 return Hoeknieuw2;
aschut 6:14a9c4f30c86 124 }
aschut 6:14a9c4f30c86 125
aschut 6:14a9c4f30c86 126 else {
aschut 6:14a9c4f30c86 127 return ElbowReference;
aschut 6:14a9c4f30c86 128 }
aschut 6:14a9c4f30c86 129 }
aschut 6:14a9c4f30c86 130
aschut 9:aa5d6636197b 131 float Limits1(float Polshoek2)
aschut 6:14a9c4f30c86 132 {
aschut 6:14a9c4f30c86 133
aschut 9:aa5d6636197b 134 if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) { //Binnen de limieten
aschut 9:aa5d6636197b 135 Polshoek3 = Polshoek2;
aschut 9:aa5d6636197b 136 }
aschut 9:aa5d6636197b 137
aschut 9:aa5d6636197b 138 else {
aschut 9:aa5d6636197b 139 if (Polshoek2 >= upperlim1) { //Boven de limiet
aschut 9:aa5d6636197b 140 Polshoek3 = upperlim1;
aschut 9:aa5d6636197b 141 } else { //Onder de limiet
aschut 9:aa5d6636197b 142 Polshoek3 = lowerlim1;
aschut 9:aa5d6636197b 143 }
aschut 9:aa5d6636197b 144 }
aschut 9:aa5d6636197b 145
aschut 9:aa5d6636197b 146 return Polshoek3;
aschut 9:aa5d6636197b 147 }
aschut 9:aa5d6636197b 148
aschut 9:aa5d6636197b 149
aschut 9:aa5d6636197b 150 float Limits2(float Ellebooghoek2)
aschut 9:aa5d6636197b 151 {
aschut 9:aa5d6636197b 152
aschut 9:aa5d6636197b 153 if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) { //Binnen de limieten
aschut 5:4b25551aeb6e 154 Ellebooghoek3 = Ellebooghoek2;
aschut 6:14a9c4f30c86 155 }
aschut 6:14a9c4f30c86 156
aschut 5:4b25551aeb6e 157 else {
aschut 9:aa5d6636197b 158 if (Ellebooghoek2 >= upperlim2) { //Boven de limiet
aschut 9:aa5d6636197b 159 Ellebooghoek3 = upperlim2;
aschut 6:14a9c4f30c86 160 } else { //Onder de limiet
aschut 9:aa5d6636197b 161 Ellebooghoek3 = lowerlim2;
aschut 5:4b25551aeb6e 162 }
aschut 5:4b25551aeb6e 163 }
aschut 6:14a9c4f30c86 164
aschut 5:4b25551aeb6e 165 return Ellebooghoek3;
aschut 6:14a9c4f30c86 166 }
aschut 6:14a9c4f30c86 167
aschut 6:14a9c4f30c86 168
aschut 4:99f7fdce608e 169
aschut 0:a9a42914138c 170
aschut 6:14a9c4f30c86 171 // ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~
aschut 9:aa5d6636197b 172 double PID_controller1(double error1)
aschut 6:14a9c4f30c86 173 {
aschut 9:aa5d6636197b 174 static double error1_integral = 0;
aschut 9:aa5d6636197b 175 static double error1_prev = error1; // initialization with this value only done once!
aschut 20:ad425c7f84b6 176 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
aschut 20:ad425c7f84b6 177 /* Zet dit aan als je motor 1 wilt testen
aschut 14:30d32f51dd9f 178 //PID testing
aschut 14:30d32f51dd9f 179 Kp1 = 30 * Pot2;
aschut 9:aa5d6636197b 180 Ki1 = 10 * Pot1;
aschut 7:9a1007e35bac 181
aschut 7:9a1007e35bac 182 if (!But2) {
aschut 14:30d32f51dd9f 183 Kd1 = Kd1 + 0.001;
aschut 7:9a1007e35bac 184 }
aschut 7:9a1007e35bac 185 if (!But1){
aschut 14:30d32f51dd9f 186 Kd1 = Kd1 - 0.001;
aschut 7:9a1007e35bac 187 }
aschut 19:24f5e36c6490 188 */
aschut 7:9a1007e35bac 189
aschut 6:14a9c4f30c86 190 // Proportional part:
aschut 9:aa5d6636197b 191 double u_k1 = Kp1 * error1;
aschut 6:14a9c4f30c86 192
aschut 6:14a9c4f30c86 193 // Integral part
aschut 9:aa5d6636197b 194 error1_integral = error1_integral + error1 * Ts;
aschut 9:aa5d6636197b 195 double u_i1 = Ki1* error1_integral;
aschut 6:14a9c4f30c86 196
aschut 6:14a9c4f30c86 197 // Derivative part
aschut 9:aa5d6636197b 198 double error1_derivative = (error1 - error1_prev)/Ts;
aschut 9:aa5d6636197b 199 double filtered_error1_derivative = LowPassFilter.step(error1_derivative);
aschut 9:aa5d6636197b 200 double u_d1 = Kd1 * filtered_error1_derivative;
aschut 9:aa5d6636197b 201 error1_prev = error1;
aschut 6:14a9c4f30c86 202
aschut 6:14a9c4f30c86 203 // Sum all parts and return it
aschut 9:aa5d6636197b 204 return u_k1 + u_i1 + u_d1;
aschut 9:aa5d6636197b 205 }
aschut 9:aa5d6636197b 206 double PID_controller2(double error2)
aschut 9:aa5d6636197b 207 {
aschut 9:aa5d6636197b 208 static double error2_integral = 0;
aschut 9:aa5d6636197b 209 static double error2_prev = error2; // initialization with this value only done once!
aschut 9:aa5d6636197b 210 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
aschut 9:aa5d6636197b 211
aschut 20:ad425c7f84b6 212 // /*PID testing
aschut 20:ad425c7f84b6 213 Kp2 = 50 * Pot2;
aschut 9:aa5d6636197b 214 Ki2 = 10 * Pot1;
aschut 9:aa5d6636197b 215
aschut 9:aa5d6636197b 216 if (!But2) {
aschut 9:aa5d6636197b 217 Kd2 = Kd2 + 0.01;
aschut 9:aa5d6636197b 218 }
aschut 9:aa5d6636197b 219 if (!But1){
aschut 9:aa5d6636197b 220 Kd2 = Kd2 - 0.01;
aschut 9:aa5d6636197b 221 }
aschut 20:ad425c7f84b6 222 // */Zet dit stukje uit als je motor 1 wilt testen
aschut 9:aa5d6636197b 223
aschut 9:aa5d6636197b 224 // Proportional part:
aschut 9:aa5d6636197b 225 double u_k2 = Kp2 * error2;
aschut 9:aa5d6636197b 226
aschut 9:aa5d6636197b 227 // Integral part
aschut 9:aa5d6636197b 228 error2_integral = error2_integral + error2 * Ts;
aschut 9:aa5d6636197b 229 double u_i2 = Ki2 * error2_integral;
aschut 9:aa5d6636197b 230
aschut 9:aa5d6636197b 231 // Derivative part
aschut 9:aa5d6636197b 232 double error2_derivative = (error2 - error2_prev)/Ts;
aschut 9:aa5d6636197b 233 double filtered_error2_derivative = LowPassFilter.step(error2_derivative);
aschut 9:aa5d6636197b 234 double u_d2 = Kd2 * filtered_error2_derivative;
aschut 9:aa5d6636197b 235 error2_prev = error2;
aschut 9:aa5d6636197b 236
aschut 9:aa5d6636197b 237 // Sum all parts and return it
aschut 9:aa5d6636197b 238 return u_k2 + u_i2 + u_d2;
aschut 6:14a9c4f30c86 239 }
aschut 6:14a9c4f30c86 240
aschut 9:aa5d6636197b 241 void moter1_control(double u1)
aschut 6:14a9c4f30c86 242 {
aschut 9:aa5d6636197b 243 direction1= u1 > 0.0f; //positief = CW
aschut 20:ad425c7f84b6 244 if (fabs(u1)> 0.99f) {
aschut 20:ad425c7f84b6 245 u1 = 0.99f;
aschut 6:14a9c4f30c86 246 } else {
aschut 9:aa5d6636197b 247 u1= u1;
aschut 0:a9a42914138c 248 }
aschut 13:b7a1a4245f37 249 pwmpin1.write(fabs(u1)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
aschut 9:aa5d6636197b 250 }
aschut 9:aa5d6636197b 251
aschut 9:aa5d6636197b 252 void moter2_control(double u2)
aschut 9:aa5d6636197b 253 {
aschut 9:aa5d6636197b 254 direction2= u2 < 0.0f; //positief = CW
aschut 9:aa5d6636197b 255 if (fabs(u2)> 0.7f) {
aschut 9:aa5d6636197b 256 u2 = 0.7f;
aschut 9:aa5d6636197b 257 } else {
aschut 9:aa5d6636197b 258 u2= u2;
aschut 9:aa5d6636197b 259 }
aschut 13:b7a1a4245f37 260 pwmpin2.write(fabs(u2)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
aschut 6:14a9c4f30c86 261 }
aschut 6:14a9c4f30c86 262
aschut 4:99f7fdce608e 263 void PwmMotor(void)
aschut 6:14a9c4f30c86 264 {
aschut 20:ad425c7f84b6 265 //Reference hoek berekenen, in graden
aschut 20:ad425c7f84b6 266 //float Ellebooghoek1 = Kinematics2(pwm2); //Zet dit aan als je motor 1 wilt testen
aschut 20:ad425c7f84b6 267 //float Ellebooghoek4 = Limits2(Ellebooghoek1); //Zet dit aan als je motor 1 wilt testen
aschut 14:30d32f51dd9f 268 //ElbowReference = Ellebooghoek4;
aschut 9:aa5d6636197b 269
aschut 20:ad425c7f84b6 270 float Polshoek1 = Kinematics1(pwm2); //Zet dit uit als je motor 1 wilt testen
aschut 20:ad425c7f84b6 271 float Polshoek4 = Limits1(Polshoek1); //Zet dit aan als je motor 1 wilt testen
aschut 14:30d32f51dd9f 272 //PolsReference = Polshoek4;
aschut 6:14a9c4f30c86 273
aschut 7:9a1007e35bac 274 // Positie motor berekenen, in graden
aschut 9:aa5d6636197b 275 Pulses1 = encoder1.getPulses();
aschut 9:aa5d6636197b 276 motor_position1 = -(Pulses1/1200)*360;
aschut 6:14a9c4f30c86 277 Pulses2 = encoder2.getPulses();
aschut 9:aa5d6636197b 278 motor_position2 = -(Pulses2/4800)*360;
aschut 2:926d56babb1a 279
aschut 9:aa5d6636197b 280 double error1 = PolsReference - motor_position1;
aschut 9:aa5d6636197b 281 double u1 = PID_controller1(error1);
aschut 9:aa5d6636197b 282 moter1_control(u1);
aschut 9:aa5d6636197b 283
aschut 9:aa5d6636197b 284 double error2 = ElbowReference - motor_position2;
aschut 9:aa5d6636197b 285 double u2 = PID_controller2(error2);
aschut 9:aa5d6636197b 286 moter2_control(u2);
aschut 6:14a9c4f30c86 287
aschut 6:14a9c4f30c86 288 }
aschut 6:14a9c4f30c86 289
aschut 1:58f34947c674 290 void MotorOn(void)
aschut 6:14a9c4f30c86 291 {
aschut 9:aa5d6636197b 292 pwmpin1 = 0;
aschut 4:99f7fdce608e 293 pwmpin2 = 0;
aschut 6:14a9c4f30c86 294 Pwm.attach (PwmMotor, Ts);
aschut 6:14a9c4f30c86 295
aschut 6:14a9c4f30c86 296 }
aschut 1:58f34947c674 297 void MotorOff(void)
aschut 6:14a9c4f30c86 298 {
aschut 6:14a9c4f30c86 299 Pwm.detach ();
aschut 1:58f34947c674 300 pwmpin2 = 0;
aschut 9:aa5d6636197b 301 pwmpin1 = 0;
aschut 6:14a9c4f30c86 302 }
aschut 4:99f7fdce608e 303
aschut 6:14a9c4f30c86 304 void ContinuousReader(void)
aschut 6:14a9c4f30c86 305 {
aschut 3:ac13255164cd 306 Pot2 = pot.read();
aschut 6:14a9c4f30c86 307 Pot1 = pot0.read();
aschut 7:9a1007e35bac 308 pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
aschut 9:aa5d6636197b 309 pwm1 =(Pot1*2)-1;
aschut 6:14a9c4f30c86 310 }
aschut 6:14a9c4f30c86 311
aschut 14:30d32f51dd9f 312
aschut 20:ad425c7f84b6 313 void Kdcount (void) // Voor het testen van de PID waardes met een sinus signaal
aschut 7:9a1007e35bac 314 {
aschut 17:190c86f4828f 315 float ts = t.read();
aschut 20:ad425c7f84b6 316 A = 350; //Zet uit voor motor 2
aschut 19:24f5e36c6490 317 ElbowReference = 350 + A*sin(2*pi*ts*0.1+(3/4)*pi);
aschut 20:ad425c7f84b6 318 //A = 900; //Zet aan voor motor 1
aschut 20:ad425c7f84b6 319 //PolsReference = A*sin(2*pi*ts*0.1+(3/4)*pi);
aschut 7:9a1007e35bac 320 }
aschut 14:30d32f51dd9f 321
aschut 1:58f34947c674 322
aschut 6:14a9c4f30c86 323 int main()
aschut 6:14a9c4f30c86 324 {
aschut 17:190c86f4828f 325
aschut 6:14a9c4f30c86 326 t.start();
aschut 6:14a9c4f30c86 327 int counter = 0;
aschut 6:14a9c4f30c86 328 pwmpin2.period_us(60);
aschut 6:14a9c4f30c86 329 PotRead.attach(ContinuousReader,Ts);
aschut 17:190c86f4828f 330 Kdc.attach(Kdcount,0.01); //Voor PID waarde testen
aschut 17:190c86f4828f 331 //Kdcount();
aschut 2:926d56babb1a 332 pc.baud(115200);
aschut 6:14a9c4f30c86 333 //pc.printf("start\r\n");
aschut 4:99f7fdce608e 334 led = 1;
aschut 4:99f7fdce608e 335 led2 =1;
aschut 4:99f7fdce608e 336 led3 =1;
aschut 6:14a9c4f30c86 337
aschut 6:14a9c4f30c86 338 while (true) {
aschut 6:14a9c4f30c86 339 led3 = 0;
aschut 6:14a9c4f30c86 340 if (!button2) {
aschut 6:14a9c4f30c86 341 led3 = 1;
aschut 6:14a9c4f30c86 342 led = 0;
aschut 6:14a9c4f30c86 343 //pc.printf("MotorOn\r\n");
aschut 6:14a9c4f30c86 344 MotorOn();
aschut 6:14a9c4f30c86 345 }
aschut 6:14a9c4f30c86 346 if (!button3) {
aschut 6:14a9c4f30c86 347 //pc.printf("MotorOff\r\n");
aschut 6:14a9c4f30c86 348 PotRead.detach();
aschut 6:14a9c4f30c86 349 MotorOff();
aschut 6:14a9c4f30c86 350 }
aschut 4:99f7fdce608e 351 led = 0;
aschut 17:190c86f4828f 352
aschut 20:ad425c7f84b6 353 if(counter==10) { // Dit zend waardes naar matlab, open door het script 'serialread' te gebruiken
aschut 6:14a9c4f30c86 354 float tmp = t.read();
aschut 19:24f5e36c6490 355 printf("%f,%f,%f,%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,error2,Kp2,Ki2,Kd2);
aschut 6:14a9c4f30c86 356 counter = 0;
aschut 6:14a9c4f30c86 357 }
aschut 6:14a9c4f30c86 358 counter++;
aschut 6:14a9c4f30c86 359 wait(0.001);
aschut 1:58f34947c674 360 }
aschut 6:14a9c4f30c86 361 }
aschut 0:a9a42914138c 362
aschut 0:a9a42914138c 363
aschut 6:14a9c4f30c86 364