M9 Biorobotics Group 8 / Mbed 2 deprecated motorcalibration_StateMasjien

Moet dit er bij

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@6:e7e39d116ed0, 2019-10-29 (annotated)

Committer:
BasB
Date:
Tue Oct 29 16:11:21 2019 +0000
Revision:
6:e7e39d116ed0
Parent:
5:4d8b85b7cfc4
Child:
7:676a83def149
Manual calibratie toegevoegd met button 2 door

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BasB 0:335646ab45c0 1 #include "mbed.h"
BasB 0:335646ab45c0 2 #include "HIDScope.h"
BasB 0:335646ab45c0 3 #include "QEI.h"
BasB 0:335646ab45c0 4 #include "MODSERIAL.h"
BasB 0:335646ab45c0 5 #include "BiQuad.h"
BasB 0:335646ab45c0 6 #include "FastPWM.h"
BasB 0:335646ab45c0 7
BasB 0:335646ab45c0 8 // Button and potmeter1 control
BasB 0:335646ab45c0 9 InterruptIn button1(D11);
BasB 0:335646ab45c0 10 InterruptIn button2(D10);
BasB 0:335646ab45c0 11 InterruptIn buttonsw2(SW2);
BasB 0:335646ab45c0 12 InterruptIn buttonsw3(SW3);
BasB 0:335646ab45c0 13 AnalogIn potmeter1(A0);
BasB 0:335646ab45c0 14 AnalogIn potmeter2(A1);
BasB 0:335646ab45c0 15 AnalogIn potmeter3(A2);
BasB 0:335646ab45c0 16 AnalogIn potmeter4(A3);
BasB 2:7ea5ae2287a7 17
BasB 0:335646ab45c0 18 // Encoder
BasB 0:335646ab45c0 19 DigitalIn encA1(D9);
BasB 0:335646ab45c0 20 DigitalIn encB1(D8);
BasB 0:335646ab45c0 21 DigitalIn encA2(D13);
BasB 0:335646ab45c0 22 DigitalIn encB2(D13);
BasB 0:335646ab45c0 23 QEI encoder1(D9,D8,NC,64,QEI::X4_ENCODING); //Encoding motor 1
BasB 0:335646ab45c0 24 QEI encoder2(D13,D12,NC,64,QEI::X4_ENCODING); //Encoding motor 2
BasB 0:335646ab45c0 25 float Ts = 0.01; //Sample time
BasB 0:335646ab45c0 26 float motor1angle; //Measured angle motor 1
BasB 0:335646ab45c0 27 float motor2angle; //Measured angle motor 2
BasB 2:7ea5ae2287a7 28 float potmeter;
BasB 4:1e8da6b5f147 29 float omega1; //velocity rad/s motor 1
BasB 0:335646ab45c0 30 float omega2; //Velocity rad/s motor2
BasB 0:335646ab45c0 31 float deg2rad=0.0174532; //Conversion factor degree to rad
BasB 0:335646ab45c0 32 float rad2deg=57.29578; //Conversion factor rad to degree
BasB 0:335646ab45c0 33
BasB 0:335646ab45c0 34
BasB 0:335646ab45c0 35 // Motor
BasB 0:335646ab45c0 36 DigitalOut motor2Direction(D4);
BasB 0:335646ab45c0 37 FastPWM motor2Power(D5);
BasB 0:335646ab45c0 38 DigitalOut motor1Direction(D7);
BasB 0:335646ab45c0 39 FastPWM motor1Power(D6);
BasB 0:335646ab45c0 40
BasB 0:335646ab45c0 41 //Motorcontrol
BasB 0:335646ab45c0 42 bool motordir1;
BasB 0:335646ab45c0 43 bool motordir2;
BasB 0:335646ab45c0 44 float motor1ref=0.1745;
BasB 0:335646ab45c0 45 float motor2ref=0.0873;
BasB 0:335646ab45c0 46 double controlsignal1;
BasB 0:335646ab45c0 47 double controlsignal2;
BasB 0:335646ab45c0 48 double pi2= 6.283185;
BasB 0:335646ab45c0 49 float motor1error; //motor 1 error
BasB 0:335646ab45c0 50 float motor2error;
BasB 0:335646ab45c0 51 float Kp=0.27;
BasB 0:335646ab45c0 52 float Ki=0.35;
BasB 0:335646ab45c0 53 float Kd=0.1;
BasB 0:335646ab45c0 54 float u_p1;
BasB 0:335646ab45c0 55 float u_p2;
BasB 0:335646ab45c0 56 float u_i1;
BasB 0:335646ab45c0 57 float u_i2;
BasB 0:335646ab45c0 58
BasB 0:335646ab45c0 59 //Windup control
BasB 0:335646ab45c0 60 float ux1;
BasB 0:335646ab45c0 61 float ux2;
BasB 0:335646ab45c0 62 float up1; //Proportional contribution motor 1
BasB 0:335646ab45c0 63 float up2; //Proportional contribution motor 2
BasB 0:335646ab45c0 64 float ek1;
BasB 0:335646ab45c0 65 float ek2;
BasB 0:335646ab45c0 66 float ei1= 0.0; //Error integral motor 1
BasB 0:335646ab45c0 67 float ei2=0.0; //Error integral motor 2
BasB 0:335646ab45c0 68 float Ka= 1.0; //Integral windup gain
BasB 0:335646ab45c0 69
BasB 0:335646ab45c0 70 //RKI
BasB 0:335646ab45c0 71 float Vx=0.0; //Desired linear velocity x direction
BasB 0:335646ab45c0 72 float Vy=0.0; //Desired linear velocity y direction
BasB 0:335646ab45c0 73 float q1=0.0f*deg2rad; //Angle of first joint [rad]
BasB 0:335646ab45c0 74 float q2=-135.0f*deg2rad; //Angle of second joint [rad]
BasB 0:335646ab45c0 75 float q1dot; //Velocity of first joint [rad/s]
BasB 0:335646ab45c0 76 float q2dot; //Velocity of second joint [rad/s]
BasB 0:335646ab45c0 77 float l1=26.0; //Distance base-link [cm]
BasB 0:335646ab45c0 78 float l2=62.0; //Distance link-endpoint [cm]
BasB 0:335646ab45c0 79 float xe; //Endpoint x position [cm]
BasB 0:335646ab45c0 80 float ye; //Endpoint y position [cm]
BasB 0:335646ab45c0 81
BasB 0:335646ab45c0 82 //Hidscope
BasB 0:335646ab45c0 83 HIDScope scope(6); //Going to send x channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.
BasB 0:335646ab45c0 84
BasB 3:6e28b992b99e 85 //State maschine
BasB 6:e7e39d116ed0 86 enum Motor_States{motor_wait , motor_cal1_start , motor_cal1 , motor_cal2_start , motor_cal2 , motor_cal_manual , motor_encoderset};
BasB 3:6e28b992b99e 87 Motor_States motor_curr_state;
BasB 3:6e28b992b99e 88 bool motor_state_changed = true;
BasB 3:6e28b992b99e 89 bool motor_cal1_done = false;
BasB 3:6e28b992b99e 90 bool motor_cal2_done = false;
BasB 3:6e28b992b99e 91
BasB 3:6e28b992b99e 92 bool button1_pressed = false;
BasB 3:6e28b992b99e 93 bool button2_pressed = false;
BasB 3:6e28b992b99e 94
BasB 0:335646ab45c0 95 // PC connection
BasB 0:335646ab45c0 96 MODSERIAL pc(USBTX, USBRX);
BasB 0:335646ab45c0 97
BasB 0:335646ab45c0 98 // Intializing tickers
BasB 0:335646ab45c0 99 Ticker motorTicker;
BasB 0:335646ab45c0 100 Ticker controlTicker;
BasB 0:335646ab45c0 101 Ticker directionTicker;
BasB 0:335646ab45c0 102 Ticker encoderTicker;
BasB 0:335646ab45c0 103 Ticker scopeTicker;
BasB 3:6e28b992b99e 104 Ticker tickGlobal; //Global ticker
BasB 0:335646ab45c0 105
BasB 0:335646ab45c0 106 const float PWM_period = 1e-6;
BasB 0:335646ab45c0 107
BasB 0:335646ab45c0 108 volatile int counts1; // Encoder counts
BasB 0:335646ab45c0 109 volatile int counts2;
BasB 0:335646ab45c0 110 volatile int countsPrev1 = 0;
BasB 0:335646ab45c0 111 volatile int countsPrev2 = 0;
BasB 0:335646ab45c0 112 volatile int deltaCounts1;
BasB 0:335646ab45c0 113 volatile int deltaCounts2;
BasB 0:335646ab45c0 114
BasB 0:335646ab45c0 115 float factorin = 6.23185/64; // Convert encoder counts to angle in rad
BasB 0:335646ab45c0 116 float gearratio = 131.25; // Gear ratio of gearbox
BasB 0:335646ab45c0 117
BasB 3:6e28b992b99e 118 void button1Press()
BasB 3:6e28b992b99e 119 {
BasB 3:6e28b992b99e 120 button1_pressed = true;
BasB 3:6e28b992b99e 121 }
BasB 0:335646ab45c0 122
BasB 6:e7e39d116ed0 123 void button2Press()
BasB 6:e7e39d116ed0 124 {
BasB 6:e7e39d116ed0 125 button2_pressed = true;
BasB 6:e7e39d116ed0 126 }
BasB 0:335646ab45c0 127
BasB 3:6e28b992b99e 128 // Ticker Functions
BasB 0:335646ab45c0 129 void readEncoder()
BasB 0:335646ab45c0 130 {
BasB 0:335646ab45c0 131 counts1 = encoder1.getPulses();
BasB 0:335646ab45c0 132 deltaCounts1 = counts1 - countsPrev1;
BasB 0:335646ab45c0 133 countsPrev1 = counts1;
BasB 0:335646ab45c0 134
BasB 0:335646ab45c0 135 counts2 = encoder2.getPulses();
BasB 0:335646ab45c0 136 deltaCounts2 = counts2 - countsPrev2;
BasB 0:335646ab45c0 137 countsPrev2 = counts2;
BasB 0:335646ab45c0 138 }
BasB 0:335646ab45c0 139
BasB 4:1e8da6b5f147 140 void do_motor_cal1_start(){
BasB 4:1e8da6b5f147 141 // Entry function
BasB 4:1e8da6b5f147 142 if ( motor_state_changed == true ) {
BasB 4:1e8da6b5f147 143 motor_state_changed = false;
BasB 4:1e8da6b5f147 144 // More functions
BasB 4:1e8da6b5f147 145 }
BasB 4:1e8da6b5f147 146
BasB 4:1e8da6b5f147 147 //Do stuff
BasB 4:1e8da6b5f147 148 motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
BasB 4:1e8da6b5f147 149 omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 4:1e8da6b5f147 150 controlsignal1=0.10f;
BasB 6:e7e39d116ed0 151 motor1Power.write(abs(controlsignal1));
BasB 4:1e8da6b5f147 152 motor1Direction=0;
BasB 4:1e8da6b5f147 153
BasB 4:1e8da6b5f147 154 // State transition guard
BasB 4:1e8da6b5f147 155 if ( abs(omega1) >= 1.5f ){
BasB 4:1e8da6b5f147 156 motor_curr_state = motor_cal1;
BasB 4:1e8da6b5f147 157 motor_state_changed = true;
BasB 4:1e8da6b5f147 158 // More functions
BasB 4:1e8da6b5f147 159 }
BasB 4:1e8da6b5f147 160
BasB 4:1e8da6b5f147 161 }
BasB 4:1e8da6b5f147 162
BasB 3:6e28b992b99e 163 void do_motorCalibration1() {
BasB 3:6e28b992b99e 164 // Entry function
BasB 3:6e28b992b99e 165 if ( motor_state_changed == true ) {
BasB 3:6e28b992b99e 166 motor_state_changed = false;
BasB 3:6e28b992b99e 167 // More functions
BasB 3:6e28b992b99e 168 }
BasB 3:6e28b992b99e 169
BasB 3:6e28b992b99e 170 // Do stuff until end condition is met
BasB 1:a76fd17e18b3 171 motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
BasB 0:335646ab45c0 172 omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 2:7ea5ae2287a7 173 controlsignal1=0.0980f;
BasB 6:e7e39d116ed0 174 motor1Power.write(abs(controlsignal1));
BasB 2:7ea5ae2287a7 175 motor1Direction=0;
BasB 3:6e28b992b99e 176
BasB 3:6e28b992b99e 177 // State transition guard
BasB 4:1e8da6b5f147 178 if ( abs(omega1) <= 0.5f ) {
BasB 4:1e8da6b5f147 179 motor_curr_state = motor_cal2_start;
BasB 3:6e28b992b99e 180 motor_state_changed = true;
BasB 3:6e28b992b99e 181 // More functions
BasB 3:6e28b992b99e 182 }
BasB 3:6e28b992b99e 183 }
BasB 3:6e28b992b99e 184
BasB 5:4d8b85b7cfc4 185 void do_motor_cal2_start(){
BasB 5:4d8b85b7cfc4 186 // Entry function
BasB 5:4d8b85b7cfc4 187 if ( motor_state_changed == true ) {
BasB 5:4d8b85b7cfc4 188 motor_state_changed = false;
BasB 5:4d8b85b7cfc4 189 // More functions
BasB 5:4d8b85b7cfc4 190 }
BasB 5:4d8b85b7cfc4 191 //stuff
BasB 5:4d8b85b7cfc4 192 motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
BasB 5:4d8b85b7cfc4 193 omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 5:4d8b85b7cfc4 194 controlsignal1=0.10f;
BasB 6:e7e39d116ed0 195 motor1Power.write(abs(controlsignal1));
BasB 5:4d8b85b7cfc4 196 motor1Direction=0;
BasB 5:4d8b85b7cfc4 197
BasB 5:4d8b85b7cfc4 198 motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad
BasB 5:4d8b85b7cfc4 199 omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 5:4d8b85b7cfc4 200 controlsignal2=0.10f;
BasB 6:e7e39d116ed0 201 motor2Power.write(abs(controlsignal2));
BasB 5:4d8b85b7cfc4 202 motor2Direction=0;
BasB 5:4d8b85b7cfc4 203
BasB 5:4d8b85b7cfc4 204 // State transition guard
BasB 5:4d8b85b7cfc4 205 if ( abs(omega2) >= 1.5f ) {
BasB 5:4d8b85b7cfc4 206 motor_curr_state = motor_cal2;
BasB 5:4d8b85b7cfc4 207 motor_state_changed = true;
BasB 5:4d8b85b7cfc4 208 // More functions
BasB 5:4d8b85b7cfc4 209 }
BasB 5:4d8b85b7cfc4 210 }
BasB 4:1e8da6b5f147 211
BasB 3:6e28b992b99e 212 void do_motorCalibration2(){
BasB 3:6e28b992b99e 213 // Entry function
BasB 3:6e28b992b99e 214 if ( motor_state_changed == true ) {
BasB 3:6e28b992b99e 215 motor_state_changed = false;
BasB 3:6e28b992b99e 216 // More functions
BasB 1:a76fd17e18b3 217 }
BasB 1:a76fd17e18b3 218
BasB 3:6e28b992b99e 219 // Do stuff until end condition is met
BasB 1:a76fd17e18b3 220 motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
BasB 1:a76fd17e18b3 221 omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 5:4d8b85b7cfc4 222 controlsignal1=0.10f;
BasB 6:e7e39d116ed0 223 motor1Power.write(abs(controlsignal1));
BasB 5:4d8b85b7cfc4 224 motor1Direction=0;
BasB 0:335646ab45c0 225
BasB 0:335646ab45c0 226 motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad
BasB 5:4d8b85b7cfc4 227 omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 5:4d8b85b7cfc4 228 controlsignal2=0.10f;
BasB 6:e7e39d116ed0 229 motor2Power.write(abs(controlsignal2));
BasB 5:4d8b85b7cfc4 230 motor2Direction=0;
BasB 3:6e28b992b99e 231
BasB 3:6e28b992b99e 232 // State transition guard
BasB 5:4d8b85b7cfc4 233 if ( abs(omega2) <= 0.5f ) {
BasB 3:6e28b992b99e 234 motor_curr_state = motor_encoderset;
BasB 3:6e28b992b99e 235 motor_state_changed = true;
BasB 3:6e28b992b99e 236 // More functions
BasB 3:6e28b992b99e 237 }
BasB 0:335646ab45c0 238 }
BasB 0:335646ab45c0 239
BasB 3:6e28b992b99e 240 void do_motor_Encoder_Set(){
BasB 3:6e28b992b99e 241 // Entry function
BasB 3:6e28b992b99e 242 if ( motor_state_changed == true ) {
BasB 3:6e28b992b99e 243 motor_state_changed = false;
BasB 3:6e28b992b99e 244 // More functions
BasB 3:6e28b992b99e 245 }
BasB 3:6e28b992b99e 246
BasB 3:6e28b992b99e 247 // Do stuff until end condition is met
BasB 3:6e28b992b99e 248
BasB 3:6e28b992b99e 249
BasB 3:6e28b992b99e 250 // State transition guard
BasB 3:6e28b992b99e 251 if ( omega2 <= 0.5f ) {
BasB 3:6e28b992b99e 252 motor_curr_state = motor_encoderset;
BasB 3:6e28b992b99e 253 motor_state_changed = true;
BasB 3:6e28b992b99e 254 // More functions
BasB 3:6e28b992b99e 255 }
BasB 3:6e28b992b99e 256 }
BasB 3:6e28b992b99e 257
BasB 3:6e28b992b99e 258 void do_motor_wait(){
BasB 3:6e28b992b99e 259 // Entry function
BasB 3:6e28b992b99e 260 if ( motor_state_changed == true ) {
BasB 3:6e28b992b99e 261 motor_state_changed = false;
BasB 3:6e28b992b99e 262 // More functions
BasB 3:6e28b992b99e 263 }
BasB 3:6e28b992b99e 264
BasB 3:6e28b992b99e 265 // Do nothing until end condition is met
BasB 3:6e28b992b99e 266
BasB 3:6e28b992b99e 267 // State transition guard
BasB 6:e7e39d116ed0 268 if ( button1_pressed ) {
BasB 3:6e28b992b99e 269 button1_pressed = false;
BasB 4:1e8da6b5f147 270 motor_curr_state = motor_cal1_start; //Beginnen met calibratie
BasB 3:6e28b992b99e 271 motor_state_changed = true;
BasB 3:6e28b992b99e 272 // More functions
BasB 6:e7e39d116ed0 273 }
BasB 6:e7e39d116ed0 274 else if ( button2_pressed ) {
BasB 6:e7e39d116ed0 275 button2_pressed = false;
BasB 6:e7e39d116ed0 276 motor_curr_state = motor_cal_manual; //Beginnen met calibratie
BasB 6:e7e39d116ed0 277 motor_state_changed = true;
BasB 6:e7e39d116ed0 278 // More functions
BasB 6:e7e39d116ed0 279 }
BasB 6:e7e39d116ed0 280
BasB 3:6e28b992b99e 281 }
BasB 3:6e28b992b99e 282
BasB 6:e7e39d116ed0 283 void do_motor_cal_manual(){
BasB 6:e7e39d116ed0 284 // Entry function
BasB 6:e7e39d116ed0 285 if ( motor_state_changed == true ) {
BasB 6:e7e39d116ed0 286 motor_state_changed = false;
BasB 6:e7e39d116ed0 287 // More functions
BasB 6:e7e39d116ed0 288 }
BasB 6:e7e39d116ed0 289
BasB 6:e7e39d116ed0 290 //Op dit moment moet je de robotarm handmatig naar zijn beginpositie brengen,
BasB 6:e7e39d116ed0 291 //en op het knopje klikken om door te gaan
BasB 6:e7e39d116ed0 292
BasB 6:e7e39d116ed0 293 if ( button2_pressed ) {
BasB 6:e7e39d116ed0 294 button2_pressed = false;
BasB 6:e7e39d116ed0 295 motor_curr_state = motor_encoderset;
BasB 6:e7e39d116ed0 296 motor_state_changed = true;
BasB 6:e7e39d116ed0 297 // More functions
BasB 6:e7e39d116ed0 298 }
BasB 0:335646ab45c0 299 }
BasB 0:335646ab45c0 300
BasB 6:e7e39d116ed0 301
BasB 3:6e28b992b99e 302 void motor_state_machine()
BasB 3:6e28b992b99e 303 {
BasB 3:6e28b992b99e 304 switch(motor_curr_state) {
BasB 3:6e28b992b99e 305 case motor_wait:
BasB 3:6e28b992b99e 306 do_motor_wait();
BasB 3:6e28b992b99e 307 break;
BasB 4:1e8da6b5f147 308 case motor_cal1_start:
BasB 4:1e8da6b5f147 309 do_motor_cal1_start();
BasB 4:1e8da6b5f147 310 break;
BasB 3:6e28b992b99e 311 case motor_cal1:
BasB 3:6e28b992b99e 312 do_motorCalibration1();
BasB 3:6e28b992b99e 313 break;
BasB 4:1e8da6b5f147 314 case motor_cal2_start:
BasB 4:1e8da6b5f147 315 do_motor_cal2_start();
BasB 4:1e8da6b5f147 316 break;
BasB 3:6e28b992b99e 317 case motor_cal2:
BasB 3:6e28b992b99e 318 do_motorCalibration2();
BasB 3:6e28b992b99e 319 break;
BasB 3:6e28b992b99e 320 case motor_encoderset:
BasB 3:6e28b992b99e 321 do_motor_Encoder_Set();
BasB 3:6e28b992b99e 322 break;
BasB 6:e7e39d116ed0 323 case motor_cal_manual:
BasB 6:e7e39d116ed0 324 do_motor_cal_manual();
BasB 6:e7e39d116ed0 325 break;
BasB 3:6e28b992b99e 326 }
BasB 3:6e28b992b99e 327 }
BasB 3:6e28b992b99e 328
BasB 3:6e28b992b99e 329 // Global loop of program
BasB 3:6e28b992b99e 330 void tickGlobalFunc()
BasB 3:6e28b992b99e 331 {
BasB 3:6e28b992b99e 332 //sampleSignal();
BasB 3:6e28b992b99e 333 //emg_state_machine();
BasB 3:6e28b992b99e 334 motor_state_machine();
BasB 4:1e8da6b5f147 335 readEncoder();
BasB 3:6e28b992b99e 336 // controller();
BasB 3:6e28b992b99e 337 // outputToMotors();
BasB 3:6e28b992b99e 338 }
BasB 3:6e28b992b99e 339
BasB 3:6e28b992b99e 340
BasB 0:335646ab45c0 341 int main()
BasB 0:335646ab45c0 342 {
BasB 0:335646ab45c0 343 pc.baud(115200);
BasB 0:335646ab45c0 344 pc.printf("\r\nStarting...\r\n\r\n");
BasB 0:335646ab45c0 345 motor1Power.period(PWM_period);
BasB 0:335646ab45c0 346 motor2Power.period(PWM_period);
BasB 2:7ea5ae2287a7 347
BasB 3:6e28b992b99e 348 motor_curr_state = motor_wait; // Start off in EMG Wait state
BasB 3:6e28b992b99e 349 tickGlobal.attach( &tickGlobalFunc, Ts );
BasB 0:335646ab45c0 350
BasB 4:1e8da6b5f147 351 button1.fall(&button1Press);
BasB 6:e7e39d116ed0 352 button2.fall(&button2Press);
BasB 0:335646ab45c0 353
BasB 0:335646ab45c0 354 while (true) {
BasB 2:7ea5ae2287a7 355 pc.printf("Omega1: %f Omega 2: %f controlsignal1: %f \r\n", omega1, omega2, controlsignal1);
BasB 4:1e8da6b5f147 356 pc.printf("Currentstate: %i motor_cal1: %i\r\n",motor_curr_state, motor_cal1);
BasB 0:335646ab45c0 357 wait(0.5);
BasB 0:335646ab45c0 358 }
BasB 0:335646ab45c0 359 }