M9 Biorobotics Group 8 / Mbed 2 deprecated motorcalibration_StateMasjien

Moet dit er bij

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@11:cd7be08f46b0, 2019-10-29 (annotated)

Committer:
BasB
Date:
Tue Oct 29 20:57:56 2019 +0000
Revision:
11:cd7be08f46b0
Parent:
10:990287a722d2
Child:
12:55b76c319364
commit voordat Freek ging prutsen aan rki met vx en vy

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 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 11:cd7be08f46b0 28 float motor1angle=(-135.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 11:cd7be08f46b0 47 float motor1ref=(-135.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 0:335646ab45c0 77 float q2=-135.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 9:298469a70280 89 enum Motor_States{motor_wait , motor_encoderset , motor_wait2 , motor_bewegen};
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 0:335646ab45c0 108 const float PWM_period = 1e-6;
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
BasB 9:298469a70280 211 if (motor_RKI==true){
BasB 9:298469a70280 212 //Vy=potmeter1.read()*10.0*motortoggle;
BasB 9:298469a70280 213 //Vy=potmeter2.read()*10*motortoggle;
BasB 9:298469a70280 214 static float t=0;
BasB 9:298469a70280 215 Vx=6.0f*sin(1.0f*t);
BasB 11:cd7be08f46b0 216 Vy=0.0f;
BasB 9:298469a70280 217 t+=Ts;
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 11:cd7be08f46b0 250 motor1angle = (counts1 * factorin / gearratio)-((135.0*5.5*deg2rad)+(10.0*5.5*deg2rad)); // Angle of motor shaft in rad + correctie voor q1 en q2
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 11:cd7be08f46b0 257
BasB 9:298469a70280 258 motor1Power.write(abs(controlsignal1));
BasB 9:298469a70280 259 motor1Direction= motordir1;
BasB 9:298469a70280 260
BasB 10:990287a722d2 261 counts2= counts2af - counts2offset;
BasB 11:cd7be08f46b0 262 motor2angle = (counts2 * factorin / gearratio)-(10.0*2.75*deg2rad); // Angle of motor shaft in rad + correctie voor q1
BasB 9:298469a70280 263 omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 9:298469a70280 264 motor2error=motor2ref-motor2angle;
BasB 9:298469a70280 265 if (controlsignal2<0){
BasB 9:298469a70280 266 motordir2= 0;}
BasB 9:298469a70280 267 else {
BasB 9:298469a70280 268 motordir2= 1;}
BasB 11:cd7be08f46b0 269 if (motor_calibration_done == true){
BasB 11:cd7be08f46b0 270 motor2Power.write(abs(controlsignal2));}
BasB 9:298469a70280 271 motor2Direction= motordir2;
BasB 9:298469a70280 272 }
BasB 9:298469a70280 273
BasB 8:3cfc8be293d3 274 void do_motor_wait(){
BasB 3:6e28b992b99e 275 // Entry function
BasB 3:6e28b992b99e 276 if ( motor_state_changed == true ) {
BasB 3:6e28b992b99e 277 motor_state_changed = false;
BasB 3:6e28b992b99e 278 // More functions
BasB 3:6e28b992b99e 279 }
BasB 3:6e28b992b99e 280
BasB 8:3cfc8be293d3 281 // Do nothing until end condition is met
BasB 5:4d8b85b7cfc4 282
BasB 8:3cfc8be293d3 283 // State transition guard
BasB 8:3cfc8be293d3 284 if ( button2_pressed ) {
BasB 8:3cfc8be293d3 285 button2_pressed = false;
BasB 8:3cfc8be293d3 286 motor_curr_state = motor_encoderset; //Beginnen met calibratie
BasB 3:6e28b992b99e 287 motor_state_changed = true;
BasB 3:6e28b992b99e 288 // More functions
BasB 3:6e28b992b99e 289 }
BasB 8:3cfc8be293d3 290
BasB 8:3cfc8be293d3 291 }
BasB 0:335646ab45c0 292
BasB 3:6e28b992b99e 293 void do_motor_Encoder_Set(){
BasB 3:6e28b992b99e 294 // Entry function
BasB 3:6e28b992b99e 295 if ( motor_state_changed == true ) {
BasB 3:6e28b992b99e 296 motor_state_changed = false;
BasB 3:6e28b992b99e 297 // More functions
BasB 3:6e28b992b99e 298 }
BasB 7:676a83def149 299 motor1Power.write(0.0f);
BasB 7:676a83def149 300 motor2Power.write(0.0f);
BasB 10:990287a722d2 301 counts1offset = counts1af ;
BasB 10:990287a722d2 302 counts2offset = counts2af;
BasB 7:676a83def149 303
BasB 3:6e28b992b99e 304 // State transition guard
BasB 7:676a83def149 305 if ( button2_pressed ) {
BasB 9:298469a70280 306 button2_pressed = false;
BasB 9:298469a70280 307 motor_calibration_done = true;
BasB 9:298469a70280 308 motor_curr_state = motor_wait2;
BasB 3:6e28b992b99e 309 motor_state_changed = true;
BasB 3:6e28b992b99e 310 }
BasB 3:6e28b992b99e 311 }
BasB 3:6e28b992b99e 312
BasB 9:298469a70280 313 void do_motor_wait2(){
BasB 9:298469a70280 314 // Entry function
BasB 9:298469a70280 315 if ( motor_state_changed == true ) {
BasB 9:298469a70280 316 motor_state_changed = false;
BasB 9:298469a70280 317 // More functions
BasB 9:298469a70280 318 }
BasB 9:298469a70280 319
BasB 9:298469a70280 320 // Do nothing until end condition is met
BasB 9:298469a70280 321
BasB 9:298469a70280 322 // State transition guard
BasB 9:298469a70280 323 if ( button2_pressed ) {
BasB 9:298469a70280 324 button2_pressed = false;
BasB 10:990287a722d2 325 motor_curr_state = motor_bewegen;
BasB 9:298469a70280 326 motor_state_changed = true;
BasB 9:298469a70280 327 // More functions
BasB 9:298469a70280 328 }
BasB 9:298469a70280 329
BasB 9:298469a70280 330 }
BasB 9:298469a70280 331
BasB 9:298469a70280 332 void do_motor_bewegen(){
BasB 9:298469a70280 333 // Entry function
BasB 9:298469a70280 334 if ( motor_state_changed == true ) {
BasB 9:298469a70280 335 motor_state_changed = false;
BasB 9:298469a70280 336 // More functions
BasB 9:298469a70280 337 }
BasB 9:298469a70280 338
BasB 9:298469a70280 339 motor_RKI=true;
BasB 9:298469a70280 340
BasB 9:298469a70280 341 if ( button2_pressed ) {
BasB 9:298469a70280 342 button2_pressed = false;
BasB 9:298469a70280 343 motor_RKI = false;
BasB 9:298469a70280 344 motor_curr_state = motor_wait2;
BasB 9:298469a70280 345 motor_state_changed = true;
BasB 9:298469a70280 346 // More functions
BasB 9:298469a70280 347 }
BasB 9:298469a70280 348
BasB 9:298469a70280 349
BasB 9:298469a70280 350 }
BasB 9:298469a70280 351
BasB 3:6e28b992b99e 352 void motor_state_machine()
BasB 3:6e28b992b99e 353 {
BasB 3:6e28b992b99e 354 switch(motor_curr_state) {
BasB 3:6e28b992b99e 355 case motor_wait:
BasB 3:6e28b992b99e 356 do_motor_wait();
BasB 3:6e28b992b99e 357 break;
BasB 3:6e28b992b99e 358 case motor_encoderset:
BasB 3:6e28b992b99e 359 do_motor_Encoder_Set();
BasB 3:6e28b992b99e 360 break;
BasB 9:298469a70280 361 case motor_wait2:
BasB 9:298469a70280 362 do_motor_wait2();
BasB 9:298469a70280 363 break;
BasB 9:298469a70280 364 case motor_bewegen:
BasB 9:298469a70280 365 do_motor_bewegen();
BasB 9:298469a70280 366 break;
BasB 3:6e28b992b99e 367 }
BasB 3:6e28b992b99e 368 }
BasB 3:6e28b992b99e 369
BasB 3:6e28b992b99e 370 // Global loop of program
BasB 3:6e28b992b99e 371 void tickGlobalFunc()
BasB 3:6e28b992b99e 372 {
BasB 3:6e28b992b99e 373 //sampleSignal();
BasB 3:6e28b992b99e 374 //emg_state_machine();
BasB 3:6e28b992b99e 375 motor_state_machine();
BasB 4:1e8da6b5f147 376 readEncoder();
BasB 9:298469a70280 377 PID_controller();
BasB 9:298469a70280 378 motorControl();
BasB 9:298469a70280 379 RKI();
BasB 9:298469a70280 380
BasB 3:6e28b992b99e 381 // controller();
BasB 3:6e28b992b99e 382 // outputToMotors();
BasB 3:6e28b992b99e 383 }
BasB 3:6e28b992b99e 384
BasB 3:6e28b992b99e 385
BasB 0:335646ab45c0 386 int main()
BasB 0:335646ab45c0 387 {
BasB 0:335646ab45c0 388 pc.baud(115200);
BasB 0:335646ab45c0 389 pc.printf("\r\nStarting...\r\n\r\n");
BasB 0:335646ab45c0 390 motor1Power.period(PWM_period);
BasB 0:335646ab45c0 391 motor2Power.period(PWM_period);
BasB 2:7ea5ae2287a7 392
BasB 3:6e28b992b99e 393 motor_curr_state = motor_wait; // Start off in EMG Wait state
BasB 3:6e28b992b99e 394 tickGlobal.attach( &tickGlobalFunc, Ts );
BasB 0:335646ab45c0 395
BasB 4:1e8da6b5f147 396 button1.fall(&button1Press);
BasB 6:e7e39d116ed0 397 button2.fall(&button2Press);
BasB 0:335646ab45c0 398
BasB 0:335646ab45c0 399 while (true) {
BasB 10:990287a722d2 400 pc.printf("counts1offset %i counts1af: %i counts1: %i\r\n",counts1offset,counts1af,counts1);
BasB 10:990287a722d2 401 pc.printf("counts2offset %i counts2af: %i counts2: %i\r\n",counts2offset,counts2af,counts2);
BasB 10:990287a722d2 402 pc.printf("state: %i motor1ref: %f motor1angle: %f\r\n",motor_curr_state,motor1ref,motor1angle);
BasB 11:cd7be08f46b0 403 pc.printf("q1: %f q2: %f motor1error: %f \r\n",q1*rad2deg,q2*rad2deg, motor1error);
BasB 11:cd7be08f46b0 404
BasB 0:335646ab45c0 405 wait(0.5);
BasB 0:335646ab45c0 406 }
BasB 0:335646ab45c0 407 }