M9 Biorobotics Group 8 / Mbed 2 deprecated motorcalibration_StateMasjien

Moet dit er bij

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@9:298469a70280, 2019-10-29 (annotated)

Committer:
BasB
Date:
Tue Oct 29 18:55:53 2019 +0000
Revision:
9:298469a70280
Parent:
8:3cfc8be293d3
Child:
10:990287a722d2
RKI, PID, Motorcontrol states/functies toegevoegd

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 7:676a83def149 28 float motor1offset; //Offset bij calibratie
BasB 7:676a83def149 29 float motor2offset;
BasB 2:7ea5ae2287a7 30 float potmeter;
BasB 4:1e8da6b5f147 31 float omega1; //velocity rad/s motor 1
BasB 0:335646ab45c0 32 float omega2; //Velocity rad/s motor2
BasB 0:335646ab45c0 33 float deg2rad=0.0174532; //Conversion factor degree to rad
BasB 0:335646ab45c0 34 float rad2deg=57.29578; //Conversion factor rad to degree
BasB 0:335646ab45c0 35
BasB 0:335646ab45c0 36
BasB 0:335646ab45c0 37 // Motor
BasB 0:335646ab45c0 38 DigitalOut motor2Direction(D4);
BasB 0:335646ab45c0 39 FastPWM motor2Power(D5);
BasB 0:335646ab45c0 40 DigitalOut motor1Direction(D7);
BasB 0:335646ab45c0 41 FastPWM motor1Power(D6);
BasB 0:335646ab45c0 42
BasB 0:335646ab45c0 43 //Motorcontrol
BasB 0:335646ab45c0 44 bool motordir1;
BasB 0:335646ab45c0 45 bool motordir2;
BasB 0:335646ab45c0 46 float motor1ref=0.1745;
BasB 0:335646ab45c0 47 float motor2ref=0.0873;
BasB 0:335646ab45c0 48 double controlsignal1;
BasB 0:335646ab45c0 49 double controlsignal2;
BasB 0:335646ab45c0 50 double pi2= 6.283185;
BasB 0:335646ab45c0 51 float motor1error; //motor 1 error
BasB 0:335646ab45c0 52 float motor2error;
BasB 0:335646ab45c0 53 float Kp=0.27;
BasB 0:335646ab45c0 54 float Ki=0.35;
BasB 0:335646ab45c0 55 float Kd=0.1;
BasB 0:335646ab45c0 56 float u_p1;
BasB 0:335646ab45c0 57 float u_p2;
BasB 0:335646ab45c0 58 float u_i1;
BasB 0:335646ab45c0 59 float u_i2;
BasB 0:335646ab45c0 60
BasB 0:335646ab45c0 61 //Windup control
BasB 0:335646ab45c0 62 float ux1;
BasB 0:335646ab45c0 63 float ux2;
BasB 0:335646ab45c0 64 float up1; //Proportional contribution motor 1
BasB 0:335646ab45c0 65 float up2; //Proportional contribution motor 2
BasB 0:335646ab45c0 66 float ek1;
BasB 0:335646ab45c0 67 float ek2;
BasB 0:335646ab45c0 68 float ei1= 0.0; //Error integral motor 1
BasB 0:335646ab45c0 69 float ei2=0.0; //Error integral motor 2
BasB 0:335646ab45c0 70 float Ka= 1.0; //Integral windup gain
BasB 0:335646ab45c0 71
BasB 0:335646ab45c0 72 //RKI
BasB 0:335646ab45c0 73 float Vx=0.0; //Desired linear velocity x direction
BasB 0:335646ab45c0 74 float Vy=0.0; //Desired linear velocity y direction
BasB 0:335646ab45c0 75 float q1=0.0f*deg2rad; //Angle of first joint [rad]
BasB 0:335646ab45c0 76 float q2=-135.0f*deg2rad; //Angle of second joint [rad]
BasB 0:335646ab45c0 77 float q1dot; //Velocity of first joint [rad/s]
BasB 0:335646ab45c0 78 float q2dot; //Velocity of second joint [rad/s]
BasB 0:335646ab45c0 79 float l1=26.0; //Distance base-link [cm]
BasB 0:335646ab45c0 80 float l2=62.0; //Distance link-endpoint [cm]
BasB 0:335646ab45c0 81 float xe; //Endpoint x position [cm]
BasB 0:335646ab45c0 82 float ye; //Endpoint y position [cm]
BasB 0:335646ab45c0 83
BasB 0:335646ab45c0 84 //Hidscope
BasB 0:335646ab45c0 85 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 86
BasB 3:6e28b992b99e 87 //State maschine
BasB 9:298469a70280 88 enum Motor_States{motor_wait , motor_encoderset , motor_wait2 , motor_bewegen};
BasB 3:6e28b992b99e 89 Motor_States motor_curr_state;
BasB 3:6e28b992b99e 90 bool motor_state_changed = true;
BasB 9:298469a70280 91 bool motor_calibration_done = false;
BasB 9:298469a70280 92 bool motor_RKI=false;
BasB 3:6e28b992b99e 93 bool button1_pressed = false;
BasB 3:6e28b992b99e 94 bool button2_pressed = false;
BasB 3:6e28b992b99e 95
BasB 0:335646ab45c0 96 // PC connection
BasB 0:335646ab45c0 97 MODSERIAL pc(USBTX, USBRX);
BasB 0:335646ab45c0 98
BasB 0:335646ab45c0 99 // Intializing tickers
BasB 0:335646ab45c0 100 Ticker motorTicker;
BasB 0:335646ab45c0 101 Ticker controlTicker;
BasB 0:335646ab45c0 102 Ticker directionTicker;
BasB 0:335646ab45c0 103 Ticker encoderTicker;
BasB 0:335646ab45c0 104 Ticker scopeTicker;
BasB 3:6e28b992b99e 105 Ticker tickGlobal; //Global ticker
BasB 0:335646ab45c0 106
BasB 0:335646ab45c0 107 const float PWM_period = 1e-6;
BasB 0:335646ab45c0 108
BasB 0:335646ab45c0 109 volatile int counts1; // Encoder counts
BasB 0:335646ab45c0 110 volatile int counts2;
BasB 0:335646ab45c0 111 volatile int countsPrev1 = 0;
BasB 0:335646ab45c0 112 volatile int countsPrev2 = 0;
BasB 0:335646ab45c0 113 volatile int deltaCounts1;
BasB 0:335646ab45c0 114 volatile int deltaCounts2;
BasB 0:335646ab45c0 115
BasB 0:335646ab45c0 116 float factorin = 6.23185/64; // Convert encoder counts to angle in rad
BasB 0:335646ab45c0 117 float gearratio = 131.25; // Gear ratio of gearbox
BasB 0:335646ab45c0 118
BasB 3:6e28b992b99e 119 void button1Press()
BasB 3:6e28b992b99e 120 {
BasB 3:6e28b992b99e 121 button1_pressed = true;
BasB 3:6e28b992b99e 122 }
BasB 0:335646ab45c0 123
BasB 6:e7e39d116ed0 124 void button2Press()
BasB 6:e7e39d116ed0 125 {
BasB 6:e7e39d116ed0 126 button2_pressed = true;
BasB 6:e7e39d116ed0 127 }
BasB 0:335646ab45c0 128
BasB 3:6e28b992b99e 129 // Ticker Functions
BasB 0:335646ab45c0 130 void readEncoder()
BasB 0:335646ab45c0 131 {
BasB 0:335646ab45c0 132 counts1 = encoder1.getPulses();
BasB 0:335646ab45c0 133 deltaCounts1 = counts1 - countsPrev1;
BasB 0:335646ab45c0 134 countsPrev1 = counts1;
BasB 0:335646ab45c0 135
BasB 0:335646ab45c0 136 counts2 = encoder2.getPulses();
BasB 0:335646ab45c0 137 deltaCounts2 = counts2 - countsPrev2;
BasB 0:335646ab45c0 138 countsPrev2 = counts2;
BasB 0:335646ab45c0 139 }
BasB 0:335646ab45c0 140
BasB 9:298469a70280 141 void PID_controller(){
BasB 9:298469a70280 142
BasB 9:298469a70280 143 static float error_integral1=0;
BasB 9:298469a70280 144 static float e_prev1=motor1error;
BasB 9:298469a70280 145
BasB 9:298469a70280 146 //Proportional part:
BasB 9:298469a70280 147 u_p1=Kp*motor1error;
BasB 9:298469a70280 148
BasB 9:298469a70280 149 //Integral part
BasB 9:298469a70280 150 error_integral1=error_integral1+ei1*Ts;
BasB 9:298469a70280 151 u_i1=Ki*error_integral1;
BasB 9:298469a70280 152
BasB 9:298469a70280 153 //Derivative part
BasB 9:298469a70280 154 float error_derivative1=(motor1error-e_prev1)/Ts;
BasB 9:298469a70280 155 float u_d1=Kd*error_derivative1;
BasB 9:298469a70280 156 e_prev1=motor1error;
BasB 9:298469a70280 157
BasB 9:298469a70280 158 // Sum and limit
BasB 9:298469a70280 159 up1= u_p1+u_i1+u_d1;
BasB 9:298469a70280 160 if (up1>1){
BasB 9:298469a70280 161 controlsignal1=1;}
BasB 9:298469a70280 162 else if (up1<-1){
BasB 9:298469a70280 163 controlsignal1=-1;}
BasB 9:298469a70280 164 else {
BasB 9:298469a70280 165 controlsignal1=up1;}
BasB 9:298469a70280 166
BasB 9:298469a70280 167 // To prevent windup
BasB 9:298469a70280 168 ux1= up1-controlsignal1;
BasB 9:298469a70280 169 ek1= Ka*ux1;
BasB 9:298469a70280 170 ei1= motor1error-ek1;
BasB 9:298469a70280 171
BasB 9:298469a70280 172 // Motor 2
BasB 9:298469a70280 173
BasB 9:298469a70280 174 static float error_integral2=0;
BasB 9:298469a70280 175 static float e_prev2=motor2error;
BasB 9:298469a70280 176
BasB 9:298469a70280 177 //Proportional part:
BasB 9:298469a70280 178 u_p2=Kp*motor2error;
BasB 9:298469a70280 179
BasB 9:298469a70280 180 //Integral part
BasB 9:298469a70280 181 error_integral2=error_integral2+ei2*Ts;
BasB 9:298469a70280 182 u_i2=Ki*error_integral2;
BasB 9:298469a70280 183
BasB 9:298469a70280 184 //Derivative part
BasB 9:298469a70280 185 float error_derivative2=(motor2error-e_prev2)/Ts;
BasB 9:298469a70280 186 float u_d2=Kd*error_derivative2;
BasB 9:298469a70280 187 e_prev2=motor2error;
BasB 9:298469a70280 188
BasB 9:298469a70280 189 // Sum and limit
BasB 9:298469a70280 190 up2= u_p2+u_i2+u_d2;
BasB 9:298469a70280 191 if (up2>1.0f){
BasB 9:298469a70280 192 controlsignal2=1.0f;}
BasB 9:298469a70280 193 else if (up2<-1){
BasB 9:298469a70280 194 controlsignal2=-1.0f;}
BasB 9:298469a70280 195 else {
BasB 9:298469a70280 196 controlsignal2=up2;}
BasB 9:298469a70280 197
BasB 9:298469a70280 198 // To prevent windup
BasB 9:298469a70280 199 ux2= up2-controlsignal2;
BasB 9:298469a70280 200 ek2= Ka*ux2;
BasB 9:298469a70280 201 ei2= motor2error-ek2;
BasB 9:298469a70280 202 }
BasB 9:298469a70280 203
BasB 9:298469a70280 204 void RKI(){
BasB 9:298469a70280 205
BasB 9:298469a70280 206 if (motor_RKI==true){
BasB 9:298469a70280 207 //Vy=potmeter1.read()*10.0*motortoggle;
BasB 9:298469a70280 208 //Vy=potmeter2.read()*10*motortoggle;
BasB 9:298469a70280 209 static float t=0;
BasB 9:298469a70280 210 Vx=6.0f*sin(1.0f*t);
BasB 9:298469a70280 211 t+=Ts;
BasB 9:298469a70280 212 //Vx=-1.0*;
BasB 9:298469a70280 213 Vy=0.0f;
BasB 9:298469a70280 214 q1dot=(l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
BasB 9:298469a70280 215 q2dot=((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
BasB 9:298469a70280 216 q1=q1+q1dot*Ts;
BasB 9:298469a70280 217 q2=q2+q2dot*Ts;
BasB 9:298469a70280 218
BasB 9:298469a70280 219 xe=l1*cos(q1)+l2*cos(q1+q2);
BasB 9:298469a70280 220 ye=l1*sin(q1)+l2*sin(q1+q2);
BasB 9:298469a70280 221
BasB 9:298469a70280 222
BasB 9:298469a70280 223
BasB 9:298469a70280 224 if (q1<0.0f){
BasB 9:298469a70280 225 q1=0.0;}
BasB 9:298469a70280 226 else if (q1>90.0f*deg2rad){
BasB 9:298469a70280 227 q1=90.0f*deg2rad;}
BasB 9:298469a70280 228 else{
BasB 9:298469a70280 229 q1=q1;}
BasB 9:298469a70280 230
BasB 9:298469a70280 231 if (q2>-45.0*deg2rad){
BasB 9:298469a70280 232 q2=-45.0*deg2rad;}
BasB 9:298469a70280 233 else if (q2<-135.0*deg2rad){
BasB 9:298469a70280 234 q2=-135.0*deg2rad;}
BasB 9:298469a70280 235 else{
BasB 9:298469a70280 236 q2=q2;}
BasB 9:298469a70280 237
BasB 9:298469a70280 238 motor1ref=5.5f*q1+5.5f*q2;
BasB 9:298469a70280 239 motor2ref=2.75f*q1;
BasB 9:298469a70280 240 }
BasB 9:298469a70280 241 }
BasB 9:298469a70280 242
BasB 9:298469a70280 243
BasB 9:298469a70280 244 void motorControl(){
BasB 9:298469a70280 245 motor1angle = (counts1 * factorin / gearratio)-(135.0*5.5*deg2rad); // Angle of motor shaft in rad
BasB 9:298469a70280 246 omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 9:298469a70280 247 motor1error=motor1ref-motor1angle;
BasB 9:298469a70280 248 if (controlsignal1<0){
BasB 9:298469a70280 249 motordir1= 0;}
BasB 9:298469a70280 250 else {
BasB 9:298469a70280 251 motordir1= 1;}
BasB 9:298469a70280 252 motor1Power.write(abs(controlsignal1));
BasB 9:298469a70280 253 motor1Direction= motordir1;
BasB 9:298469a70280 254
BasB 9:298469a70280 255 motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad
BasB 9:298469a70280 256 omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 9:298469a70280 257 motor2error=motor2ref-motor2angle;
BasB 9:298469a70280 258 if (controlsignal2<0){
BasB 9:298469a70280 259 motordir2= 0;}
BasB 9:298469a70280 260 else {
BasB 9:298469a70280 261 motordir2= 1;}
BasB 9:298469a70280 262 motor2Power.write(abs(controlsignal2));
BasB 9:298469a70280 263 motor2Direction= motordir2;
BasB 9:298469a70280 264 }
BasB 9:298469a70280 265
BasB 8:3cfc8be293d3 266 void do_motor_wait(){
BasB 3:6e28b992b99e 267 // Entry function
BasB 3:6e28b992b99e 268 if ( motor_state_changed == true ) {
BasB 3:6e28b992b99e 269 motor_state_changed = false;
BasB 3:6e28b992b99e 270 // More functions
BasB 3:6e28b992b99e 271 }
BasB 3:6e28b992b99e 272
BasB 8:3cfc8be293d3 273 // Do nothing until end condition is met
BasB 5:4d8b85b7cfc4 274
BasB 8:3cfc8be293d3 275 // State transition guard
BasB 8:3cfc8be293d3 276 if ( button2_pressed ) {
BasB 8:3cfc8be293d3 277 button2_pressed = false;
BasB 8:3cfc8be293d3 278 motor_curr_state = motor_encoderset; //Beginnen met calibratie
BasB 3:6e28b992b99e 279 motor_state_changed = true;
BasB 3:6e28b992b99e 280 // More functions
BasB 3:6e28b992b99e 281 }
BasB 8:3cfc8be293d3 282
BasB 8:3cfc8be293d3 283 }
BasB 0:335646ab45c0 284
BasB 3:6e28b992b99e 285 void do_motor_Encoder_Set(){
BasB 3:6e28b992b99e 286 // Entry function
BasB 3:6e28b992b99e 287 if ( motor_state_changed == true ) {
BasB 3:6e28b992b99e 288 motor_state_changed = false;
BasB 3:6e28b992b99e 289 // More functions
BasB 3:6e28b992b99e 290 }
BasB 7:676a83def149 291 motor1Power.write(0.0f);
BasB 7:676a83def149 292 motor2Power.write(0.0f);
BasB 7:676a83def149 293 motor1offset = (counts1 * factorin / gearratio);
BasB 7:676a83def149 294 motor2offset = (counts2 * factorin / gearratio);
BasB 7:676a83def149 295
BasB 3:6e28b992b99e 296 // State transition guard
BasB 7:676a83def149 297 if ( button2_pressed ) {
BasB 9:298469a70280 298 button2_pressed = false;
BasB 9:298469a70280 299 motor_calibration_done = true;
BasB 9:298469a70280 300 motor_curr_state = motor_wait2;
BasB 3:6e28b992b99e 301 motor_state_changed = true;
BasB 3:6e28b992b99e 302 }
BasB 3:6e28b992b99e 303 }
BasB 3:6e28b992b99e 304
BasB 9:298469a70280 305 void do_motor_wait2(){
BasB 9:298469a70280 306 // Entry function
BasB 9:298469a70280 307 if ( motor_state_changed == true ) {
BasB 9:298469a70280 308 motor_state_changed = false;
BasB 9:298469a70280 309 // More functions
BasB 9:298469a70280 310 }
BasB 9:298469a70280 311
BasB 9:298469a70280 312 // Do nothing until end condition is met
BasB 9:298469a70280 313
BasB 9:298469a70280 314 // State transition guard
BasB 9:298469a70280 315 if ( button2_pressed ) {
BasB 9:298469a70280 316 button2_pressed = false;
BasB 9:298469a70280 317 motor_curr_state = motor_encoderset;
BasB 9:298469a70280 318 motor_state_changed = true;
BasB 9:298469a70280 319 // More functions
BasB 9:298469a70280 320 }
BasB 9:298469a70280 321
BasB 9:298469a70280 322 }
BasB 9:298469a70280 323
BasB 9:298469a70280 324 void do_motor_bewegen(){
BasB 9:298469a70280 325 // Entry function
BasB 9:298469a70280 326 if ( motor_state_changed == true ) {
BasB 9:298469a70280 327 motor_state_changed = false;
BasB 9:298469a70280 328 // More functions
BasB 9:298469a70280 329 }
BasB 9:298469a70280 330
BasB 9:298469a70280 331 motor_RKI=true;
BasB 9:298469a70280 332
BasB 9:298469a70280 333 if ( button2_pressed ) {
BasB 9:298469a70280 334 button2_pressed = false;
BasB 9:298469a70280 335 motor_RKI = false;
BasB 9:298469a70280 336 motor_curr_state = motor_wait2;
BasB 9:298469a70280 337 motor_state_changed = true;
BasB 9:298469a70280 338 // More functions
BasB 9:298469a70280 339 }
BasB 9:298469a70280 340
BasB 9:298469a70280 341
BasB 9:298469a70280 342 }
BasB 9:298469a70280 343
BasB 3:6e28b992b99e 344 void motor_state_machine()
BasB 3:6e28b992b99e 345 {
BasB 3:6e28b992b99e 346 switch(motor_curr_state) {
BasB 3:6e28b992b99e 347 case motor_wait:
BasB 3:6e28b992b99e 348 do_motor_wait();
BasB 3:6e28b992b99e 349 break;
BasB 3:6e28b992b99e 350 case motor_encoderset:
BasB 3:6e28b992b99e 351 do_motor_Encoder_Set();
BasB 3:6e28b992b99e 352 break;
BasB 9:298469a70280 353 case motor_wait2:
BasB 9:298469a70280 354 do_motor_wait2();
BasB 9:298469a70280 355 break;
BasB 9:298469a70280 356 case motor_bewegen:
BasB 9:298469a70280 357 do_motor_bewegen();
BasB 9:298469a70280 358 break;
BasB 3:6e28b992b99e 359 }
BasB 3:6e28b992b99e 360 }
BasB 3:6e28b992b99e 361
BasB 3:6e28b992b99e 362 // Global loop of program
BasB 3:6e28b992b99e 363 void tickGlobalFunc()
BasB 3:6e28b992b99e 364 {
BasB 3:6e28b992b99e 365 //sampleSignal();
BasB 3:6e28b992b99e 366 //emg_state_machine();
BasB 3:6e28b992b99e 367 motor_state_machine();
BasB 4:1e8da6b5f147 368 readEncoder();
BasB 9:298469a70280 369 PID_controller();
BasB 9:298469a70280 370 motorControl();
BasB 9:298469a70280 371 RKI();
BasB 9:298469a70280 372
BasB 3:6e28b992b99e 373 // controller();
BasB 3:6e28b992b99e 374 // outputToMotors();
BasB 3:6e28b992b99e 375 }
BasB 3:6e28b992b99e 376
BasB 3:6e28b992b99e 377
BasB 0:335646ab45c0 378 int main()
BasB 0:335646ab45c0 379 {
BasB 0:335646ab45c0 380 pc.baud(115200);
BasB 0:335646ab45c0 381 pc.printf("\r\nStarting...\r\n\r\n");
BasB 0:335646ab45c0 382 motor1Power.period(PWM_period);
BasB 0:335646ab45c0 383 motor2Power.period(PWM_period);
BasB 2:7ea5ae2287a7 384
BasB 3:6e28b992b99e 385 motor_curr_state = motor_wait; // Start off in EMG Wait state
BasB 3:6e28b992b99e 386 tickGlobal.attach( &tickGlobalFunc, Ts );
BasB 0:335646ab45c0 387
BasB 4:1e8da6b5f147 388 button1.fall(&button1Press);
BasB 6:e7e39d116ed0 389 button2.fall(&button2Press);
BasB 0:335646ab45c0 390
BasB 0:335646ab45c0 391 while (true) {
BasB 2:7ea5ae2287a7 392 pc.printf("Omega1: %f Omega 2: %f controlsignal1: %f \r\n", omega1, omega2, controlsignal1);
BasB 7:676a83def149 393 pc.printf("Currentstate: %i\r\n",motor_curr_state);
BasB 7:676a83def149 394 pc.printf("motor1offset: %f motor2offset: %f\r\n",motor1offset,motor2offset);
BasB 0:335646ab45c0 395 wait(0.5);
BasB 0:335646ab45c0 396 }
BasB 0:335646ab45c0 397 }