Jordi Luong / Mbed 2 deprecated Project_BioRobotics

Project BioRobotics Group 19

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@21:2e732eb85daf, 2017-11-03 (annotated)

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