M9 Biorobotics Group 8 / Mbed 2 deprecated motorcalibration_StateMasjien

Moet dit er bij

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@10:990287a722d2, 2019-10-29 (annotated)

Committer:
BasB
Date:
Tue Oct 29 20:05:39 2019 +0000
Revision:
10:990287a722d2
Parent:
9:298469a70280
Child:
11:cd7be08f46b0
Offset functie werkt. Let op dat q1 en q2 bij je globale variabelen al de hoek van je calibratie positie hebben en dat daarvoor gecorrigeerd wordt.

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 10:990287a722d2 26 float motor1angle=-135.0*0.0174532*5.5; //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 10:990287a722d2 46 float motor1ref=-135.0*0.0174532*5.5;
BasB 10:990287a722d2 47 float motor2ref;
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 10:990287a722d2 111 volatile int counts1af;
BasB 10:990287a722d2 112 volatile int counts2af;
BasB 10:990287a722d2 113 int counts1offset;
BasB 10:990287a722d2 114 int counts2offset;
BasB 0:335646ab45c0 115 volatile int countsPrev1 = 0;
BasB 0:335646ab45c0 116 volatile int countsPrev2 = 0;
BasB 0:335646ab45c0 117 volatile int deltaCounts1;
BasB 0:335646ab45c0 118 volatile int deltaCounts2;
BasB 0:335646ab45c0 119
BasB 0:335646ab45c0 120 float factorin = 6.23185/64; // Convert encoder counts to angle in rad
BasB 0:335646ab45c0 121 float gearratio = 131.25; // Gear ratio of gearbox
BasB 0:335646ab45c0 122
BasB 3:6e28b992b99e 123 void button1Press()
BasB 3:6e28b992b99e 124 {
BasB 3:6e28b992b99e 125 button1_pressed = true;
BasB 3:6e28b992b99e 126 }
BasB 0:335646ab45c0 127
BasB 6:e7e39d116ed0 128 void button2Press()
BasB 6:e7e39d116ed0 129 {
BasB 6:e7e39d116ed0 130 button2_pressed = true;
BasB 6:e7e39d116ed0 131 }
BasB 0:335646ab45c0 132
BasB 3:6e28b992b99e 133 // Ticker Functions
BasB 0:335646ab45c0 134 void readEncoder()
BasB 0:335646ab45c0 135 {
BasB 10:990287a722d2 136 counts1af = encoder1.getPulses();
BasB 10:990287a722d2 137 deltaCounts1 = counts1af - countsPrev1;
BasB 10:990287a722d2 138 countsPrev1 = counts1af;
BasB 0:335646ab45c0 139
BasB 10:990287a722d2 140 counts2af = encoder2.getPulses();
BasB 10:990287a722d2 141 deltaCounts2 = counts2af - countsPrev2;
BasB 10:990287a722d2 142 countsPrev2 = counts2af;
BasB 0:335646ab45c0 143 }
BasB 0:335646ab45c0 144
BasB 9:298469a70280 145 void PID_controller(){
BasB 9:298469a70280 146
BasB 9:298469a70280 147 static float error_integral1=0;
BasB 9:298469a70280 148 static float e_prev1=motor1error;
BasB 9:298469a70280 149
BasB 9:298469a70280 150 //Proportional part:
BasB 9:298469a70280 151 u_p1=Kp*motor1error;
BasB 9:298469a70280 152
BasB 9:298469a70280 153 //Integral part
BasB 9:298469a70280 154 error_integral1=error_integral1+ei1*Ts;
BasB 9:298469a70280 155 u_i1=Ki*error_integral1;
BasB 9:298469a70280 156
BasB 9:298469a70280 157 //Derivative part
BasB 9:298469a70280 158 float error_derivative1=(motor1error-e_prev1)/Ts;
BasB 9:298469a70280 159 float u_d1=Kd*error_derivative1;
BasB 9:298469a70280 160 e_prev1=motor1error;
BasB 9:298469a70280 161
BasB 9:298469a70280 162 // Sum and limit
BasB 9:298469a70280 163 up1= u_p1+u_i1+u_d1;
BasB 9:298469a70280 164 if (up1>1){
BasB 9:298469a70280 165 controlsignal1=1;}
BasB 9:298469a70280 166 else if (up1<-1){
BasB 9:298469a70280 167 controlsignal1=-1;}
BasB 9:298469a70280 168 else {
BasB 9:298469a70280 169 controlsignal1=up1;}
BasB 9:298469a70280 170
BasB 9:298469a70280 171 // To prevent windup
BasB 9:298469a70280 172 ux1= up1-controlsignal1;
BasB 9:298469a70280 173 ek1= Ka*ux1;
BasB 9:298469a70280 174 ei1= motor1error-ek1;
BasB 9:298469a70280 175
BasB 9:298469a70280 176 // Motor 2
BasB 9:298469a70280 177
BasB 9:298469a70280 178 static float error_integral2=0;
BasB 9:298469a70280 179 static float e_prev2=motor2error;
BasB 9:298469a70280 180
BasB 9:298469a70280 181 //Proportional part:
BasB 9:298469a70280 182 u_p2=Kp*motor2error;
BasB 9:298469a70280 183
BasB 9:298469a70280 184 //Integral part
BasB 9:298469a70280 185 error_integral2=error_integral2+ei2*Ts;
BasB 9:298469a70280 186 u_i2=Ki*error_integral2;
BasB 9:298469a70280 187
BasB 9:298469a70280 188 //Derivative part
BasB 9:298469a70280 189 float error_derivative2=(motor2error-e_prev2)/Ts;
BasB 9:298469a70280 190 float u_d2=Kd*error_derivative2;
BasB 9:298469a70280 191 e_prev2=motor2error;
BasB 9:298469a70280 192
BasB 9:298469a70280 193 // Sum and limit
BasB 9:298469a70280 194 up2= u_p2+u_i2+u_d2;
BasB 9:298469a70280 195 if (up2>1.0f){
BasB 9:298469a70280 196 controlsignal2=1.0f;}
BasB 9:298469a70280 197 else if (up2<-1){
BasB 9:298469a70280 198 controlsignal2=-1.0f;}
BasB 9:298469a70280 199 else {
BasB 9:298469a70280 200 controlsignal2=up2;}
BasB 9:298469a70280 201
BasB 9:298469a70280 202 // To prevent windup
BasB 9:298469a70280 203 ux2= up2-controlsignal2;
BasB 9:298469a70280 204 ek2= Ka*ux2;
BasB 9:298469a70280 205 ei2= motor2error-ek2;
BasB 9:298469a70280 206 }
BasB 9:298469a70280 207
BasB 9:298469a70280 208 void RKI(){
BasB 9:298469a70280 209
BasB 9:298469a70280 210 if (motor_RKI==true){
BasB 9:298469a70280 211 //Vy=potmeter1.read()*10.0*motortoggle;
BasB 9:298469a70280 212 //Vy=potmeter2.read()*10*motortoggle;
BasB 9:298469a70280 213 static float t=0;
BasB 9:298469a70280 214 Vx=6.0f*sin(1.0f*t);
BasB 9:298469a70280 215 t+=Ts;
BasB 9:298469a70280 216 //Vx=-1.0*;
BasB 9:298469a70280 217 Vy=0.0f;
BasB 9:298469a70280 218 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 219 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 220 q1=q1+q1dot*Ts;
BasB 9:298469a70280 221 q2=q2+q2dot*Ts;
BasB 9:298469a70280 222
BasB 9:298469a70280 223 xe=l1*cos(q1)+l2*cos(q1+q2);
BasB 9:298469a70280 224 ye=l1*sin(q1)+l2*sin(q1+q2);
BasB 9:298469a70280 225
BasB 9:298469a70280 226
BasB 9:298469a70280 227
BasB 9:298469a70280 228 if (q1<0.0f){
BasB 9:298469a70280 229 q1=0.0;}
BasB 9:298469a70280 230 else if (q1>90.0f*deg2rad){
BasB 9:298469a70280 231 q1=90.0f*deg2rad;}
BasB 9:298469a70280 232 else{
BasB 9:298469a70280 233 q1=q1;}
BasB 9:298469a70280 234
BasB 9:298469a70280 235 if (q2>-45.0*deg2rad){
BasB 9:298469a70280 236 q2=-45.0*deg2rad;}
BasB 9:298469a70280 237 else if (q2<-135.0*deg2rad){
BasB 9:298469a70280 238 q2=-135.0*deg2rad;}
BasB 9:298469a70280 239 else{
BasB 9:298469a70280 240 q2=q2;}
BasB 9:298469a70280 241
BasB 9:298469a70280 242 motor1ref=5.5f*q1+5.5f*q2;
BasB 9:298469a70280 243 motor2ref=2.75f*q1;
BasB 9:298469a70280 244 }
BasB 9:298469a70280 245 }
BasB 9:298469a70280 246
BasB 9:298469a70280 247
BasB 9:298469a70280 248 void motorControl(){
BasB 10:990287a722d2 249 counts1= counts1af-counts1offset;
BasB 9:298469a70280 250 motor1angle = (counts1 * factorin / gearratio)-(135.0*5.5*deg2rad); // Angle of motor shaft in rad
BasB 9:298469a70280 251 omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 9:298469a70280 252 motor1error=motor1ref-motor1angle;
BasB 9:298469a70280 253 if (controlsignal1<0){
BasB 9:298469a70280 254 motordir1= 0;}
BasB 9:298469a70280 255 else {
BasB 9:298469a70280 256 motordir1= 1;}
BasB 9:298469a70280 257 motor1Power.write(abs(controlsignal1));
BasB 9:298469a70280 258 motor1Direction= motordir1;
BasB 9:298469a70280 259
BasB 10:990287a722d2 260 counts2= counts2af - counts2offset;
BasB 9:298469a70280 261 motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad
BasB 9:298469a70280 262 omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 9:298469a70280 263 motor2error=motor2ref-motor2angle;
BasB 9:298469a70280 264 if (controlsignal2<0){
BasB 9:298469a70280 265 motordir2= 0;}
BasB 9:298469a70280 266 else {
BasB 9:298469a70280 267 motordir2= 1;}
BasB 9:298469a70280 268 motor2Power.write(abs(controlsignal2));
BasB 9:298469a70280 269 motor2Direction= motordir2;
BasB 9:298469a70280 270 }
BasB 9:298469a70280 271
BasB 8:3cfc8be293d3 272 void do_motor_wait(){
BasB 3:6e28b992b99e 273 // Entry function
BasB 3:6e28b992b99e 274 if ( motor_state_changed == true ) {
BasB 3:6e28b992b99e 275 motor_state_changed = false;
BasB 3:6e28b992b99e 276 // More functions
BasB 3:6e28b992b99e 277 }
BasB 3:6e28b992b99e 278
BasB 8:3cfc8be293d3 279 // Do nothing until end condition is met
BasB 5:4d8b85b7cfc4 280
BasB 8:3cfc8be293d3 281 // State transition guard
BasB 8:3cfc8be293d3 282 if ( button2_pressed ) {
BasB 8:3cfc8be293d3 283 button2_pressed = false;
BasB 8:3cfc8be293d3 284 motor_curr_state = motor_encoderset; //Beginnen met calibratie
BasB 3:6e28b992b99e 285 motor_state_changed = true;
BasB 3:6e28b992b99e 286 // More functions
BasB 3:6e28b992b99e 287 }
BasB 8:3cfc8be293d3 288
BasB 8:3cfc8be293d3 289 }
BasB 0:335646ab45c0 290
BasB 3:6e28b992b99e 291 void do_motor_Encoder_Set(){
BasB 3:6e28b992b99e 292 // Entry function
BasB 3:6e28b992b99e 293 if ( motor_state_changed == true ) {
BasB 3:6e28b992b99e 294 motor_state_changed = false;
BasB 3:6e28b992b99e 295 // More functions
BasB 3:6e28b992b99e 296 }
BasB 7:676a83def149 297 motor1Power.write(0.0f);
BasB 7:676a83def149 298 motor2Power.write(0.0f);
BasB 10:990287a722d2 299 counts1offset = counts1af ;
BasB 10:990287a722d2 300 counts2offset = counts2af;
BasB 7:676a83def149 301
BasB 3:6e28b992b99e 302 // State transition guard
BasB 7:676a83def149 303 if ( button2_pressed ) {
BasB 9:298469a70280 304 button2_pressed = false;
BasB 9:298469a70280 305 motor_calibration_done = true;
BasB 9:298469a70280 306 motor_curr_state = motor_wait2;
BasB 3:6e28b992b99e 307 motor_state_changed = true;
BasB 3:6e28b992b99e 308 }
BasB 3:6e28b992b99e 309 }
BasB 3:6e28b992b99e 310
BasB 9:298469a70280 311 void do_motor_wait2(){
BasB 9:298469a70280 312 // Entry function
BasB 9:298469a70280 313 if ( motor_state_changed == true ) {
BasB 9:298469a70280 314 motor_state_changed = false;
BasB 9:298469a70280 315 // More functions
BasB 9:298469a70280 316 }
BasB 9:298469a70280 317
BasB 9:298469a70280 318 // Do nothing until end condition is met
BasB 9:298469a70280 319
BasB 9:298469a70280 320 // State transition guard
BasB 9:298469a70280 321 if ( button2_pressed ) {
BasB 9:298469a70280 322 button2_pressed = false;
BasB 10:990287a722d2 323 motor_curr_state = motor_bewegen;
BasB 9:298469a70280 324 motor_state_changed = true;
BasB 9:298469a70280 325 // More functions
BasB 9:298469a70280 326 }
BasB 9:298469a70280 327
BasB 9:298469a70280 328 }
BasB 9:298469a70280 329
BasB 9:298469a70280 330 void do_motor_bewegen(){
BasB 9:298469a70280 331 // Entry function
BasB 9:298469a70280 332 if ( motor_state_changed == true ) {
BasB 9:298469a70280 333 motor_state_changed = false;
BasB 9:298469a70280 334 // More functions
BasB 9:298469a70280 335 }
BasB 9:298469a70280 336
BasB 9:298469a70280 337 motor_RKI=true;
BasB 9:298469a70280 338
BasB 9:298469a70280 339 if ( button2_pressed ) {
BasB 9:298469a70280 340 button2_pressed = false;
BasB 9:298469a70280 341 motor_RKI = false;
BasB 9:298469a70280 342 motor_curr_state = motor_wait2;
BasB 9:298469a70280 343 motor_state_changed = true;
BasB 9:298469a70280 344 // More functions
BasB 9:298469a70280 345 }
BasB 9:298469a70280 346
BasB 9:298469a70280 347
BasB 9:298469a70280 348 }
BasB 9:298469a70280 349
BasB 3:6e28b992b99e 350 void motor_state_machine()
BasB 3:6e28b992b99e 351 {
BasB 3:6e28b992b99e 352 switch(motor_curr_state) {
BasB 3:6e28b992b99e 353 case motor_wait:
BasB 3:6e28b992b99e 354 do_motor_wait();
BasB 3:6e28b992b99e 355 break;
BasB 3:6e28b992b99e 356 case motor_encoderset:
BasB 3:6e28b992b99e 357 do_motor_Encoder_Set();
BasB 3:6e28b992b99e 358 break;
BasB 9:298469a70280 359 case motor_wait2:
BasB 9:298469a70280 360 do_motor_wait2();
BasB 9:298469a70280 361 break;
BasB 9:298469a70280 362 case motor_bewegen:
BasB 9:298469a70280 363 do_motor_bewegen();
BasB 9:298469a70280 364 break;
BasB 3:6e28b992b99e 365 }
BasB 3:6e28b992b99e 366 }
BasB 3:6e28b992b99e 367
BasB 3:6e28b992b99e 368 // Global loop of program
BasB 3:6e28b992b99e 369 void tickGlobalFunc()
BasB 3:6e28b992b99e 370 {
BasB 3:6e28b992b99e 371 //sampleSignal();
BasB 3:6e28b992b99e 372 //emg_state_machine();
BasB 3:6e28b992b99e 373 motor_state_machine();
BasB 4:1e8da6b5f147 374 readEncoder();
BasB 9:298469a70280 375 PID_controller();
BasB 9:298469a70280 376 motorControl();
BasB 9:298469a70280 377 RKI();
BasB 9:298469a70280 378
BasB 3:6e28b992b99e 379 // controller();
BasB 3:6e28b992b99e 380 // outputToMotors();
BasB 3:6e28b992b99e 381 }
BasB 3:6e28b992b99e 382
BasB 3:6e28b992b99e 383
BasB 0:335646ab45c0 384 int main()
BasB 0:335646ab45c0 385 {
BasB 0:335646ab45c0 386 pc.baud(115200);
BasB 0:335646ab45c0 387 pc.printf("\r\nStarting...\r\n\r\n");
BasB 0:335646ab45c0 388 motor1Power.period(PWM_period);
BasB 0:335646ab45c0 389 motor2Power.period(PWM_period);
BasB 2:7ea5ae2287a7 390
BasB 3:6e28b992b99e 391 motor_curr_state = motor_wait; // Start off in EMG Wait state
BasB 3:6e28b992b99e 392 tickGlobal.attach( &tickGlobalFunc, Ts );
BasB 0:335646ab45c0 393
BasB 4:1e8da6b5f147 394 button1.fall(&button1Press);
BasB 6:e7e39d116ed0 395 button2.fall(&button2Press);
BasB 0:335646ab45c0 396
BasB 0:335646ab45c0 397 while (true) {
BasB 10:990287a722d2 398 pc.printf("counts1offset %i counts1af: %i counts1: %i\r\n",counts1offset,counts1af,counts1);
BasB 10:990287a722d2 399 pc.printf("counts2offset %i counts2af: %i counts2: %i\r\n",counts2offset,counts2af,counts2);
BasB 10:990287a722d2 400 pc.printf("state: %i motor1ref: %f motor1angle: %f\r\n",motor_curr_state,motor1ref,motor1angle);
BasB 0:335646ab45c0 401 wait(0.5);
BasB 0:335646ab45c0 402 }
BasB 0:335646ab45c0 403 }