M9 Biorobotics Group 8 / Mbed 2 deprecated motorcalibration_StateMasjien

Moet dit er bij

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@13:41fc5b6262c0, 2019-10-29 (annotated)

Committer:
BasB
Date:
Tue Oct 29 21:33:42 2019 +0000
Revision:
13:41fc5b6262c0
Parent:
12:55b76c319364
Child:
14:8aa57a7c5b30
Dit is echt de shit! Hij kan met state machine calibreren en naar home position bewegen en dan aangestuurd worden met een gewenste 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 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 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 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 213 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 214 q1=q1+q1dot*Ts;
BasB 9:298469a70280 215 q2=q2+q2dot*Ts;
BasB 9:298469a70280 216
BasB 9:298469a70280 217 xe=l1*cos(q1)+l2*cos(q1+q2);
BasB 9:298469a70280 218 ye=l1*sin(q1)+l2*sin(q1+q2);
BasB 9:298469a70280 219
BasB 9:298469a70280 220
BasB 9:298469a70280 221
BasB 9:298469a70280 222 if (q1<0.0f){
BasB 9:298469a70280 223 q1=0.0;}
BasB 13:41fc5b6262c0 224 else if (q1>65.0f*deg2rad){
BasB 13:41fc5b6262c0 225 q1=65.0f*deg2rad;}
BasB 9:298469a70280 226 else{
BasB 9:298469a70280 227 q1=q1;}
BasB 9:298469a70280 228
BasB 13:41fc5b6262c0 229 if (q2>-50.0*deg2rad){
BasB 13:41fc5b6262c0 230 q2=-50.0*deg2rad;}
BasB 9:298469a70280 231 else if (q2<-135.0*deg2rad){
BasB 9:298469a70280 232 q2=-135.0*deg2rad;}
BasB 9:298469a70280 233 else{
BasB 9:298469a70280 234 q2=q2;}
BasB 9:298469a70280 235
BasB 9:298469a70280 236 motor1ref=5.5f*q1+5.5f*q2;
BasB 9:298469a70280 237 motor2ref=2.75f*q1;
BasB 9:298469a70280 238 }
BasB 9:298469a70280 239 }
BasB 9:298469a70280 240
BasB 9:298469a70280 241
BasB 9:298469a70280 242 void motorControl(){
BasB 10:990287a722d2 243 counts1= counts1af-counts1offset;
BasB 13:41fc5b6262c0 244 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 245 omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 9:298469a70280 246 motor1error=motor1ref-motor1angle;
BasB 9:298469a70280 247 if (controlsignal1<0){
BasB 9:298469a70280 248 motordir1= 0;}
BasB 9:298469a70280 249 else {
BasB 9:298469a70280 250 motordir1= 1;}
BasB 11:cd7be08f46b0 251
BasB 9:298469a70280 252 motor1Power.write(abs(controlsignal1));
BasB 9:298469a70280 253 motor1Direction= motordir1;
BasB 9:298469a70280 254
BasB 10:990287a722d2 255 counts2= counts2af - counts2offset;
BasB 11:cd7be08f46b0 256 motor2angle = (counts2 * factorin / gearratio)-(10.0*2.75*deg2rad); // Angle of motor shaft in rad + correctie voor q1
BasB 9:298469a70280 257 omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 9:298469a70280 258 motor2error=motor2ref-motor2angle;
BasB 9:298469a70280 259 if (controlsignal2<0){
BasB 9:298469a70280 260 motordir2= 0;}
BasB 9:298469a70280 261 else {
BasB 9:298469a70280 262 motordir2= 1;}
BasB 11:cd7be08f46b0 263 if (motor_calibration_done == true){
BasB 11:cd7be08f46b0 264 motor2Power.write(abs(controlsignal2));}
BasB 9:298469a70280 265 motor2Direction= motordir2;
BasB 9:298469a70280 266 }
BasB 9:298469a70280 267
BasB 8:3cfc8be293d3 268 void do_motor_wait(){
BasB 3:6e28b992b99e 269 // Entry function
BasB 3:6e28b992b99e 270 if ( motor_state_changed == true ) {
BasB 3:6e28b992b99e 271 motor_state_changed = false;
BasB 3:6e28b992b99e 272 // More functions
BasB 3:6e28b992b99e 273 }
BasB 3:6e28b992b99e 274
BasB 8:3cfc8be293d3 275 // Do nothing until end condition is met
BasB 5:4d8b85b7cfc4 276
BasB 8:3cfc8be293d3 277 // State transition guard
BasB 8:3cfc8be293d3 278 if ( button2_pressed ) {
BasB 8:3cfc8be293d3 279 button2_pressed = false;
BasB 8:3cfc8be293d3 280 motor_curr_state = motor_encoderset; //Beginnen met calibratie
BasB 3:6e28b992b99e 281 motor_state_changed = true;
BasB 3:6e28b992b99e 282 // More functions
BasB 3:6e28b992b99e 283 }
BasB 8:3cfc8be293d3 284
BasB 8:3cfc8be293d3 285 }
BasB 0:335646ab45c0 286
BasB 3:6e28b992b99e 287 void do_motor_Encoder_Set(){
BasB 3:6e28b992b99e 288 // Entry function
BasB 3:6e28b992b99e 289 if ( motor_state_changed == true ) {
BasB 3:6e28b992b99e 290 motor_state_changed = false;
BasB 3:6e28b992b99e 291 // More functions
BasB 3:6e28b992b99e 292 }
BasB 7:676a83def149 293 motor1Power.write(0.0f);
BasB 7:676a83def149 294 motor2Power.write(0.0f);
BasB 10:990287a722d2 295 counts1offset = counts1af ;
BasB 10:990287a722d2 296 counts2offset = counts2af;
BasB 7:676a83def149 297
BasB 3:6e28b992b99e 298 // State transition guard
BasB 7:676a83def149 299 if ( button2_pressed ) {
BasB 9:298469a70280 300 button2_pressed = false;
BasB 9:298469a70280 301 motor_calibration_done = true;
BasB 9:298469a70280 302 motor_curr_state = motor_wait2;
BasB 3:6e28b992b99e 303 motor_state_changed = true;
BasB 3:6e28b992b99e 304 }
BasB 3:6e28b992b99e 305 }
BasB 3:6e28b992b99e 306
BasB 9:298469a70280 307 void do_motor_wait2(){
BasB 9:298469a70280 308 // Entry function
BasB 9:298469a70280 309 if ( motor_state_changed == true ) {
BasB 9:298469a70280 310 motor_state_changed = false;
BasB 9:298469a70280 311 // More functions
BasB 9:298469a70280 312 }
BasB 12:55b76c319364 313 motor_RKI=true;
BasB 9:298469a70280 314 // Do nothing until end condition is met
BasB 9:298469a70280 315
BasB 9:298469a70280 316 // State transition guard
BasB 9:298469a70280 317 if ( button2_pressed ) {
BasB 9:298469a70280 318 button2_pressed = false;
BasB 10:990287a722d2 319 motor_curr_state = motor_bewegen;
BasB 9:298469a70280 320 motor_state_changed = true;
BasB 9:298469a70280 321 // More functions
BasB 9:298469a70280 322 }
BasB 9:298469a70280 323
BasB 9:298469a70280 324 }
BasB 9:298469a70280 325
BasB 9:298469a70280 326 void do_motor_bewegen(){
BasB 9:298469a70280 327 // Entry function
BasB 9:298469a70280 328 if ( motor_state_changed == true ) {
BasB 9:298469a70280 329 motor_state_changed = false;
BasB 9:298469a70280 330 // More functions
BasB 9:298469a70280 331 }
BasB 12:55b76c319364 332 static float t=0;
BasB 12:55b76c319364 333 Vx=6.0f*sin(1.0f*t);
BasB 12:55b76c319364 334 Vy=0.0f;
BasB 12:55b76c319364 335 t+=Ts;
BasB 12:55b76c319364 336
BasB 9:298469a70280 337 if ( button2_pressed ) {
BasB 9:298469a70280 338 button2_pressed = false;
BasB 9:298469a70280 339 motor_RKI = false;
BasB 9:298469a70280 340 motor_curr_state = motor_wait2;
BasB 9:298469a70280 341 motor_state_changed = true;
BasB 9:298469a70280 342 // More functions
BasB 9:298469a70280 343 }
BasB 9:298469a70280 344
BasB 9:298469a70280 345
BasB 9:298469a70280 346 }
BasB 9:298469a70280 347
BasB 3:6e28b992b99e 348 void motor_state_machine()
BasB 3:6e28b992b99e 349 {
BasB 3:6e28b992b99e 350 switch(motor_curr_state) {
BasB 3:6e28b992b99e 351 case motor_wait:
BasB 3:6e28b992b99e 352 do_motor_wait();
BasB 3:6e28b992b99e 353 break;
BasB 3:6e28b992b99e 354 case motor_encoderset:
BasB 3:6e28b992b99e 355 do_motor_Encoder_Set();
BasB 3:6e28b992b99e 356 break;
BasB 9:298469a70280 357 case motor_wait2:
BasB 9:298469a70280 358 do_motor_wait2();
BasB 9:298469a70280 359 break;
BasB 9:298469a70280 360 case motor_bewegen:
BasB 9:298469a70280 361 do_motor_bewegen();
BasB 9:298469a70280 362 break;
BasB 3:6e28b992b99e 363 }
BasB 3:6e28b992b99e 364 }
BasB 3:6e28b992b99e 365
BasB 3:6e28b992b99e 366 // Global loop of program
BasB 3:6e28b992b99e 367 void tickGlobalFunc()
BasB 3:6e28b992b99e 368 {
BasB 3:6e28b992b99e 369 //sampleSignal();
BasB 3:6e28b992b99e 370 //emg_state_machine();
BasB 3:6e28b992b99e 371 motor_state_machine();
BasB 4:1e8da6b5f147 372 readEncoder();
BasB 9:298469a70280 373 PID_controller();
BasB 9:298469a70280 374 motorControl();
BasB 9:298469a70280 375 RKI();
BasB 9:298469a70280 376
BasB 3:6e28b992b99e 377 // controller();
BasB 3:6e28b992b99e 378 // outputToMotors();
BasB 3:6e28b992b99e 379 }
BasB 3:6e28b992b99e 380
BasB 3:6e28b992b99e 381
BasB 0:335646ab45c0 382 int main()
BasB 0:335646ab45c0 383 {
BasB 0:335646ab45c0 384 pc.baud(115200);
BasB 0:335646ab45c0 385 pc.printf("\r\nStarting...\r\n\r\n");
BasB 0:335646ab45c0 386 motor1Power.period(PWM_period);
BasB 0:335646ab45c0 387 motor2Power.period(PWM_period);
BasB 2:7ea5ae2287a7 388
BasB 3:6e28b992b99e 389 motor_curr_state = motor_wait; // Start off in EMG Wait state
BasB 3:6e28b992b99e 390 tickGlobal.attach( &tickGlobalFunc, Ts );
BasB 0:335646ab45c0 391
BasB 4:1e8da6b5f147 392 button1.fall(&button1Press);
BasB 6:e7e39d116ed0 393 button2.fall(&button2Press);
BasB 0:335646ab45c0 394
BasB 0:335646ab45c0 395 while (true) {
BasB 10:990287a722d2 396 pc.printf("counts1offset %i counts1af: %i counts1: %i\r\n",counts1offset,counts1af,counts1);
BasB 10:990287a722d2 397 pc.printf("counts2offset %i counts2af: %i counts2: %i\r\n",counts2offset,counts2af,counts2);
BasB 10:990287a722d2 398 pc.printf("state: %i motor1ref: %f motor1angle: %f\r\n",motor_curr_state,motor1ref,motor1angle);
BasB 11:cd7be08f46b0 399 pc.printf("q1: %f q2: %f motor1error: %f \r\n",q1*rad2deg,q2*rad2deg, motor1error);
BasB 11:cd7be08f46b0 400
BasB 0:335646ab45c0 401 wait(0.5);
BasB 0:335646ab45c0 402 }
BasB 0:335646ab45c0 403 }