Jordi Luong / Mbed 2 deprecated Project_BioRobotics

Project BioRobotics Group 19

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@23:a87fd4719655, 2017-11-07 (annotated)

Committer:
jordiluong
Date:
Tue Nov 07 09:33:51 2017 +0000
Revision:
23:a87fd4719655
Parent:
21:2e732eb85daf 
Final version

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 14:95acac6a07c7 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 18:2b6f41f39a7f 15 enum states{MOTORS_OFF, CALIBRATING, MOVING};
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 18:2b6f41f39a7f 20 QEI Encoder1(D10,D11,NC,32); // CONNECT ENC1A TO D12, ENC1B TO D13
jordiluong 18:2b6f41f39a7f 21 QEI Encoder2(D12,D13,NC,32); // CONNECT ENC2A TO D10, ENC2B TO D11
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 18:2b6f41f39a7f 26 DigitalOut motor2DirectionPin(D7); // Value 0: CW; 1: 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 14:95acac6a07c7 30 InterruptIn button3(SW2);
jordiluong 10:a9e344e440b8 31 InterruptIn button4(SW3);
jordiluong 14:95acac6a07c7 32 AnalogIn potmeter1(A0); // CONNECT POT1 TO A0
jordiluong 14:95acac6a07c7 33 AnalogIn potmeter2(A1); // CONNECT POT2 TO A1
jordiluong 14:95acac6a07c7 34 DigitalOut led1(LED_RED);
jordiluong 14:95acac6a07c7 35 DigitalOut led2(LED_BLUE);
jordiluong 14:95acac6a07c7 36 DigitalOut led3(LED_GREEN);
jordiluong 15:5d24f832bb7b 37 DigitalOut led4(D8); // CONNECT LED1 TO D8
jordiluong 15:5d24f832bb7b 38 DigitalOut led5(D9); // CONNECT LED2 TO D9
jordiluong 14:95acac6a07c7 39 AnalogIn emg_r(A2); // CONNECT EMG TO A2
jordiluong 14:95acac6a07c7 40 AnalogIn emg_l(A3); // CONNECT EMG TO A3
jordiluong 13:ec227b229f3d 41
jordiluong 7:757e95b4dc46 42 // MOTOR CONTROL ///////////////////////////////////////////////////////////////
jordiluong 14:95acac6a07c7 43 Ticker controllerTicker; // Ticker for the controller
jordiluong 19:6f720e4fcb47 44 const double controllerTs = 1/201.3; // Time step for controllerTicker [s]
jordiluong 5:0d3e8694726e 45 const double motorRatio = 131.25; // Ratio of the gearbox in the motors []
jordiluong 7:757e95b4dc46 46 const double radPerPulse = 2*pi/(32*motorRatio); // Amount of radians the motor rotates per encoder pulse [rad/pulse]
jordiluong 14:95acac6a07c7 47 volatile double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start
jordiluong 19:6f720e4fcb47 48 const double velocity = 0.02; // X and Y velocity of the end effector when desired
jordiluong 13:ec227b229f3d 49
jordiluong 7:757e95b4dc46 50 // MOTOR 1
jordiluong 14:95acac6a07c7 51 volatile double position1; // Position of motor 1 [rad]
jordiluong 18:2b6f41f39a7f 52 volatile double reference1 = 2*pi*-5/360; // Desired rotation of motor 1 [rad]
jordiluong 16:2cf8c2705936 53 const double motor1Max = 0; // Maximum rotation of motor 1 [rad]
jordiluong 16:2cf8c2705936 54 const double motor1Min = 2*pi*-40/360; // Minimum rotation of motor 1 [rad]
jordiluong 5:0d3e8694726e 55 // Controller gains
jordiluong 14:95acac6a07c7 56 const double motor1_KP = 13; // Position gain []
jordiluong 13:ec227b229f3d 57 const double motor1_KI = 7; // Integral gain []
jordiluong 13:ec227b229f3d 58 const double motor1_KD = 0.3; // Derivative gain []
jordiluong 5:0d3e8694726e 59 double motor1_err_int = 0, motor1_prev_err = 0;
jordiluong 5:0d3e8694726e 60 // Derivative filter coefficients
jordiluong 14:95acac6a07c7 61 const double motor1_f_a1 = 0.25, motor1_f_a2 = 0.8; // Derivative filter coefficients []
jordiluong 14:95acac6a07c7 62 const double motor1_f_b0 = 1, motor1_f_b1 = 2, motor1_f_b2 = 0.8; // Derivative filter coefficients []
jordiluong 5:0d3e8694726e 63 // Filter variables
jordiluong 5:0d3e8694726e 64 double motor1_f_v1 = 0, motor1_f_v2 = 0;
jordiluong 13:ec227b229f3d 65
jordiluong 7:757e95b4dc46 66 // MOTOR 2
jordiluong 14:95acac6a07c7 67 volatile double position2; // Position of motor 2 [rad]
jordiluong 14:95acac6a07c7 68 volatile double reference2 = 0; // Desired rotation of motor 2 [rad]
jordiluong 16:2cf8c2705936 69 const double motor2Max = 2*pi*25/360; // Maximum rotation of motor 2 [rad]
jordiluong 16:2cf8c2705936 70 const double motor2Min = 2*pi*-28/360; // Minimum rotation of motor 2 [rad]
jordiluong 5:0d3e8694726e 71 // Controller gains
jordiluong 14:95acac6a07c7 72 const double motor2_KP = 13; // Position gain []
jordiluong 13:ec227b229f3d 73 const double motor2_KI = 5; // Integral gain []
jordiluong 13:ec227b229f3d 74 const double motor2_KD = 0.1; // Derivative gain []
jordiluong 10:a9e344e440b8 75 double motor2_err_int = 0, motor2_prev_err = 0;
jordiluong 5:0d3e8694726e 76 // Derivative filter coefficients
jordiluong 14:95acac6a07c7 77 const double motor2_f_a1 = 0.25, motor2_f_a2 = 0.8; // Derivative filter coefficients []
jordiluong 14:95acac6a07c7 78 const double motor2_f_b0 = 1, motor2_f_b1 = 2, motor2_f_b2 = 0.8; // Derivative filter coefficients []
jordiluong 5:0d3e8694726e 79 // Filter variables
jordiluong 10:a9e344e440b8 80 double motor2_f_v1 = 0, motor2_f_v2 = 0;
jordiluong 13:ec227b229f3d 81
jordiluong 18:2b6f41f39a7f 82 // EMG /////////////////////////////////////////////////////////////////////////
jordiluong 18:2b6f41f39a7f 83 Ticker emgLeft; // Ticker for EMG of left arm
jordiluong 18:2b6f41f39a7f 84 Ticker emgRight; // Ticker for EMG of right arm
jordiluong 21:2e732eb85daf 85 const double emgTs = 0.4993; // Time step for EMG sampling [s]
jordiluong 14:95acac6a07c7 86 // Filters
jordiluong 18:2b6f41f39a7f 87 BiQuadChain bqc;
jordiluong 18:2b6f41f39a7f 88 BiQuad bq2_high(0.875182, -1.750364, 0.87518, -1.73472, 0.766004); // High pass filter
jordiluong 18:2b6f41f39a7f 89 BiQuad bq3_notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780); // Notch filter
jordiluong 18:2b6f41f39a7f 90 BiQuad bq1_low(3.65747e-2, 7.31495e-2, 3.65747e-2, -1.390892, 0.537191); // Low pass filter
jordiluong 14:95acac6a07c7 91 // Right arm
jordiluong 14:95acac6a07c7 92 volatile double emgFiltered_r;
jordiluong 14:95acac6a07c7 93 volatile double filteredAbs_r;
jordiluong 14:95acac6a07c7 94 volatile double emg_value_r;
jordiluong 14:95acac6a07c7 95 volatile double onoffsignal_r;
jordiluong 18:2b6f41f39a7f 96 volatile bool check_calibration_r = false;
jordiluong 14:95acac6a07c7 97 volatile double avg_emg_r;
jordiluong 14:95acac6a07c7 98 volatile bool active_r = false;
jordiluong 14:95acac6a07c7 99 // Left arm
jordiluong 14:95acac6a07c7 100 volatile double emgFiltered_l;
jordiluong 14:95acac6a07c7 101 volatile double filteredAbs_l;
jordiluong 14:95acac6a07c7 102 volatile double emg_value_l;
jordiluong 14:95acac6a07c7 103 volatile double onoffsignal_l;
jordiluong 18:2b6f41f39a7f 104 volatile bool check_calibration_l = false;
jordiluong 14:95acac6a07c7 105 volatile double avg_emg_l;
jordiluong 14:95acac6a07c7 106 volatile bool active_l = false;
jordiluong 15:5d24f832bb7b 107
jordiluong 18:2b6f41f39a7f 108 // PROCESS EMG SIGNALS /////////////////////////////////////////////////////////
jordiluong 18:2b6f41f39a7f 109 Ticker processTicker; // Ticker for processing of EMG
jordiluong 21:2e732eb85daf 110 const double processTs = 0.101; // Time step for processing of EMG [s]
jordiluong 18:2b6f41f39a7f 111
jordiluong 18:2b6f41f39a7f 112 volatile bool xdir = true, ydir = false; // Direction the EMG signal moves the end effector
jordiluong 18:2b6f41f39a7f 113 volatile int count = 0; // Counter to change direction
jordiluong 23:a87fd4719655 114
jordiluong 13:ec227b229f3d 115
jordiluong 7:757e95b4dc46 116 // FUNCTIONS ///////////////////////////////////////////////////////////////////
jordiluong 7:757e95b4dc46 117 // BIQUAD FILTER FOR DERIVATIVE OF ERROR ///////////////////////////////////////
jordiluong 5:0d3e8694726e 118 double biquad(double u, double &v1, double &v2, const double a1,
jordiluong 5:0d3e8694726e 119 const double a2, const double b0, const double b1, const double b2)
jordiluong 0:80ac024b84cb 120 {
jordiluong 5:0d3e8694726e 121 double v = u - a1*v1 - a2*v2;
jordiluong 5:0d3e8694726e 122 double y = b0*v + b1*v1 + b2*v2;
jordiluong 5:0d3e8694726e 123 v2 = v1; v1 = v;
jordiluong 5:0d3e8694726e 124 return y;
jordiluong 0:80ac024b84cb 125 }
jordiluong 13:ec227b229f3d 126
jordiluong 7:757e95b4dc46 127 // PID-CONTROLLER WITH FILTER //////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 128 double PID_Controller(double e, const double Kp, const double Ki,
jordiluong 5:0d3e8694726e 129 const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1,
jordiluong 5:0d3e8694726e 130 double &f_v2, const double f_a1, const double f_a2, const double f_b0,
jordiluong 5:0d3e8694726e 131 const double f_b1, const double f_b2)
jordiluong 0:80ac024b84cb 132 {
jordiluong 5:0d3e8694726e 133 // Derivative
jordiluong 8:78d8ccf84a38 134 double e_der = (e - e_prev)/Ts; // Calculate the derivative of error
jordiluong 12:12b72bd60fd1 135 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 136 e_prev = e;
jordiluong 5:0d3e8694726e 137 // Integral
jordiluong 8:78d8ccf84a38 138 e_int = e_int + Ts*e; // Calculate the integral of error
jordiluong 5:0d3e8694726e 139 // PID
jordiluong 18:2b6f41f39a7f 140 return Kp*e + Ki*e_int + Kd*e_der; // Calculate motor value
jordiluong 3:5c3edcd29448 141 }
jordiluong 13:ec227b229f3d 142
jordiluong 7:757e95b4dc46 143 // MOTOR 1 /////////////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 144 void RotateMotor1(double motor1Value)
jordiluong 3:5c3edcd29448 145 {
jordiluong 18:2b6f41f39a7f 146 if(currentState == MOVING) // Check if state is MOVING
jordiluong 5:0d3e8694726e 147 {
jordiluong 10:a9e344e440b8 148 if(motor1Value >= 0) motor1DirectionPin = 0; // Rotate motor 1 CW
jordiluong 10:a9e344e440b8 149 else motor1DirectionPin = 1; // Rotate motor 1 CCW
jordiluong 5:0d3e8694726e 150
jordiluong 5:0d3e8694726e 151 if(fabs(motor1Value) > 1) motor1MagnitudePin = 1;
jordiluong 5:0d3e8694726e 152 else motor1MagnitudePin = fabs(motor1Value);
jordiluong 5:0d3e8694726e 153 }
jordiluong 5:0d3e8694726e 154 else motor1MagnitudePin = 0;
jordiluong 3:5c3edcd29448 155 }
jordiluong 13:ec227b229f3d 156
jordiluong 10:a9e344e440b8 157 // MOTOR 2 /////////////////////////////////////////////////////////////////////
jordiluong 10:a9e344e440b8 158 void RotateMotor2(double motor2Value)
jordiluong 10:a9e344e440b8 159 {
jordiluong 18:2b6f41f39a7f 160 if(currentState == MOVING) // Check if state is MOVING
jordiluong 10:a9e344e440b8 161 {
jordiluong 18:2b6f41f39a7f 162 if(motor2Value >= 0) motor2DirectionPin = 1; // Rotate motor 2 CW
jordiluong 18:2b6f41f39a7f 163 else motor2DirectionPin = 0; // Rotate motor 2 CCW
jordiluong 10:a9e344e440b8 164
jordiluong 10:a9e344e440b8 165 if(fabs(motor2Value) > 1) motor2MagnitudePin = 1;
jordiluong 10:a9e344e440b8 166 else motor2MagnitudePin = fabs(motor2Value);
jordiluong 10:a9e344e440b8 167 }
jordiluong 10:a9e344e440b8 168 else motor2MagnitudePin = 0;
jordiluong 10:a9e344e440b8 169 }
jordiluong 13:ec227b229f3d 170
jordiluong 18:2b6f41f39a7f 171 // MOTOR PID-CONTROLLER //////////////////////////////////////////////////////
jordiluong 18:2b6f41f39a7f 172 void MotorController()
jordiluong 5:0d3e8694726e 173 {
jordiluong 14:95acac6a07c7 174 if(currentState == MOVING)
jordiluong 14:95acac6a07c7 175 {
jordiluong 18:2b6f41f39a7f 176 position1 = radPerPulse * Encoder1.getPulses(); // Get current position of motor 1
jordiluong 18:2b6f41f39a7f 177 position2 = radPerPulse * Encoder2.getPulses(); // Get current position of motor 2
jordiluong 18:2b6f41f39a7f 178
jordiluong 18:2b6f41f39a7f 179 double motor1Value = PID_Controller(reference1 - position1, motor1_KP, // Calculate motor value motor 1
jordiluong 18:2b6f41f39a7f 180 motor1_KI, motor1_KD, controllerTs, motor1_err_int, motor1_prev_err,
jordiluong 14:95acac6a07c7 181 motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0,
jordiluong 14:95acac6a07c7 182 motor1_f_b1, motor1_f_b2);
jordiluong 14:95acac6a07c7 183
jordiluong 18:2b6f41f39a7f 184 double motor2Value = PID_Controller(reference2 - position2, motor2_KP, // Calculate motor value motor 2
jordiluong 18:2b6f41f39a7f 185 motor2_KI, motor2_KD, controllerTs, motor2_err_int, motor2_prev_err,
jordiluong 14:95acac6a07c7 186 motor2_f_v1, motor2_f_v2, motor2_f_a1, motor2_f_a2, motor2_f_b0,
jordiluong 14:95acac6a07c7 187 motor2_f_b1, motor2_f_b2);
jordiluong 14:95acac6a07c7 188
jordiluong 18:2b6f41f39a7f 189 RotateMotor1(motor1Value); // Rotate motor 1
jordiluong 18:2b6f41f39a7f 190 RotateMotor2(motor2Value); // Rotate motor 2
jordiluong 14:95acac6a07c7 191 }
jordiluong 10:a9e344e440b8 192 }
jordiluong 18:2b6f41f39a7f 193
jordiluong 7:757e95b4dc46 194 // TURN OFF MOTORS /////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 195 void TurnMotorsOff()
jordiluong 5:0d3e8694726e 196 {
jordiluong 5:0d3e8694726e 197 motor1MagnitudePin = 0;
jordiluong 5:0d3e8694726e 198 motor2MagnitudePin = 0;
jordiluong 5:0d3e8694726e 199 }
jordiluong 13:ec227b229f3d 200
jordiluong 14:95acac6a07c7 201 // EMG /////////////////////////////////////////////////////////////////////////
jordiluong 14:95acac6a07c7 202 // Filter EMG signal of right arm
jordiluong 18:2b6f41f39a7f 203 void filter_r()
jordiluong 18:2b6f41f39a7f 204 {
jordiluong 18:2b6f41f39a7f 205 if(check_calibration_r == 1)
jordiluong 18:2b6f41f39a7f 206 {
jordiluong 18:2b6f41f39a7f 207 emg_value_r = emg_r.read(); // Get EMG signal
jordiluong 18:2b6f41f39a7f 208 emgFiltered_r = bqc.step(emg_value_r); // Filter EMG signal using BiQuad Chain
jordiluong 18:2b6f41f39a7f 209 filteredAbs_r = abs(emgFiltered_r); // Takes absolute value
jordiluong 18:2b6f41f39a7f 210
jordiluong 18:2b6f41f39a7f 211 if (avg_emg_r != 0)
jordiluong 18:2b6f41f39a7f 212 {
jordiluong 18:2b6f41f39a7f 213 onoffsignal_r = filteredAbs_r/avg_emg_r; // Divide the emg_r signal by the max emg_r to calibrate the signal per person
jordiluong 18:2b6f41f39a7f 214 }
jordiluong 14:95acac6a07c7 215 }
jordiluong 14:95acac6a07c7 216 }
jordiluong 14:95acac6a07c7 217
jordiluong 14:95acac6a07c7 218 // Filter EMG signal of left arm
jordiluong 18:2b6f41f39a7f 219 void filter_l()
jordiluong 18:2b6f41f39a7f 220 {
jordiluong 18:2b6f41f39a7f 221 if(check_calibration_l == 1)
jordiluong 18:2b6f41f39a7f 222 {
jordiluong 14:95acac6a07c7 223 emg_value_l = emg_l.read();
jordiluong 18:2b6f41f39a7f 224 emgFiltered_l = bqc.step(emg_value_l);
jordiluong 14:95acac6a07c7 225 filteredAbs_l = abs( emgFiltered_l );
jordiluong 18:2b6f41f39a7f 226 if (avg_emg_l != 0)
jordiluong 18:2b6f41f39a7f 227 {
jordiluong 18:2b6f41f39a7f 228 onoffsignal_l = filteredAbs_l/avg_emg_l;
jordiluong 18:2b6f41f39a7f 229 }
jordiluong 10:a9e344e440b8 230 }
jordiluong 14:95acac6a07c7 231 }
jordiluong 14:95acac6a07c7 232
jordiluong 14:95acac6a07c7 233 // Check threshold right arm
jordiluong 18:2b6f41f39a7f 234 void check_emg_r()
jordiluong 18:2b6f41f39a7f 235 {
jordiluong 18:2b6f41f39a7f 236 double filteredAbs_temp_r;
jordiluong 14:95acac6a07c7 237
jordiluong 20:ab391a133a01 238 if((check_calibration_l == 1) && (check_calibration_r == 1)) // Check if EMG is calibrated
jordiluong 18:2b6f41f39a7f 239 {
jordiluong 20:ab391a133a01 240 for(int i = 0; i<250; i++) // Take samples of EMG
jordiluong 18:2b6f41f39a7f 241 {
jordiluong 20:ab391a133a01 242 filter_r(); // Filter signal
jordiluong 18:2b6f41f39a7f 243 filteredAbs_temp_r = filteredAbs_temp_r + onoffsignal_r;
jordiluong 18:2b6f41f39a7f 244 wait(0.0004);
jordiluong 18:2b6f41f39a7f 245 }
jordiluong 20:ab391a133a01 246 filteredAbs_temp_r = filteredAbs_temp_r/250; // Take mean of signal
jordiluong 20:ab391a133a01 247 if(filteredAbs_temp_r <= 0.3) // If signal is lower than threshold, arm is not active
jordiluong 18:2b6f41f39a7f 248 {
jordiluong 20:ab391a133a01 249 led1.write(1);
jordiluong 18:2b6f41f39a7f 250 active_r = false;
jordiluong 18:2b6f41f39a7f 251 }
jordiluong 20:ab391a133a01 252 else if(filteredAbs_temp_r > 0.3) // If signal is higher than threshold, arm is active
jordiluong 18:2b6f41f39a7f 253 {
jordiluong 18:2b6f41f39a7f 254 led1.write(0);
jordiluong 18:2b6f41f39a7f 255 active_r = true;
jordiluong 18:2b6f41f39a7f 256 }
jordiluong 10:a9e344e440b8 257 }
jordiluong 14:95acac6a07c7 258 }
jordiluong 18:2b6f41f39a7f 259
jordiluong 18:2b6f41f39a7f 260 // Check threshold left arm
jordiluong 18:2b6f41f39a7f 261 void check_emg_l()
jordiluong 18:2b6f41f39a7f 262 {
jordiluong 18:2b6f41f39a7f 263 double filteredAbs_temp_l;
jordiluong 14:95acac6a07c7 264
jordiluong 18:2b6f41f39a7f 265 if((check_calibration_l == 1) && (check_calibration_r == 1))
jordiluong 18:2b6f41f39a7f 266 {
jordiluong 18:2b6f41f39a7f 267 for( int i = 0; i<250; i++)
jordiluong 18:2b6f41f39a7f 268 {
jordiluong 18:2b6f41f39a7f 269 filter_l();
jordiluong 18:2b6f41f39a7f 270 filteredAbs_temp_l = filteredAbs_temp_l + onoffsignal_l;
jordiluong 18:2b6f41f39a7f 271 wait(0.0004);
jordiluong 18:2b6f41f39a7f 272 }
jordiluong 18:2b6f41f39a7f 273 filteredAbs_temp_l = filteredAbs_temp_l/250;
jordiluong 20:ab391a133a01 274 if(filteredAbs_temp_l <= 0.3)
jordiluong 18:2b6f41f39a7f 275 {
jordiluong 18:2b6f41f39a7f 276 led2.write(1);
jordiluong 18:2b6f41f39a7f 277 active_l = false;
jordiluong 18:2b6f41f39a7f 278 }
jordiluong 20:ab391a133a01 279 else if(filteredAbs_temp_l > 0.3)
jordiluong 18:2b6f41f39a7f 280 {
jordiluong 18:2b6f41f39a7f 281 led2.write(0);
jordiluong 18:2b6f41f39a7f 282 active_l = true;
jordiluong 18:2b6f41f39a7f 283 }
jordiluong 14:95acac6a07c7 284 }
jordiluong 10:a9e344e440b8 285 }
jordiluong 13:ec227b229f3d 286
jordiluong 14:95acac6a07c7 287 // Calibrate right arm
jordiluong 18:2b6f41f39a7f 288 void calibration_r()
jordiluong 18:2b6f41f39a7f 289 {
jordiluong 18:2b6f41f39a7f 290 led3.write(0);
jordiluong 14:95acac6a07c7 291
jordiluong 18:2b6f41f39a7f 292 double signal_collection_r = 0;
jordiluong 20:ab391a133a01 293 for(int n =0; n < 5000; n++) // Take samples of EMG signal
jordiluong 18:2b6f41f39a7f 294 {
jordiluong 20:ab391a133a01 295 emg_value_r = emg_r.read(); // Read EMG signal
jordiluong 20:ab391a133a01 296 emgFiltered_r = bqc.step(emg_value_r); // Filter signal
jordiluong 20:ab391a133a01 297 filteredAbs_r = abs(emgFiltered_r); // Take absolute value
jordiluong 18:2b6f41f39a7f 298 signal_collection_r = signal_collection_r + filteredAbs_r ;
jordiluong 18:2b6f41f39a7f 299 wait(0.0004);
jordiluong 14:95acac6a07c7 300
jordiluong 18:2b6f41f39a7f 301 if (n == 4999)
jordiluong 18:2b6f41f39a7f 302 {
jordiluong 20:ab391a133a01 303 avg_emg_r = signal_collection_r / n; // Take mean value
jordiluong 18:2b6f41f39a7f 304 }
jordiluong 18:2b6f41f39a7f 305 }
jordiluong 18:2b6f41f39a7f 306 led3.write(1);
jordiluong 20:ab391a133a01 307 check_calibration_r = 1; // Calibration of right arm is done
jordiluong 14:95acac6a07c7 308 }
jordiluong 14:95acac6a07c7 309
jordiluong 14:95acac6a07c7 310 // Calibrate left arm
jordiluong 18:2b6f41f39a7f 311 void calibration_l()
jordiluong 18:2b6f41f39a7f 312 {
jordiluong 18:2b6f41f39a7f 313 led3.write(0);
jordiluong 14:95acac6a07c7 314
jordiluong 18:2b6f41f39a7f 315 double signal_collection_l = 0;
jordiluong 20:ab391a133a01 316 for(int n = 0; n < 5000; n++)
jordiluong 18:2b6f41f39a7f 317 {
jordiluong 18:2b6f41f39a7f 318 emg_value_l = emg_l.read();
jordiluong 18:2b6f41f39a7f 319 emgFiltered_l = bqc.step(emg_value_l);
jordiluong 18:2b6f41f39a7f 320 filteredAbs_l = abs(emgFiltered_l);
jordiluong 18:2b6f41f39a7f 321 signal_collection_l = signal_collection_l + filteredAbs_l ;
jordiluong 18:2b6f41f39a7f 322 wait(0.0004);
jordiluong 14:95acac6a07c7 323
jordiluong 18:2b6f41f39a7f 324 if (n == 4999)
jordiluong 18:2b6f41f39a7f 325 {
jordiluong 18:2b6f41f39a7f 326 avg_emg_l = signal_collection_l / n;
jordiluong 18:2b6f41f39a7f 327 }
jordiluong 18:2b6f41f39a7f 328 }
jordiluong 18:2b6f41f39a7f 329 led3.write(1);
jordiluong 18:2b6f41f39a7f 330 wait(1);
jordiluong 18:2b6f41f39a7f 331 check_calibration_l = 1;
jordiluong 14:95acac6a07c7 332 }
jordiluong 14:95acac6a07c7 333
jordiluong 14:95acac6a07c7 334 // DETERMINE JOINT VELOCITIES //////////////////////////////////////////////////
jordiluong 14:95acac6a07c7 335 void JointVelocities()
jordiluong 10:a9e344e440b8 336 {
jordiluong 10:a9e344e440b8 337 if(currentState == MOVING)
jordiluong 10:a9e344e440b8 338 {
jordiluong 19:6f720e4fcb47 339 position1 = radPerPulse * Encoder1.getPulses();
jordiluong 19:6f720e4fcb47 340 position2 = radPerPulse * Encoder2.getPulses();
jordiluong 14:95acac6a07c7 341
jordiluong 18:2b6f41f39a7f 342 if(active_l && active_r) // If both left and right EMG are active for 1 second the direction of control changes
jordiluong 16:2cf8c2705936 343 {
jordiluong 16:2cf8c2705936 344 count += 1;
jordiluong 18:2b6f41f39a7f 345 if(count == 20)
jordiluong 16:2cf8c2705936 346 {
jordiluong 16:2cf8c2705936 347 active_l = false;
jordiluong 16:2cf8c2705936 348 active_r = false;
jordiluong 16:2cf8c2705936 349 xdir = !xdir;
jordiluong 16:2cf8c2705936 350 ydir = !ydir;
jordiluong 16:2cf8c2705936 351 led4 = !led4;
jordiluong 16:2cf8c2705936 352 led5 = !led5;
jordiluong 16:2cf8c2705936 353 xVelocity = 0;
jordiluong 16:2cf8c2705936 354 yVelocity = 0;
jordiluong 16:2cf8c2705936 355 }
jordiluong 16:2cf8c2705936 356 }
jordiluong 16:2cf8c2705936 357 else count = 0;
jordiluong 16:2cf8c2705936 358
jordiluong 18:2b6f41f39a7f 359 if(xdir) // Control in x-direction
jordiluong 14:95acac6a07c7 360 {
jordiluong 18:2b6f41f39a7f 361 if(active_r && count == 0 && // Checks whether EMG is active, changing direction and max rotation of motors
jordiluong 18:2b6f41f39a7f 362 reference1 > motor1Min && reference2 < motor2Max)
jordiluong 15:5d24f832bb7b 363 {
jordiluong 18:2b6f41f39a7f 364 xVelocity = velocity; // Give velocity to end effector
jordiluong 15:5d24f832bb7b 365 }
jordiluong 18:2b6f41f39a7f 366 else if(active_l && count == 0 &&
jordiluong 18:2b6f41f39a7f 367 reference1 < motor1Max && reference2 > motor2Min)
jordiluong 15:5d24f832bb7b 368 {
jordiluong 15:5d24f832bb7b 369 xVelocity = -velocity;
jordiluong 15:5d24f832bb7b 370 }
jordiluong 15:5d24f832bb7b 371 else xVelocity = 0;
jordiluong 14:95acac6a07c7 372 }
jordiluong 18:2b6f41f39a7f 373 else if(ydir) // Control in y-direction
jordiluong 14:95acac6a07c7 374 {
jordiluong 20:ab391a133a01 375 if(active_r && count == 0 && reference2 < motor2Max )
jordiluong 15:5d24f832bb7b 376 {
jordiluong 15:5d24f832bb7b 377 yVelocity = velocity;
jordiluong 15:5d24f832bb7b 378 }
jordiluong 20:ab391a133a01 379 else if(active_l && count == 0 && reference2 > motor2Min )
jordiluong 15:5d24f832bb7b 380 {
jordiluong 15:5d24f832bb7b 381 yVelocity = -velocity;
jordiluong 15:5d24f832bb7b 382 }
jordiluong 15:5d24f832bb7b 383 else yVelocity = 0;
jordiluong 14:95acac6a07c7 384 }
jordiluong 14:95acac6a07c7 385
jordiluong 14:95acac6a07c7 386 // Construct Jacobian
jordiluong 14:95acac6a07c7 387 double q[4];
jordiluong 23:a87fd4719655 388 q[0] = position1, q[1] = -position1;
jordiluong 14:95acac6a07c7 389 q[2] = position2, q[3] = -position2;
jordiluong 14:95acac6a07c7 390
jordiluong 18:2b6f41f39a7f 391 double T2[3]; // Second column of the jacobian
jordiluong 18:2b6f41f39a7f 392 double T3[3]; // Third column of the jacobian
jordiluong 18:2b6f41f39a7f 393 double T4[3]; // Fourth column of the jacobian
jordiluong 14:95acac6a07c7 394 double T1[6];
jordiluong 14:95acac6a07c7 395 static const signed char b_T1[3] = { 1, 0, 0 };
jordiluong 14:95acac6a07c7 396 double J_data[6];
jordiluong 14:95acac6a07c7 397
jordiluong 14:95acac6a07c7 398 T2[0] = 1.0;
jordiluong 14:95acac6a07c7 399 T2[1] = 0.365 * cos(q[0]);
jordiluong 14:95acac6a07c7 400 T2[2] = 0.365 * sin(q[0]);
jordiluong 14:95acac6a07c7 401 T3[0] = 1.0;
jordiluong 18:2b6f41f39a7f 402 T3[1] = 0.365 * cos(q[0]) + 0.2353720459187964 *
jordiluong 18:2b6f41f39a7f 403 sin((0.21406068356382149 + q[0]) + q[1]);
jordiluong 18:2b6f41f39a7f 404 T3[2] = 0.365 * sin(q[0]) - 0.2353720459187964 *
jordiluong 18:2b6f41f39a7f 405 cos((0.21406068356382149 + q[0]) + q[1]);
jordiluong 14:95acac6a07c7 406 T4[0] = 1.0;
jordiluong 18:2b6f41f39a7f 407 T4[1] = (0.365 * cos(q[0]) + 0.2353720459187964 *
jordiluong 18:2b6f41f39a7f 408 sin((0.21406068356382149 + q[0]) + q[1])) +
jordiluong 18:2b6f41f39a7f 409 0.265 * sin((q[0] + q[1]) + q[2]);
jordiluong 18:2b6f41f39a7f 410 T4[2] = (0.365 * sin(q[0]) - 0.2353720459187964 *
jordiluong 18:2b6f41f39a7f 411 cos((0.21406068356382149 + q[0]) + q[1])) - 0.265 *
jordiluong 18:2b6f41f39a7f 412 cos((q[0] + q[1]) + q[2]);
jordiluong 14:95acac6a07c7 413
jordiluong 14:95acac6a07c7 414 for (int i = 0; i < 3; i++)
jordiluong 14:95acac6a07c7 415 {
jordiluong 14:95acac6a07c7 416 T1[i] = (double)b_T1[i] - T2[i];
jordiluong 14:95acac6a07c7 417 T1[3 + i] = T3[i] - T4[i];
jordiluong 14:95acac6a07c7 418 }
jordiluong 14:95acac6a07c7 419
jordiluong 14:95acac6a07c7 420 for (int i = 0; i < 2; i++)
jordiluong 14:95acac6a07c7 421 {
jordiluong 14:95acac6a07c7 422 for (int j = 0; j < 3; j++)
jordiluong 14:95acac6a07c7 423 {
jordiluong 14:95acac6a07c7 424 J_data[j + 3 * i] = T1[j + 3 * i];
jordiluong 14:95acac6a07c7 425 }
jordiluong 14:95acac6a07c7 426 }
jordiluong 14:95acac6a07c7 427
jordiluong 14:95acac6a07c7 428 // Here the first row of the Jacobian is cut off, so we do not take rotation into consideration
jordiluong 14:95acac6a07c7 429 // Note: the matrices from now on will we constructed rowwise
jordiluong 14:95acac6a07c7 430 double Jvelocity[4];
jordiluong 14:95acac6a07c7 431 Jvelocity[0] = J_data[1];
jordiluong 14:95acac6a07c7 432 Jvelocity[1] = J_data[4];
jordiluong 14:95acac6a07c7 433 Jvelocity[2] = J_data[2];
jordiluong 14:95acac6a07c7 434 Jvelocity[3] = J_data[5];
jordiluong 14:95acac6a07c7 435
jordiluong 14:95acac6a07c7 436 // Creating the inverse Jacobian
jordiluong 18:2b6f41f39a7f 437 double Jvelocity_inv[4]; // The inverse matrix of the jacobian
jordiluong 18:2b6f41f39a7f 438 double determ = Jvelocity[0]*Jvelocity[3]-Jvelocity[1]*Jvelocity[2]; // The determinant of the matrix
jordiluong 14:95acac6a07c7 439 Jvelocity_inv[0] = Jvelocity[3]/determ;
jordiluong 14:95acac6a07c7 440 Jvelocity_inv[1] = -Jvelocity[1]/determ;
jordiluong 14:95acac6a07c7 441 Jvelocity_inv[2] = -Jvelocity[2]/determ;
jordiluong 14:95acac6a07c7 442 Jvelocity_inv[3] = Jvelocity[0]/determ;
jordiluong 14:95acac6a07c7 443
jordiluong 14:95acac6a07c7 444 // Now the velocity of the joints are found by giving the velocity of the end-effector and the inverse jacobian
jordiluong 18:2b6f41f39a7f 445 double msh[2]; // The velocity the joints have to have
jordiluong 14:95acac6a07c7 446 msh[0] = xVelocity*Jvelocity_inv[0] + yVelocity*Jvelocity_inv[1];
jordiluong 14:95acac6a07c7 447 msh[1] = xVelocity*Jvelocity_inv[2] + yVelocity*Jvelocity_inv[3];
jordiluong 14:95acac6a07c7 448
jordiluong 18:2b6f41f39a7f 449 // Determine reference position of motor 1
jordiluong 18:2b6f41f39a7f 450 if(reference1 + msh[0]*processTs > motor1Max) reference1 = motor1Max;
jordiluong 18:2b6f41f39a7f 451 else if(reference1 + msh[0]*processTs < motor1Min) reference1 = motor1Min;
jordiluong 18:2b6f41f39a7f 452 else reference1 = reference1 + msh[0]*processTs;
jordiluong 14:95acac6a07c7 453
jordiluong 18:2b6f41f39a7f 454 // Determine reference position of motor 2
jordiluong 18:2b6f41f39a7f 455 if(reference2 + msh[1]*processTs > motor2Max) reference2 = motor2Max;
jordiluong 18:2b6f41f39a7f 456 else if(reference2 + msh[1]*processTs < motor2Min) reference2 = motor2Min;
jordiluong 18:2b6f41f39a7f 457 else reference2 = reference2 + msh[1]*processTs;
jordiluong 10:a9e344e440b8 458 }
jordiluong 10:a9e344e440b8 459 }
jordiluong 13:ec227b229f3d 460
jordiluong 7:757e95b4dc46 461 // STATES //////////////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 462 void ProcessStateMachine()
jordiluong 0:80ac024b84cb 463 {
jordiluong 0:80ac024b84cb 464 switch(currentState)
jordiluong 0:80ac024b84cb 465 {
jordiluong 0:80ac024b84cb 466 case MOTORS_OFF:
jordiluong 0:80ac024b84cb 467 {
jordiluong 0:80ac024b84cb 468 // State initialization
jordiluong 0:80ac024b84cb 469 if(stateChanged)
jordiluong 0:80ac024b84cb 470 {
jordiluong 15:5d24f832bb7b 471 pc.printf("Entering MOTORS_OFF \r\n"
jordiluong 15:5d24f832bb7b 472 "Press button 1 to enter CALIBRATING \r\n");
jordiluong 5:0d3e8694726e 473 TurnMotorsOff(); // Turn motors off
jordiluong 0:80ac024b84cb 474 stateChanged = false;
jordiluong 0:80ac024b84cb 475 }
jordiluong 0:80ac024b84cb 476
jordiluong 20:ab391a133a01 477 // Calibration button
jordiluong 4:ea7689bf97e1 478 if(!button1)
jordiluong 0:80ac024b84cb 479 {
jordiluong 14:95acac6a07c7 480 currentState = CALIBRATING;
jordiluong 14:95acac6a07c7 481 stateChanged = true;
jordiluong 14:95acac6a07c7 482 break;
jordiluong 14:95acac6a07c7 483 }
jordiluong 14:95acac6a07c7 484 break;
jordiluong 14:95acac6a07c7 485 }
jordiluong 14:95acac6a07c7 486
jordiluong 14:95acac6a07c7 487 case CALIBRATING:
jordiluong 14:95acac6a07c7 488 {
jordiluong 14:95acac6a07c7 489 // State initialization
jordiluong 14:95acac6a07c7 490 if(stateChanged)
jordiluong 14:95acac6a07c7 491 {
jordiluong 15:5d24f832bb7b 492 pc.printf("Entering CALIBRATING \r\n"
jordiluong 18:2b6f41f39a7f 493 "Tighten muscles until green LED is off \r\n");
jordiluong 14:95acac6a07c7 494 stateChanged = false;
jordiluong 18:2b6f41f39a7f 495 calibration_r(); // Calibrate right arm
jordiluong 18:2b6f41f39a7f 496 calibration_l(); // Calibrate left arm
jordiluong 14:95acac6a07c7 497 currentState = MOVING;
jordiluong 14:95acac6a07c7 498 stateChanged = true;
jordiluong 14:95acac6a07c7 499 }
jordiluong 4:ea7689bf97e1 500 break;
jordiluong 0:80ac024b84cb 501 }
jordiluong 0:80ac024b84cb 502
jordiluong 0:80ac024b84cb 503 case MOVING:
jordiluong 0:80ac024b84cb 504 {
jordiluong 0:80ac024b84cb 505 // State initialization
jordiluong 0:80ac024b84cb 506 if(stateChanged)
jordiluong 0:80ac024b84cb 507 {
jordiluong 15:5d24f832bb7b 508 pc.printf("Entering MOVING \r\n");
jordiluong 0:80ac024b84cb 509 stateChanged = false;
jordiluong 0:80ac024b84cb 510 }
jordiluong 4:ea7689bf97e1 511 break;
jordiluong 0:80ac024b84cb 512 }
jordiluong 0:80ac024b84cb 513
jordiluong 0:80ac024b84cb 514 default:
jordiluong 0:80ac024b84cb 515 {
jordiluong 5:0d3e8694726e 516 TurnMotorsOff(); // Turn motors off for safety
jordiluong 4:ea7689bf97e1 517 break;
jordiluong 0:80ac024b84cb 518 }
jordiluong 0:80ac024b84cb 519 }
jordiluong 0:80ac024b84cb 520 }
jordiluong 13:ec227b229f3d 521
jordiluong 7:757e95b4dc46 522 // MAIN FUNCTION ///////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 523 int main()
jordiluong 0:80ac024b84cb 524 {
jordiluong 0:80ac024b84cb 525 // Serial communication
jordiluong 0:80ac024b84cb 526 pc.baud(115200);
jordiluong 0:80ac024b84cb 527
jordiluong 18:2b6f41f39a7f 528 // Start values of LEDs
jordiluong 14:95acac6a07c7 529 led1.write(1);
jordiluong 14:95acac6a07c7 530 led2.write(1);
jordiluong 14:95acac6a07c7 531 led3.write(1);
jordiluong 15:5d24f832bb7b 532 led4.write(1);
jordiluong 15:5d24f832bb7b 533 led5.write(0);
jordiluong 15:5d24f832bb7b 534
jordiluong 18:2b6f41f39a7f 535 bqc.add(&bq1_low ).add(&bq2_high ).add(&bq3_notch ); // Make BiQuad Chain
jordiluong 7:757e95b4dc46 536
jordiluong 18:2b6f41f39a7f 537 processTicker.attach(&JointVelocities, processTs); // Ticker to process EMG
jordiluong 20:ab391a133a01 538 controllerTicker.attach(&MotorController, controllerTs); // Ticker to control motors
jordiluong 18:2b6f41f39a7f 539 emgRight.attach(&check_emg_r, emgTs); // Ticker to sample EMG of right arm
jordiluong 18:2b6f41f39a7f 540 emgLeft.attach(&check_emg_l, emgTs); // Ticker to sample EMG of left arm
jordiluong 12:12b72bd60fd1 541
jordiluong 18:2b6f41f39a7f 542 motor1MagnitudePin.period_ms(1); // PWM frequency of motor 1 (Should actually be 5 - 10 kHz)
jordiluong 18:2b6f41f39a7f 543 motor2MagnitudePin.period_ms(1); // PWM frequency of motor 2 (Should actually be 5 - 10 kHz)
jordiluong 4:ea7689bf97e1 544
jordiluong 0:80ac024b84cb 545 while(true)
jordiluong 0:80ac024b84cb 546 {
jordiluong 0:80ac024b84cb 547 ProcessStateMachine(); // Execute states function
jordiluong 0:80ac024b84cb 548 }
jordiluong 0:80ac024b84cb 549 }