M9 Biorobotics Group 8 / Mbed 2 deprecated motorcalibration_StateMasjien

Moet dit er bij

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@15:75070aedf4b7, 2019-10-30 (annotated)

Committer:
BasB
Date:
Wed Oct 30 13:32:33 2019 +0000
Revision:
15:75070aedf4b7
Parent:
14:8aa57a7c5b30
Child:
16:6334ad516980
Aangepaste, werkende state machine met failure mode (SW3)

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