Henk van Gansen / Mbed 2 deprecated Project_BioRobotics

Vanmorgen aan gewerkt

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Project_BioRobotics by Jordi Luong

main.cpp@10:a9e344e440b8, 2017-10-27 (annotated)

Committer:
jordiluong
Date:
Fri Oct 27 08:43:34 2017 +0000
Revision:
10:a9e344e440b8
Parent:
8:78d8ccf84a38
Child:
11:5c06c97c3673
Added inverse kinematics (not working completey)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jordiluong 0:80ac024b84cb 1 #include "BiQuad.h"
jordiluong 0:80ac024b84cb 2 #include "FastPWM.h"
jordiluong 0:80ac024b84cb 3 #include "HIDScope.h"
jordiluong 5:0d3e8694726e 4 #include <math.h>
jordiluong 0:80ac024b84cb 5 #include "mbed.h"
jordiluong 5:0d3e8694726e 6 #include "MODSERIAL.h"
jordiluong 0:80ac024b84cb 7 #include "QEI.h"
jordiluong 0:80ac024b84cb 8
jordiluong 5:0d3e8694726e 9 const double pi = 3.1415926535897; // Definition of pi
jordiluong 5:0d3e8694726e 10
jordiluong 7:757e95b4dc46 11 // SERIAL COMMUNICATION WITH PC ////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 12 MODSERIAL pc(USBTX, USBRX);
jordiluong 0:80ac024b84cb 13
jordiluong 7:757e95b4dc46 14 // STATES //////////////////////////////////////////////////////////////////////
jordiluong 3:5c3edcd29448 15 enum states{MOTORS_OFF, MOVING, HITTING};
jordiluong 0:80ac024b84cb 16 states currentState = MOTORS_OFF; // Start with motors off
jordiluong 0:80ac024b84cb 17 bool stateChanged = true; // Make sure the initialization of first state is executed
jordiluong 0:80ac024b84cb 18
jordiluong 7:757e95b4dc46 19 // ENCODER /////////////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 20 QEI Encoder1(D10,D11,NC,32); // CONNECT ENC1A TO D10, ENC1B TO D11
jordiluong 5:0d3e8694726e 21 QEI Encoder2(D12,D13,NC,32); // CONNECT ENC2A TO D12, ENC2B TO D13
jordiluong 4:ea7689bf97e1 22
jordiluong 7:757e95b4dc46 23 // PINS ////////////////////////////////////////////////////////////////////////
jordiluong 4:ea7689bf97e1 24 DigitalOut motor1DirectionPin(D4); // Value 0: CCW; 1: CW
jordiluong 3:5c3edcd29448 25 PwmOut motor1MagnitudePin(D5);
jordiluong 3:5c3edcd29448 26 DigitalOut motor2DirectionPin(D7); // Value 0: CW or CCW?; 1: CW or CCW?
jordiluong 3:5c3edcd29448 27 PwmOut motor2MagnitudePin(D6);
jordiluong 4:ea7689bf97e1 28 InterruptIn button1(D2); // CONNECT BUT1 TO D2
jordiluong 4:ea7689bf97e1 29 InterruptIn button2(D3); // CONNECT BUT2 TO D3
jordiluong 10:a9e344e440b8 30 InterruptIn button3(SW2);
jordiluong 10:a9e344e440b8 31 InterruptIn button4(SW3);
jordiluong 7:757e95b4dc46 32 AnalogIn emg(A0);
jordiluong 3:5c3edcd29448 33
jordiluong 7:757e95b4dc46 34 // MOTOR CONTROL ///////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 35 Ticker controllerTicker;
jordiluong 10:a9e344e440b8 36 const double controller_Ts = 0.01; // Time step for controllerTicker [s]; Should be between 5kHz and 10kHz
jordiluong 5:0d3e8694726e 37 const double motorRatio = 131.25; // Ratio of the gearbox in the motors []
jordiluong 7:757e95b4dc46 38 const double radPerPulse = 2*pi/(32*motorRatio); // Amount of radians the motor rotates per encoder pulse [rad/pulse]
jordiluong 10:a9e344e440b8 39 double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector
jordiluong 7:757e95b4dc46 40
jordiluong 7:757e95b4dc46 41 // MOTOR 1
jordiluong 10:a9e344e440b8 42 double position1; // Position of motor 1 [rad]
jordiluong 10:a9e344e440b8 43 volatile double reference1 = 0; // Desired rotation of motor 1 [rad]
jordiluong 10:a9e344e440b8 44 double motor1Max = 2*pi*45/360; // Maximum rotation of motor 1 [rad]
jordiluong 10:a9e344e440b8 45 double motor1Min = 0; // Minimum rotation of motor 1 [rad]
jordiluong 5:0d3e8694726e 46 // Controller gains
jordiluong 10:a9e344e440b8 47 const double motor1_KP = 5; // Position gain []
jordiluong 10:a9e344e440b8 48 const double motor1_KI = 5; // Integral gain []
jordiluong 10:a9e344e440b8 49 const double motor1_KD = 0.0; // Derivative gain []
jordiluong 5:0d3e8694726e 50 double motor1_err_int = 0, motor1_prev_err = 0;
jordiluong 5:0d3e8694726e 51 // Derivative filter coefficients
jordiluong 7:757e95b4dc46 52 const double motor1_f_a1 = 1.0, motor1_f_a2 = 2.0; // Derivative filter coefficients []
jordiluong 7:757e95b4dc46 53 const double motor1_f_b0 = 1.0, motor1_f_b1 = 3.0, motor1_f_b2 = 4.0; // Derivative filter coefficients []
jordiluong 5:0d3e8694726e 54 // Filter variables
jordiluong 5:0d3e8694726e 55 double motor1_f_v1 = 0, motor1_f_v2 = 0;
jordiluong 7:757e95b4dc46 56
jordiluong 7:757e95b4dc46 57 // MOTOR 2
jordiluong 10:a9e344e440b8 58 double position2; // Position of motor 2 [rad]
jordiluong 10:a9e344e440b8 59 volatile double reference2 = 0; // Desired rotation of motor 2 [rad]
jordiluong 10:a9e344e440b8 60 double motor2Max = 2*pi*45/360; // Maximum rotation of motor 2 [rad]
jordiluong 10:a9e344e440b8 61 double motor2Min = 2*pi*-45/360; // Minimum rotation of motor 2 [rad]
jordiluong 5:0d3e8694726e 62 // Controller gains
jordiluong 10:a9e344e440b8 63 const double motor2_KP = 3; // Position gain []
jordiluong 10:a9e344e440b8 64 const double motor2_KI = 3; // Integral gain []
jordiluong 10:a9e344e440b8 65 const double motor2_KD = 0.0; // Derivative gain []
jordiluong 10:a9e344e440b8 66 double motor2_err_int = 0, motor2_prev_err = 0;
jordiluong 5:0d3e8694726e 67 // Derivative filter coefficients
jordiluong 10:a9e344e440b8 68 const double motor2_f_a1 = 1.0, motor2_f_a2 = 2.0; // Derivative filter coefficients []
jordiluong 10:a9e344e440b8 69 const double motor2_f_b0 = 1.0, motor2_f_b1 = 3.0, motor2_f_b2 = 4.0; // Derivative filter coefficients []
jordiluong 5:0d3e8694726e 70 // Filter variables
jordiluong 10:a9e344e440b8 71 double motor2_f_v1 = 0, motor2_f_v2 = 0;
jordiluong 0:80ac024b84cb 72
jordiluong 7:757e95b4dc46 73 // EMG FILTER //////////////////////////////////////////////////////////////////
jordiluong 7:757e95b4dc46 74 BiQuadChain bqc;
jordiluong 7:757e95b4dc46 75 BiQuad bq1(0, 0, 0, 0, 0); // EMG filter coefficients []
jordiluong 7:757e95b4dc46 76 BiQuad bq2(0, 0, 0, 0, 0); // EMG filter coefficients []
jordiluong 7:757e95b4dc46 77 Ticker emgSampleTicker;
jordiluong 10:a9e344e440b8 78 double emg_Ts = 0.1; // Time step for EMG sampling [s]
jordiluong 7:757e95b4dc46 79
jordiluong 7:757e95b4dc46 80
jordiluong 7:757e95b4dc46 81 // FUNCTIONS ///////////////////////////////////////////////////////////////////
jordiluong 7:757e95b4dc46 82 // BIQUAD FILTER FOR DERIVATIVE OF ERROR ///////////////////////////////////////
jordiluong 5:0d3e8694726e 83 double biquad(double u, double &v1, double &v2, const double a1,
jordiluong 5:0d3e8694726e 84 const double a2, const double b0, const double b1, const double b2)
jordiluong 0:80ac024b84cb 85 {
jordiluong 5:0d3e8694726e 86 double v = u - a1*v1 - a2*v2;
jordiluong 5:0d3e8694726e 87 double y = b0*v + b1*v1 + b2*v2;
jordiluong 5:0d3e8694726e 88 v2 = v1; v1 = v;
jordiluong 5:0d3e8694726e 89 return y;
jordiluong 0:80ac024b84cb 90 }
jordiluong 0:80ac024b84cb 91
jordiluong 7:757e95b4dc46 92 // P-CONTROLLER ////////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 93 double P_Controller(double error, const double Kp)
jordiluong 0:80ac024b84cb 94 {
jordiluong 5:0d3e8694726e 95 return Kp * error;
jordiluong 0:80ac024b84cb 96 }
jordiluong 0:80ac024b84cb 97
jordiluong 7:757e95b4dc46 98 // PID-CONTROLLER WITH FILTER //////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 99 double PID_Controller(double e, const double Kp, const double Ki,
jordiluong 5:0d3e8694726e 100 const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1,
jordiluong 5:0d3e8694726e 101 double &f_v2, const double f_a1, const double f_a2, const double f_b0,
jordiluong 5:0d3e8694726e 102 const double f_b1, const double f_b2)
jordiluong 0:80ac024b84cb 103 {
jordiluong 5:0d3e8694726e 104 // Derivative
jordiluong 8:78d8ccf84a38 105 double e_der = (e - e_prev)/Ts; // Calculate the derivative of error
jordiluong 10:a9e344e440b8 106 //pc.printf("der error %f \r\n", e_der);
jordiluong 10:a9e344e440b8 107 //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); // Filter the derivative of error
jordiluong 5:0d3e8694726e 108 e_prev = e;
jordiluong 5:0d3e8694726e 109 // Integral
jordiluong 8:78d8ccf84a38 110 e_int = e_int + Ts*e; // Calculate the integral of error
jordiluong 5:0d3e8694726e 111 // PID
jordiluong 10:a9e344e440b8 112 //pc.printf("NAAA der error %f \r\n", e_der);
jordiluong 10:a9e344e440b8 113 return -(Kp*e + Ki*e_int + Kd*e_der);
jordiluong 3:5c3edcd29448 114 }
jordiluong 3:5c3edcd29448 115
jordiluong 7:757e95b4dc46 116 // MOTOR 1 /////////////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 117 void RotateMotor1(double motor1Value)
jordiluong 3:5c3edcd29448 118 {
jordiluong 10:a9e344e440b8 119 if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING
jordiluong 5:0d3e8694726e 120 {
jordiluong 10:a9e344e440b8 121 if(motor1Value >= 0) motor1DirectionPin = 0; // Rotate motor 1 CW
jordiluong 10:a9e344e440b8 122 else motor1DirectionPin = 1; // Rotate motor 1 CCW
jordiluong 5:0d3e8694726e 123
jordiluong 5:0d3e8694726e 124 if(fabs(motor1Value) > 1) motor1MagnitudePin = 1;
jordiluong 5:0d3e8694726e 125 else motor1MagnitudePin = fabs(motor1Value);
jordiluong 5:0d3e8694726e 126 }
jordiluong 5:0d3e8694726e 127 else motor1MagnitudePin = 0;
jordiluong 3:5c3edcd29448 128 }
jordiluong 0:80ac024b84cb 129
jordiluong 10:a9e344e440b8 130 // MOTOR 2 /////////////////////////////////////////////////////////////////////
jordiluong 10:a9e344e440b8 131 void RotateMotor2(double motor2Value)
jordiluong 10:a9e344e440b8 132 {
jordiluong 10:a9e344e440b8 133 if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING
jordiluong 10:a9e344e440b8 134 {
jordiluong 10:a9e344e440b8 135 if(motor2Value >= 0) motor2DirectionPin = 1; // Rotate motor 1 CW
jordiluong 10:a9e344e440b8 136 else motor2DirectionPin = 0; // Rotate motor 1 CCW
jordiluong 10:a9e344e440b8 137
jordiluong 10:a9e344e440b8 138 if(fabs(motor2Value) > 1) motor2MagnitudePin = 1;
jordiluong 10:a9e344e440b8 139 else motor2MagnitudePin = fabs(motor2Value);
jordiluong 10:a9e344e440b8 140 }
jordiluong 10:a9e344e440b8 141 else motor2MagnitudePin = 0;
jordiluong 10:a9e344e440b8 142 }
jordiluong 10:a9e344e440b8 143
jordiluong 7:757e95b4dc46 144 // MOTOR 1 P-CONTROLLER ////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 145 void Motor1PController()
jordiluong 5:0d3e8694726e 146 {
jordiluong 10:a9e344e440b8 147 position1 = radPerPulse * Encoder1.getPulses(); // Calculate the rotation of motor 1
jordiluong 10:a9e344e440b8 148 //pc.printf("%f", position1);
jordiluong 5:0d3e8694726e 149 double motor1Value = P_Controller(reference1 - position1, motor1_KP);
jordiluong 5:0d3e8694726e 150 RotateMotor1(motor1Value);
jordiluong 5:0d3e8694726e 151 }
jordiluong 5:0d3e8694726e 152
jordiluong 7:757e95b4dc46 153 // MOTOR 1 PID-CONTROLLER //////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 154 void Motor1Controller()
jordiluong 5:0d3e8694726e 155 {
jordiluong 10:a9e344e440b8 156 position1 = radPerPulse * Encoder1.getPulses();
jordiluong 10:a9e344e440b8 157 pc.printf("error %f \r\n", reference1 - position1);
jordiluong 5:0d3e8694726e 158 double motor1Value = PID_Controller(reference1 - position1, motor1_KP,
jordiluong 5:0d3e8694726e 159 motor1_KI, motor1_KD, controller_Ts, motor1_err_int, motor1_prev_err,
jordiluong 5:0d3e8694726e 160 motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0,
jordiluong 5:0d3e8694726e 161 motor1_f_b1, motor1_f_b2);
jordiluong 10:a9e344e440b8 162 //pc.printf("motor value %f \r\n",motor1Value);
jordiluong 5:0d3e8694726e 163 RotateMotor1(motor1Value);
jordiluong 5:0d3e8694726e 164 }
jordiluong 5:0d3e8694726e 165
jordiluong 10:a9e344e440b8 166 // MOTOR 2 PID-CONTROLLER //////////////////////////////////////////////////////
jordiluong 10:a9e344e440b8 167 void Motor2Controller()
jordiluong 10:a9e344e440b8 168 {
jordiluong 10:a9e344e440b8 169 position2 = radPerPulse * Encoder2.getPulses();
jordiluong 10:a9e344e440b8 170 //pc.printf("%f", position2);
jordiluong 10:a9e344e440b8 171 double motor2Value = PID_Controller(reference2 - position2, motor2_KP,
jordiluong 10:a9e344e440b8 172 motor2_KI, motor2_KD, controller_Ts, motor2_err_int, motor2_prev_err,
jordiluong 10:a9e344e440b8 173 motor2_f_v1, motor2_f_v2, motor2_f_a1, motor2_f_a2, motor2_f_b0,
jordiluong 10:a9e344e440b8 174 motor2_f_b1, motor2_f_b2);
jordiluong 10:a9e344e440b8 175 pc.printf("motor value %f \r\n",motor2Value);
jordiluong 10:a9e344e440b8 176 RotateMotor2(motor2Value);
jordiluong 10:a9e344e440b8 177 }
jordiluong 10:a9e344e440b8 178
jordiluong 7:757e95b4dc46 179 // TURN OFF MOTORS /////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 180 void TurnMotorsOff()
jordiluong 5:0d3e8694726e 181 {
jordiluong 5:0d3e8694726e 182 motor1MagnitudePin = 0;
jordiluong 5:0d3e8694726e 183 motor2MagnitudePin = 0;
jordiluong 5:0d3e8694726e 184 }
jordiluong 5:0d3e8694726e 185
jordiluong 10:a9e344e440b8 186 // DETERMINE JOINT VELOCITIES //////////////////////////////////////////////////
jordiluong 10:a9e344e440b8 187 void jointVelocities()
jordiluong 10:a9e344e440b8 188 {
jordiluong 10:a9e344e440b8 189 position1 = radPerPulse * Encoder1.getPulses();
jordiluong 10:a9e344e440b8 190 position2 = radPerPulse * Encoder2.getPulses();
jordiluong 10:a9e344e440b8 191
jordiluong 10:a9e344e440b8 192
jordiluong 10:a9e344e440b8 193 if(!button1 ) xVelocity = 0.3;//&& position1 > motor1Min && position2 > motor2Min
jordiluong 10:a9e344e440b8 194 else if(!button2 ) xVelocity = -0.3;//&& position1 < motor1Max && position2 < motor2Max
jordiluong 10:a9e344e440b8 195 else xVelocity = 0;
jordiluong 10:a9e344e440b8 196
jordiluong 10:a9e344e440b8 197 if(!button3 ) yVelocity = 0.3;//&& position1 < motor1Max && position2 < motor2Max
jordiluong 10:a9e344e440b8 198 else if(!button4 ) yVelocity = -0.3;//&& position1 < motor1Max && position2 > motor2Min
jordiluong 10:a9e344e440b8 199 else yVelocity = 0;
jordiluong 10:a9e344e440b8 200
jordiluong 10:a9e344e440b8 201 //pc.printf("x %f, y %f \r\n", xVelocity, yVelocity);
jordiluong 10:a9e344e440b8 202
jordiluong 10:a9e344e440b8 203 // Construct Jacobian
jordiluong 10:a9e344e440b8 204 double q[4];
jordiluong 10:a9e344e440b8 205 q[0] = position1, q[1] = -position1;
jordiluong 10:a9e344e440b8 206 q[2] = position2, q[3] = -position2;
jordiluong 10:a9e344e440b8 207
jordiluong 10:a9e344e440b8 208 double T2[3]; // Second column of the jacobian
jordiluong 10:a9e344e440b8 209 double T3[3]; // Third column of the jacobian
jordiluong 10:a9e344e440b8 210 double T4[3]; // Fourth column of the jacobian
jordiluong 10:a9e344e440b8 211 double T1[6];
jordiluong 10:a9e344e440b8 212 static const signed char b_T1[3] = { 1, 0, 0 };
jordiluong 10:a9e344e440b8 213 double J_data[6];
jordiluong 10:a9e344e440b8 214
jordiluong 10:a9e344e440b8 215 T2[0] = 1.0;
jordiluong 10:a9e344e440b8 216 T2[1] = 0.365 * cos(q[0]);
jordiluong 10:a9e344e440b8 217 T2[2] = 0.365 * sin(q[0]);
jordiluong 10:a9e344e440b8 218 T3[0] = 1.0;
jordiluong 10:a9e344e440b8 219 T3[1] = 0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 +
jordiluong 10:a9e344e440b8 220 q[0]) + q[1]);
jordiluong 10:a9e344e440b8 221 T3[2] = 0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 +
jordiluong 10:a9e344e440b8 222 q[0]) + q[1]);
jordiluong 10:a9e344e440b8 223 T4[0] = 1.0;
jordiluong 10:a9e344e440b8 224 T4[1] = (0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 +
jordiluong 10:a9e344e440b8 225 q[0]) + q[1])) + 0.265 * sin((q[0] + q[1]) + q[2]);
jordiluong 10:a9e344e440b8 226 T4[2] = (0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 +
jordiluong 10:a9e344e440b8 227 q[0]) + q[1])) - 0.265 * cos((q[0] + q[1]) + q[2]);
jordiluong 10:a9e344e440b8 228
jordiluong 10:a9e344e440b8 229 for (int i = 0; i < 3; i++)
jordiluong 10:a9e344e440b8 230 {
jordiluong 10:a9e344e440b8 231 T1[i] = (double)b_T1[i] - T2[i];
jordiluong 10:a9e344e440b8 232 T1[3 + i] = T3[i] - T4[i];
jordiluong 10:a9e344e440b8 233 }
jordiluong 10:a9e344e440b8 234
jordiluong 10:a9e344e440b8 235 for (int i = 0; i < 2; i++)
jordiluong 10:a9e344e440b8 236 {
jordiluong 10:a9e344e440b8 237 for (int j = 0; j < 3; j++)
jordiluong 10:a9e344e440b8 238 {
jordiluong 10:a9e344e440b8 239 J_data[j + 3 * i] = T1[j + 3 * i];
jordiluong 10:a9e344e440b8 240 }
jordiluong 10:a9e344e440b8 241 }
jordiluong 10:a9e344e440b8 242
jordiluong 10:a9e344e440b8 243 // Here the first row of the Jacobian is cut off, so we do not take rotation into consideration
jordiluong 10:a9e344e440b8 244 // Note: the matrices from now on will we constructed rowwise
jordiluong 10:a9e344e440b8 245 double Jvelocity[4];
jordiluong 10:a9e344e440b8 246 Jvelocity[0] = J_data[1];
jordiluong 10:a9e344e440b8 247 Jvelocity[1] = J_data[4];
jordiluong 10:a9e344e440b8 248 Jvelocity[2] = J_data[2];
jordiluong 10:a9e344e440b8 249 Jvelocity[3] = J_data[5];
jordiluong 10:a9e344e440b8 250
jordiluong 10:a9e344e440b8 251 // Creating the inverse Jacobian
jordiluong 10:a9e344e440b8 252 double Jvelocity_inv[4]; // The inverse matrix of the jacobian
jordiluong 10:a9e344e440b8 253 double determ = Jvelocity[0]*Jvelocity[3]-Jvelocity[1]*Jvelocity[2]; // The determinant of the matrix
jordiluong 10:a9e344e440b8 254 Jvelocity_inv[0] = Jvelocity[3]/determ;
jordiluong 10:a9e344e440b8 255 Jvelocity_inv[1] = -Jvelocity[1]/determ;
jordiluong 10:a9e344e440b8 256 Jvelocity_inv[2] = -Jvelocity[2]/determ;
jordiluong 10:a9e344e440b8 257 Jvelocity_inv[3] = Jvelocity[0]/determ;
jordiluong 10:a9e344e440b8 258
jordiluong 10:a9e344e440b8 259 // Now the velocity of the joints are found by giving the velocity of the end-effector and the inverse jacobian
jordiluong 10:a9e344e440b8 260 double msh[2]; // This is the velocity the joints have to have
jordiluong 10:a9e344e440b8 261 msh[0] = xVelocity*Jvelocity_inv[0] + yVelocity*Jvelocity_inv[1];
jordiluong 10:a9e344e440b8 262 msh[1] = xVelocity*Jvelocity_inv[2] + yVelocity*Jvelocity_inv[3];
jordiluong 10:a9e344e440b8 263
jordiluong 10:a9e344e440b8 264 if(position1 + msh[0]*emg_Ts > motor1Max) reference1 = motor1Max;
jordiluong 10:a9e344e440b8 265 else if(position1 + msh[0]*emg_Ts < motor1Min) reference1 = motor1Min;
jordiluong 10:a9e344e440b8 266 else reference1 = position1 + msh[0]*emg_Ts;
jordiluong 10:a9e344e440b8 267
jordiluong 10:a9e344e440b8 268 if(position2 + msh[1]*emg_Ts > motor2Max) reference2 = motor2Max;
jordiluong 10:a9e344e440b8 269 else if(position2 + msh[1]*emg_Ts < motor2Min) reference2 = motor2Min;
jordiluong 10:a9e344e440b8 270 else reference2 = position2 + msh[1]*emg_Ts;
jordiluong 10:a9e344e440b8 271
jordiluong 10:a9e344e440b8 272 Motor1Controller(), Motor2Controller();
jordiluong 10:a9e344e440b8 273
jordiluong 10:a9e344e440b8 274 pc.printf("position 1 %f, 2 %f \r\n", position1/2/pi*360, position2/2/pi*360);
jordiluong 10:a9e344e440b8 275 pc.printf("reference 1 %f, 2 %f \r\n", reference1/2/pi*360, reference2/2/pi*360);
jordiluong 10:a9e344e440b8 276 pc.printf("msh*Ts 1 %f, 2 %f \r\n\n", msh[0]*emg_Ts, msh[1]*emg_Ts);
jordiluong 10:a9e344e440b8 277 }
jordiluong 10:a9e344e440b8 278
jordiluong 10:a9e344e440b8 279 // EMG /////////////////////////////////////////////////////////////////////////
jordiluong 10:a9e344e440b8 280 void EmgSample()
jordiluong 10:a9e344e440b8 281 {
jordiluong 10:a9e344e440b8 282 if(currentState == MOVING)
jordiluong 10:a9e344e440b8 283 {
jordiluong 10:a9e344e440b8 284 //double emgFiltered = bqc.step(emg.read()); // Filter the EMG signal
jordiluong 10:a9e344e440b8 285 /*
jordiluong 10:a9e344e440b8 286 If EMG signal 1 --> xVelocity / yVelocity = velocity
jordiluong 10:a9e344e440b8 287 Else if EMG signal 2 --> xVelocity / yVelocity = velocity
jordiluong 10:a9e344e440b8 288 Else xVelocity / yVelocity = 0
jordiluong 10:a9e344e440b8 289 */
jordiluong 10:a9e344e440b8 290 jointVelocities();
jordiluong 10:a9e344e440b8 291 }
jordiluong 10:a9e344e440b8 292 }
jordiluong 10:a9e344e440b8 293
jordiluong 7:757e95b4dc46 294 // STATES //////////////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 295 void ProcessStateMachine()
jordiluong 0:80ac024b84cb 296 {
jordiluong 0:80ac024b84cb 297 switch(currentState)
jordiluong 0:80ac024b84cb 298 {
jordiluong 0:80ac024b84cb 299 case MOTORS_OFF:
jordiluong 0:80ac024b84cb 300 {
jordiluong 0:80ac024b84cb 301 // State initialization
jordiluong 0:80ac024b84cb 302 if(stateChanged)
jordiluong 0:80ac024b84cb 303 {
jordiluong 10:a9e344e440b8 304 pc.printf("Entering MOTORS_OFF \r\n"
jordiluong 10:a9e344e440b8 305 "Press button 1 to enter MOVING \r\n");
jordiluong 5:0d3e8694726e 306 TurnMotorsOff(); // Turn motors off
jordiluong 0:80ac024b84cb 307 stateChanged = false;
jordiluong 0:80ac024b84cb 308 }
jordiluong 0:80ac024b84cb 309
jordiluong 0:80ac024b84cb 310 // Home command
jordiluong 4:ea7689bf97e1 311 if(!button1)
jordiluong 0:80ac024b84cb 312 {
jordiluong 0:80ac024b84cb 313 currentState = MOVING;
jordiluong 0:80ac024b84cb 314 stateChanged = true;
jordiluong 0:80ac024b84cb 315 break;
jordiluong 0:80ac024b84cb 316 }
jordiluong 4:ea7689bf97e1 317 break;
jordiluong 0:80ac024b84cb 318 }
jordiluong 0:80ac024b84cb 319
jordiluong 0:80ac024b84cb 320 case MOVING:
jordiluong 0:80ac024b84cb 321 {
jordiluong 0:80ac024b84cb 322 // State initialization
jordiluong 0:80ac024b84cb 323 if(stateChanged)
jordiluong 0:80ac024b84cb 324 {
jordiluong 10:a9e344e440b8 325 pc.printf("Entering MOVING \r\n"
jordiluong 10:a9e344e440b8 326 "Press button 2 to enter HITTING \r\n");
jordiluong 0:80ac024b84cb 327 stateChanged = false;
jordiluong 0:80ac024b84cb 328 }
jordiluong 0:80ac024b84cb 329
jordiluong 0:80ac024b84cb 330 // Hit command
jordiluong 10:a9e344e440b8 331 /*if(!button2)
jordiluong 0:80ac024b84cb 332 {
jordiluong 0:80ac024b84cb 333 currentState = HITTING;
jordiluong 0:80ac024b84cb 334 stateChanged = true;
jordiluong 0:80ac024b84cb 335 break;
jordiluong 0:80ac024b84cb 336 }
jordiluong 10:a9e344e440b8 337 */
jordiluong 4:ea7689bf97e1 338 break;
jordiluong 0:80ac024b84cb 339 }
jordiluong 0:80ac024b84cb 340
jordiluong 0:80ac024b84cb 341 case HITTING:
jordiluong 0:80ac024b84cb 342 {
jordiluong 0:80ac024b84cb 343 // State initialization
jordiluong 0:80ac024b84cb 344 if(stateChanged)
jordiluong 0:80ac024b84cb 345 {
jordiluong 0:80ac024b84cb 346 pc.printf("Entering HITTING \r\n");
jordiluong 5:0d3e8694726e 347 stateChanged = false;
jordiluong 4:ea7689bf97e1 348 //HitBall(); // Hit the ball
jordiluong 0:80ac024b84cb 349 currentState = MOVING;
jordiluong 0:80ac024b84cb 350 stateChanged = true;
jordiluong 0:80ac024b84cb 351 break;
jordiluong 0:80ac024b84cb 352 }
jordiluong 4:ea7689bf97e1 353 break;
jordiluong 0:80ac024b84cb 354 }
jordiluong 0:80ac024b84cb 355
jordiluong 0:80ac024b84cb 356 default:
jordiluong 0:80ac024b84cb 357 {
jordiluong 5:0d3e8694726e 358 TurnMotorsOff(); // Turn motors off for safety
jordiluong 4:ea7689bf97e1 359 break;
jordiluong 0:80ac024b84cb 360 }
jordiluong 0:80ac024b84cb 361 }
jordiluong 0:80ac024b84cb 362 }
jordiluong 0:80ac024b84cb 363
jordiluong 7:757e95b4dc46 364 // MAIN FUNCTION ///////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 365 int main()
jordiluong 0:80ac024b84cb 366 {
jordiluong 0:80ac024b84cb 367 // Serial communication
jordiluong 0:80ac024b84cb 368 pc.baud(115200);
jordiluong 0:80ac024b84cb 369
jordiluong 4:ea7689bf97e1 370 pc.printf("START \r\n");
jordiluong 4:ea7689bf97e1 371
jordiluong 8:78d8ccf84a38 372 bqc.add(&bq1).add(&bq2); // Make BiQuad Chain
jordiluong 7:757e95b4dc46 373
jordiluong 10:a9e344e440b8 374 //controllerTicker.attach(&Motor1PController, controller_Ts); // Ticker to control motor 1 (P)
jordiluong 10:a9e344e440b8 375 //controllerTicker.attach(&Motor1Controller, controller_Ts); // Ticker to control motor 1 (PID)
jordiluong 8:78d8ccf84a38 376 emgSampleTicker.attach(&EmgSample, emg_Ts); // Ticker to sample EMG
jordiluong 4:ea7689bf97e1 377
jordiluong 0:80ac024b84cb 378 while(true)
jordiluong 0:80ac024b84cb 379 {
jordiluong 0:80ac024b84cb 380 ProcessStateMachine(); // Execute states function
jordiluong 0:80ac024b84cb 381 }
jordiluong 0:80ac024b84cb 382 }