Jordi Luong / Mbed 2 deprecated Project_BioRobotics

Project BioRobotics Group 19

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@22:68e3dc374488, 2017-11-03 (annotated)

Committer:
jordiluong
Date:
Fri Nov 03 10:16:54 2017 +0000
Revision:
22:68e3dc374488
Parent:
13:ec227b229f3d 
Minor update

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