Jordi Luong / Mbed 2 deprecated Project_BioRobotics

Project BioRobotics Group 19

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@18:2b6f41f39a7f, 2017-11-02 (annotated)

Committer:
jordiluong
Date:
Thu Nov 02 16:23:26 2017 +0000
Revision:
18:2b6f41f39a7f
Parent:
17:f8dd5b8e4b52
Child:
19:6f720e4fcb47
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 12:12b72bd60fd1 13 HIDScope scope(4);
jordiluong 13:ec227b229f3d 14
jordiluong 7:757e95b4dc46 15 // STATES //////////////////////////////////////////////////////////////////////
jordiluong 18:2b6f41f39a7f 16 enum states{MOTORS_OFF, CALIBRATING, MOVING};
jordiluong 0:80ac024b84cb 17 states currentState = MOTORS_OFF; // Start with motors off
jordiluong 0:80ac024b84cb 18 bool stateChanged = true; // Make sure the initialization of first state is executed
jordiluong 13:ec227b229f3d 19
jordiluong 7:757e95b4dc46 20 // ENCODER /////////////////////////////////////////////////////////////////////
jordiluong 18:2b6f41f39a7f 21 QEI Encoder1(D10,D11,NC,32); // CONNECT ENC1A TO D12, ENC1B TO D13
jordiluong 18:2b6f41f39a7f 22 QEI Encoder2(D12,D13,NC,32); // CONNECT ENC2A TO D10, ENC2B TO D11
jordiluong 13:ec227b229f3d 23
jordiluong 7:757e95b4dc46 24 // PINS ////////////////////////////////////////////////////////////////////////
jordiluong 4:ea7689bf97e1 25 DigitalOut motor1DirectionPin(D4); // Value 0: CCW; 1: CW
jordiluong 3:5c3edcd29448 26 PwmOut motor1MagnitudePin(D5);
jordiluong 18:2b6f41f39a7f 27 DigitalOut motor2DirectionPin(D7); // Value 0: CW; 1: CCW
jordiluong 3:5c3edcd29448 28 PwmOut motor2MagnitudePin(D6);
jordiluong 4:ea7689bf97e1 29 InterruptIn button1(D2); // CONNECT BUT1 TO D2
jordiluong 4:ea7689bf97e1 30 InterruptIn button2(D3); // CONNECT BUT2 TO D3
jordiluong 14:95acac6a07c7 31 InterruptIn button3(SW2);
jordiluong 10:a9e344e440b8 32 InterruptIn button4(SW3);
jordiluong 14:95acac6a07c7 33 AnalogIn potmeter1(A0); // CONNECT POT1 TO A0
jordiluong 14:95acac6a07c7 34 AnalogIn potmeter2(A1); // CONNECT POT2 TO A1
jordiluong 14:95acac6a07c7 35 DigitalOut led1(LED_RED);
jordiluong 14:95acac6a07c7 36 DigitalOut led2(LED_BLUE);
jordiluong 14:95acac6a07c7 37 DigitalOut led3(LED_GREEN);
jordiluong 15:5d24f832bb7b 38 DigitalOut led4(D8); // CONNECT LED1 TO D8
jordiluong 15:5d24f832bb7b 39 DigitalOut led5(D9); // CONNECT LED2 TO D9
jordiluong 14:95acac6a07c7 40 AnalogIn emg_r(A2); // CONNECT EMG TO A2
jordiluong 14:95acac6a07c7 41 AnalogIn emg_l(A3); // CONNECT EMG TO A3
jordiluong 13:ec227b229f3d 42
jordiluong 7:757e95b4dc46 43 // MOTOR CONTROL ///////////////////////////////////////////////////////////////
jordiluong 14:95acac6a07c7 44 Ticker controllerTicker; // Ticker for the controller
jordiluong 18:2b6f41f39a7f 45 const double controllerTs = 1/200.1; // Time step for controllerTicker [s]
jordiluong 5:0d3e8694726e 46 const double motorRatio = 131.25; // Ratio of the gearbox in the motors []
jordiluong 7:757e95b4dc46 47 const double radPerPulse = 2*pi/(32*motorRatio); // Amount of radians the motor rotates per encoder pulse [rad/pulse]
jordiluong 14:95acac6a07c7 48 volatile double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start
jordiluong 16:2cf8c2705936 49 const double velocity = 0.03; // X and Y velocity of the end effector when desired
jordiluong 13:ec227b229f3d 50
jordiluong 7:757e95b4dc46 51 // MOTOR 1
jordiluong 14:95acac6a07c7 52 volatile double position1; // Position of motor 1 [rad]
jordiluong 18:2b6f41f39a7f 53 volatile double reference1 = 2*pi*-5/360; // Desired rotation of motor 1 [rad]
jordiluong 16:2cf8c2705936 54 const double motor1Max = 0; // Maximum rotation of motor 1 [rad]
jordiluong 16:2cf8c2705936 55 const double motor1Min = 2*pi*-40/360; // Minimum rotation of motor 1 [rad]
jordiluong 5:0d3e8694726e 56 // Controller gains
jordiluong 14:95acac6a07c7 57 const double motor1_KP = 13; // Position gain []
jordiluong 13:ec227b229f3d 58 const double motor1_KI = 7; // Integral gain []
jordiluong 13:ec227b229f3d 59 const double motor1_KD = 0.3; // Derivative gain []
jordiluong 5:0d3e8694726e 60 double motor1_err_int = 0, motor1_prev_err = 0;
jordiluong 5:0d3e8694726e 61 // Derivative filter coefficients
jordiluong 14:95acac6a07c7 62 const double motor1_f_a1 = 0.25, motor1_f_a2 = 0.8; // Derivative filter coefficients []
jordiluong 14:95acac6a07c7 63 const double motor1_f_b0 = 1, motor1_f_b1 = 2, motor1_f_b2 = 0.8; // Derivative filter coefficients []
jordiluong 5:0d3e8694726e 64 // Filter variables
jordiluong 5:0d3e8694726e 65 double motor1_f_v1 = 0, motor1_f_v2 = 0;
jordiluong 13:ec227b229f3d 66
jordiluong 7:757e95b4dc46 67 // MOTOR 2
jordiluong 14:95acac6a07c7 68 volatile double position2; // Position of motor 2 [rad]
jordiluong 14:95acac6a07c7 69 volatile double reference2 = 0; // Desired rotation of motor 2 [rad]
jordiluong 16:2cf8c2705936 70 const double motor2Max = 2*pi*25/360; // Maximum rotation of motor 2 [rad]
jordiluong 16:2cf8c2705936 71 const double motor2Min = 2*pi*-28/360; // Minimum rotation of motor 2 [rad]
jordiluong 5:0d3e8694726e 72 // Controller gains
jordiluong 14:95acac6a07c7 73 const double motor2_KP = 13; // Position gain []
jordiluong 13:ec227b229f3d 74 const double motor2_KI = 5; // Integral gain []
jordiluong 13:ec227b229f3d 75 const double motor2_KD = 0.1; // Derivative gain []
jordiluong 10:a9e344e440b8 76 double motor2_err_int = 0, motor2_prev_err = 0;
jordiluong 5:0d3e8694726e 77 // Derivative filter coefficients
jordiluong 14:95acac6a07c7 78 const double motor2_f_a1 = 0.25, motor2_f_a2 = 0.8; // Derivative filter coefficients []
jordiluong 14:95acac6a07c7 79 const double motor2_f_b0 = 1, motor2_f_b1 = 2, motor2_f_b2 = 0.8; // Derivative filter coefficients []
jordiluong 5:0d3e8694726e 80 // Filter variables
jordiluong 10:a9e344e440b8 81 double motor2_f_v1 = 0, motor2_f_v2 = 0;
jordiluong 13:ec227b229f3d 82
jordiluong 18:2b6f41f39a7f 83 // EMG /////////////////////////////////////////////////////////////////////////
jordiluong 18:2b6f41f39a7f 84 Ticker emgLeft; // Ticker for EMG of left arm
jordiluong 18:2b6f41f39a7f 85 Ticker emgRight; // Ticker for EMG of right arm
jordiluong 18:2b6f41f39a7f 86 const double emgTs = 0.491; // Time step for EMG sampling
jordiluong 14:95acac6a07c7 87 // Filters
jordiluong 18:2b6f41f39a7f 88 BiQuadChain bqc;
jordiluong 18:2b6f41f39a7f 89 BiQuad bq2_high(0.875182, -1.750364, 0.87518, -1.73472, 0.766004); // High pass filter
jordiluong 18:2b6f41f39a7f 90 BiQuad bq3_notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780); // Notch filter
jordiluong 18:2b6f41f39a7f 91 BiQuad bq1_low(3.65747e-2, 7.31495e-2, 3.65747e-2, -1.390892, 0.537191); // Low pass filter
jordiluong 14:95acac6a07c7 92 // Right arm
jordiluong 14:95acac6a07c7 93 volatile double emgFiltered_r;
jordiluong 14:95acac6a07c7 94 volatile double filteredAbs_r;
jordiluong 14:95acac6a07c7 95 volatile double emg_value_r;
jordiluong 14:95acac6a07c7 96 volatile double onoffsignal_r;
jordiluong 18:2b6f41f39a7f 97 volatile bool check_calibration_r = false;
jordiluong 14:95acac6a07c7 98 volatile double avg_emg_r;
jordiluong 14:95acac6a07c7 99 volatile bool active_r = false;
jordiluong 14:95acac6a07c7 100 // Left arm
jordiluong 14:95acac6a07c7 101 volatile double emgFiltered_l;
jordiluong 14:95acac6a07c7 102 volatile double filteredAbs_l;
jordiluong 14:95acac6a07c7 103 volatile double emg_value_l;
jordiluong 14:95acac6a07c7 104 volatile double onoffsignal_l;
jordiluong 18:2b6f41f39a7f 105 volatile bool check_calibration_l = false;
jordiluong 14:95acac6a07c7 106 volatile double avg_emg_l;
jordiluong 14:95acac6a07c7 107 volatile bool active_l = false;
jordiluong 15:5d24f832bb7b 108
jordiluong 18:2b6f41f39a7f 109 // PROCESS EMG SIGNALS /////////////////////////////////////////////////////////
jordiluong 18:2b6f41f39a7f 110 Ticker processTicker; // Ticker for processing of EMG
jordiluong 18:2b6f41f39a7f 111 const double processTs = 0.1; // Time step for processing of EMG
jordiluong 18:2b6f41f39a7f 112
jordiluong 18:2b6f41f39a7f 113 volatile bool xdir = true, ydir = false; // Direction the EMG signal moves the end effector
jordiluong 18:2b6f41f39a7f 114 volatile int count = 0; // Counter to change direction
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 18:2b6f41f39a7f 238 if((check_calibration_l == 1) && (check_calibration_r == 1))
jordiluong 18:2b6f41f39a7f 239 {
jordiluong 18:2b6f41f39a7f 240 for(int i = 0; i<250; i++)
jordiluong 18:2b6f41f39a7f 241 {
jordiluong 18:2b6f41f39a7f 242 filter_r();
jordiluong 18:2b6f41f39a7f 243 filteredAbs_temp_r = filteredAbs_temp_r + onoffsignal_r;
jordiluong 18:2b6f41f39a7f 244 wait(0.0004);
jordiluong 18:2b6f41f39a7f 245 }
jordiluong 18:2b6f41f39a7f 246 filteredAbs_temp_r = filteredAbs_temp_r/250;
jordiluong 18:2b6f41f39a7f 247 if(filteredAbs_temp_r<=0.3) //if signal is lower then 0.5 the blue light goes on
jordiluong 18:2b6f41f39a7f 248 {
jordiluong 18:2b6f41f39a7f 249 led1.write(1); //led 1 is rood en uit
jordiluong 18:2b6f41f39a7f 250 active_r = false;
jordiluong 18:2b6f41f39a7f 251 }
jordiluong 18:2b6f41f39a7f 252 else if(filteredAbs_temp_r > 0.3) //if signal does not pass threshold value, blue light goes on
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 18:2b6f41f39a7f 274 if(filteredAbs_temp_l <= 0.3) //if signal is lower then 0.5 the blue light goes on
jordiluong 18:2b6f41f39a7f 275 {
jordiluong 18:2b6f41f39a7f 276 led2.write(1);
jordiluong 18:2b6f41f39a7f 277 active_l = false;
jordiluong 18:2b6f41f39a7f 278 }
jordiluong 18:2b6f41f39a7f 279 else if(filteredAbs_temp_l > 0.3) //if signal does not pass threshold value, blue light goes on
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 18:2b6f41f39a7f 293 for(int n =0; n < 5000; n++) //read for 5000 samples as calibration
jordiluong 18:2b6f41f39a7f 294 {
jordiluong 18:2b6f41f39a7f 295 filter_r();
jordiluong 18:2b6f41f39a7f 296 emg_value_r = emg_r.read();
jordiluong 18:2b6f41f39a7f 297 emgFiltered_r = bqc.step( emg_value_r );
jordiluong 18:2b6f41f39a7f 298 filteredAbs_r = abs(emgFiltered_r);
jordiluong 14:95acac6a07c7 299
jordiluong 18:2b6f41f39a7f 300 signal_collection_r = signal_collection_r + filteredAbs_r ;
jordiluong 18:2b6f41f39a7f 301 wait(0.0004);
jordiluong 14:95acac6a07c7 302
jordiluong 18:2b6f41f39a7f 303 if (n == 4999)
jordiluong 18:2b6f41f39a7f 304 {
jordiluong 18:2b6f41f39a7f 305 avg_emg_r = signal_collection_r / n;
jordiluong 18:2b6f41f39a7f 306 }
jordiluong 18:2b6f41f39a7f 307 }
jordiluong 18:2b6f41f39a7f 308 led3.write(1);
jordiluong 18:2b6f41f39a7f 309 check_calibration_r = 1;
jordiluong 14:95acac6a07c7 310 }
jordiluong 14:95acac6a07c7 311
jordiluong 14:95acac6a07c7 312 // Calibrate left arm
jordiluong 18:2b6f41f39a7f 313 void calibration_l()
jordiluong 18:2b6f41f39a7f 314 {
jordiluong 18:2b6f41f39a7f 315 led3.write(0);
jordiluong 14:95acac6a07c7 316
jordiluong 18:2b6f41f39a7f 317 double signal_collection_l = 0;
jordiluong 18:2b6f41f39a7f 318 for(int n = 0; n < 5000; n++) //read for 5000 samples as calibration
jordiluong 18:2b6f41f39a7f 319 {
jordiluong 18:2b6f41f39a7f 320 filter_l();
jordiluong 18:2b6f41f39a7f 321 emg_value_l = emg_l.read();
jordiluong 18:2b6f41f39a7f 322 emgFiltered_l = bqc.step(emg_value_l);
jordiluong 18:2b6f41f39a7f 323 filteredAbs_l = abs(emgFiltered_l);
jordiluong 18:2b6f41f39a7f 324 signal_collection_l = signal_collection_l + filteredAbs_l ;
jordiluong 18:2b6f41f39a7f 325 wait(0.0004);
jordiluong 14:95acac6a07c7 326
jordiluong 18:2b6f41f39a7f 327 if (n == 4999)
jordiluong 18:2b6f41f39a7f 328 {
jordiluong 18:2b6f41f39a7f 329 avg_emg_l = signal_collection_l / n;
jordiluong 18:2b6f41f39a7f 330 }
jordiluong 18:2b6f41f39a7f 331 }
jordiluong 18:2b6f41f39a7f 332 led3.write(1);
jordiluong 18:2b6f41f39a7f 333 wait(1);
jordiluong 18:2b6f41f39a7f 334 check_calibration_l = 1;
jordiluong 14:95acac6a07c7 335 }
jordiluong 14:95acac6a07c7 336
jordiluong 14:95acac6a07c7 337 // DETERMINE JOINT VELOCITIES //////////////////////////////////////////////////
jordiluong 14:95acac6a07c7 338 void JointVelocities()
jordiluong 10:a9e344e440b8 339 {
jordiluong 10:a9e344e440b8 340 if(currentState == MOVING)
jordiluong 10:a9e344e440b8 341 {
jordiluong 18:2b6f41f39a7f 342 //position1 = radPerPulse * Encoder1.getPulses();
jordiluong 18:2b6f41f39a7f 343 //position2 = radPerPulse * Encoder2.getPulses();
jordiluong 14:95acac6a07c7 344
jordiluong 18:2b6f41f39a7f 345 if(active_l && active_r) // If both left and right EMG are active for 1 second the direction of control changes
jordiluong 16:2cf8c2705936 346 {
jordiluong 16:2cf8c2705936 347 count += 1;
jordiluong 18:2b6f41f39a7f 348 if(count == 20)
jordiluong 16:2cf8c2705936 349 {
jordiluong 16:2cf8c2705936 350 active_l = false;
jordiluong 16:2cf8c2705936 351 active_r = false;
jordiluong 16:2cf8c2705936 352 xdir = !xdir;
jordiluong 16:2cf8c2705936 353 ydir = !ydir;
jordiluong 16:2cf8c2705936 354 led4 = !led4;
jordiluong 16:2cf8c2705936 355 led5 = !led5;
jordiluong 16:2cf8c2705936 356 xVelocity = 0;
jordiluong 16:2cf8c2705936 357 yVelocity = 0;
jordiluong 16:2cf8c2705936 358 }
jordiluong 16:2cf8c2705936 359 }
jordiluong 16:2cf8c2705936 360 else count = 0;
jordiluong 16:2cf8c2705936 361
jordiluong 18:2b6f41f39a7f 362 if(xdir) // Control in x-direction
jordiluong 14:95acac6a07c7 363 {
jordiluong 18:2b6f41f39a7f 364 if(active_r && count == 0 && // Checks whether EMG is active, changing direction and max rotation of motors
jordiluong 18:2b6f41f39a7f 365 reference1 > motor1Min && reference2 < motor2Max)
jordiluong 15:5d24f832bb7b 366 {
jordiluong 18:2b6f41f39a7f 367 xVelocity = velocity; // Give velocity to end effector
jordiluong 15:5d24f832bb7b 368 }
jordiluong 18:2b6f41f39a7f 369 else if(active_l && count == 0 &&
jordiluong 18:2b6f41f39a7f 370 reference1 < motor1Max && reference2 > motor2Min)
jordiluong 15:5d24f832bb7b 371 {
jordiluong 15:5d24f832bb7b 372 xVelocity = -velocity;
jordiluong 15:5d24f832bb7b 373 }
jordiluong 15:5d24f832bb7b 374 else xVelocity = 0;
jordiluong 14:95acac6a07c7 375 }
jordiluong 18:2b6f41f39a7f 376 else if(ydir) // Control in y-direction
jordiluong 14:95acac6a07c7 377 {
jordiluong 18:2b6f41f39a7f 378 if(active_r && count == 0 &&
jordiluong 18:2b6f41f39a7f 379 reference2 < motor2Max ) //&& reference1 > motor2Min)
jordiluong 15:5d24f832bb7b 380 {
jordiluong 15:5d24f832bb7b 381 yVelocity = velocity;
jordiluong 15:5d24f832bb7b 382 }
jordiluong 18:2b6f41f39a7f 383 else if(active_l && count == 0
jordiluong 18:2b6f41f39a7f 384 && reference2 > motor2Min ) //&& reference1 > motor2Min)
jordiluong 15:5d24f832bb7b 385 {
jordiluong 15:5d24f832bb7b 386 yVelocity = -velocity;
jordiluong 15:5d24f832bb7b 387 }
jordiluong 15:5d24f832bb7b 388 else yVelocity = 0;
jordiluong 14:95acac6a07c7 389 }
jordiluong 14:95acac6a07c7 390
jordiluong 14:95acac6a07c7 391 // Construct Jacobian
jordiluong 14:95acac6a07c7 392 double q[4];
jordiluong 14:95acac6a07c7 393 q[0] = position1, q[1] = -position1;
jordiluong 14:95acac6a07c7 394 q[2] = position2, q[3] = -position2;
jordiluong 14:95acac6a07c7 395
jordiluong 18:2b6f41f39a7f 396 double T2[3]; // Second column of the jacobian
jordiluong 18:2b6f41f39a7f 397 double T3[3]; // Third column of the jacobian
jordiluong 18:2b6f41f39a7f 398 double T4[3]; // Fourth column of the jacobian
jordiluong 14:95acac6a07c7 399 double T1[6];
jordiluong 14:95acac6a07c7 400 static const signed char b_T1[3] = { 1, 0, 0 };
jordiluong 14:95acac6a07c7 401 double J_data[6];
jordiluong 14:95acac6a07c7 402
jordiluong 14:95acac6a07c7 403 T2[0] = 1.0;
jordiluong 14:95acac6a07c7 404 T2[1] = 0.365 * cos(q[0]);
jordiluong 14:95acac6a07c7 405 T2[2] = 0.365 * sin(q[0]);
jordiluong 14:95acac6a07c7 406 T3[0] = 1.0;
jordiluong 18:2b6f41f39a7f 407 T3[1] = 0.365 * cos(q[0]) + 0.2353720459187964 *
jordiluong 18:2b6f41f39a7f 408 sin((0.21406068356382149 + q[0]) + q[1]);
jordiluong 18:2b6f41f39a7f 409 T3[2] = 0.365 * sin(q[0]) - 0.2353720459187964 *
jordiluong 18:2b6f41f39a7f 410 cos((0.21406068356382149 + q[0]) + q[1]);
jordiluong 14:95acac6a07c7 411 T4[0] = 1.0;
jordiluong 18:2b6f41f39a7f 412 T4[1] = (0.365 * cos(q[0]) + 0.2353720459187964 *
jordiluong 18:2b6f41f39a7f 413 sin((0.21406068356382149 + q[0]) + q[1])) +
jordiluong 18:2b6f41f39a7f 414 0.265 * sin((q[0] + q[1]) + q[2]);
jordiluong 18:2b6f41f39a7f 415 T4[2] = (0.365 * sin(q[0]) - 0.2353720459187964 *
jordiluong 18:2b6f41f39a7f 416 cos((0.21406068356382149 + q[0]) + q[1])) - 0.265 *
jordiluong 18:2b6f41f39a7f 417 cos((q[0] + q[1]) + q[2]);
jordiluong 14:95acac6a07c7 418
jordiluong 14:95acac6a07c7 419 for (int i = 0; i < 3; i++)
jordiluong 14:95acac6a07c7 420 {
jordiluong 14:95acac6a07c7 421 T1[i] = (double)b_T1[i] - T2[i];
jordiluong 14:95acac6a07c7 422 T1[3 + i] = T3[i] - T4[i];
jordiluong 14:95acac6a07c7 423 }
jordiluong 14:95acac6a07c7 424
jordiluong 14:95acac6a07c7 425 for (int i = 0; i < 2; i++)
jordiluong 14:95acac6a07c7 426 {
jordiluong 14:95acac6a07c7 427 for (int j = 0; j < 3; j++)
jordiluong 14:95acac6a07c7 428 {
jordiluong 14:95acac6a07c7 429 J_data[j + 3 * i] = T1[j + 3 * i];
jordiluong 14:95acac6a07c7 430 }
jordiluong 14:95acac6a07c7 431 }
jordiluong 14:95acac6a07c7 432
jordiluong 14:95acac6a07c7 433 // Here the first row of the Jacobian is cut off, so we do not take rotation into consideration
jordiluong 14:95acac6a07c7 434 // Note: the matrices from now on will we constructed rowwise
jordiluong 14:95acac6a07c7 435 double Jvelocity[4];
jordiluong 14:95acac6a07c7 436 Jvelocity[0] = J_data[1];
jordiluong 14:95acac6a07c7 437 Jvelocity[1] = J_data[4];
jordiluong 14:95acac6a07c7 438 Jvelocity[2] = J_data[2];
jordiluong 14:95acac6a07c7 439 Jvelocity[3] = J_data[5];
jordiluong 14:95acac6a07c7 440
jordiluong 14:95acac6a07c7 441 // Creating the inverse Jacobian
jordiluong 18:2b6f41f39a7f 442 double Jvelocity_inv[4]; // The inverse matrix of the jacobian
jordiluong 18:2b6f41f39a7f 443 double determ = Jvelocity[0]*Jvelocity[3]-Jvelocity[1]*Jvelocity[2]; // The determinant of the matrix
jordiluong 14:95acac6a07c7 444 Jvelocity_inv[0] = Jvelocity[3]/determ;
jordiluong 14:95acac6a07c7 445 Jvelocity_inv[1] = -Jvelocity[1]/determ;
jordiluong 14:95acac6a07c7 446 Jvelocity_inv[2] = -Jvelocity[2]/determ;
jordiluong 14:95acac6a07c7 447 Jvelocity_inv[3] = Jvelocity[0]/determ;
jordiluong 14:95acac6a07c7 448
jordiluong 14:95acac6a07c7 449 // Now the velocity of the joints are found by giving the velocity of the end-effector and the inverse jacobian
jordiluong 18:2b6f41f39a7f 450 double msh[2]; // The velocity the joints have to have
jordiluong 14:95acac6a07c7 451 msh[0] = xVelocity*Jvelocity_inv[0] + yVelocity*Jvelocity_inv[1];
jordiluong 14:95acac6a07c7 452 msh[1] = xVelocity*Jvelocity_inv[2] + yVelocity*Jvelocity_inv[3];
jordiluong 14:95acac6a07c7 453
jordiluong 18:2b6f41f39a7f 454 // Determine reference position of motor 1
jordiluong 18:2b6f41f39a7f 455 if(reference1 + msh[0]*processTs > motor1Max) reference1 = motor1Max;
jordiluong 18:2b6f41f39a7f 456 else if(reference1 + msh[0]*processTs < motor1Min) reference1 = motor1Min;
jordiluong 18:2b6f41f39a7f 457 else reference1 = reference1 + msh[0]*processTs;
jordiluong 14:95acac6a07c7 458
jordiluong 18:2b6f41f39a7f 459 // Determine reference position of motor 2
jordiluong 18:2b6f41f39a7f 460 if(reference2 + msh[1]*processTs > motor2Max) reference2 = motor2Max;
jordiluong 18:2b6f41f39a7f 461 else if(reference2 + msh[1]*processTs < motor2Min) reference2 = motor2Min;
jordiluong 18:2b6f41f39a7f 462 else reference2 = reference2 + msh[1]*processTs;
jordiluong 14:95acac6a07c7 463
jordiluong 18:2b6f41f39a7f 464 /*scope.set(0,reference1);
jordiluong 14:95acac6a07c7 465 scope.set(1,position1);
jordiluong 14:95acac6a07c7 466 scope.set(2,reference2);
jordiluong 14:95acac6a07c7 467 scope.set(3,position2);
jordiluong 18:2b6f41f39a7f 468 scope.send();*/
jordiluong 14:95acac6a07c7 469
jordiluong 14:95acac6a07c7 470 pc.printf("position 1 %f, 2 %f \r\n", position1/2/pi*360, position2/2/pi*360);
jordiluong 14:95acac6a07c7 471 pc.printf("reference 1 %f, 2 %f \r\n", reference1/2/pi*360, reference2/2/pi*360);
jordiluong 10:a9e344e440b8 472 }
jordiluong 10:a9e344e440b8 473 }
jordiluong 13:ec227b229f3d 474
jordiluong 7:757e95b4dc46 475 // STATES //////////////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 476 void ProcessStateMachine()
jordiluong 0:80ac024b84cb 477 {
jordiluong 0:80ac024b84cb 478 switch(currentState)
jordiluong 0:80ac024b84cb 479 {
jordiluong 0:80ac024b84cb 480 case MOTORS_OFF:
jordiluong 0:80ac024b84cb 481 {
jordiluong 0:80ac024b84cb 482 // State initialization
jordiluong 0:80ac024b84cb 483 if(stateChanged)
jordiluong 0:80ac024b84cb 484 {
jordiluong 15:5d24f832bb7b 485 pc.printf("Entering MOTORS_OFF \r\n"
jordiluong 15:5d24f832bb7b 486 "Press button 1 to enter CALIBRATING \r\n");
jordiluong 5:0d3e8694726e 487 TurnMotorsOff(); // Turn motors off
jordiluong 0:80ac024b84cb 488 stateChanged = false;
jordiluong 0:80ac024b84cb 489 }
jordiluong 0:80ac024b84cb 490
jordiluong 18:2b6f41f39a7f 491 // Continue button
jordiluong 4:ea7689bf97e1 492 if(!button1)
jordiluong 0:80ac024b84cb 493 {
jordiluong 14:95acac6a07c7 494 currentState = CALIBRATING;
jordiluong 14:95acac6a07c7 495 stateChanged = true;
jordiluong 14:95acac6a07c7 496 break;
jordiluong 14:95acac6a07c7 497 }
jordiluong 14:95acac6a07c7 498 break;
jordiluong 14:95acac6a07c7 499 }
jordiluong 14:95acac6a07c7 500
jordiluong 14:95acac6a07c7 501 case CALIBRATING:
jordiluong 14:95acac6a07c7 502 {
jordiluong 14:95acac6a07c7 503 // State initialization
jordiluong 14:95acac6a07c7 504 if(stateChanged)
jordiluong 14:95acac6a07c7 505 {
jordiluong 15:5d24f832bb7b 506 pc.printf("Entering CALIBRATING \r\n"
jordiluong 18:2b6f41f39a7f 507 "Tighten muscles until green LED is off \r\n");
jordiluong 14:95acac6a07c7 508 stateChanged = false;
jordiluong 18:2b6f41f39a7f 509 calibration_r(); // Calibrate right arm
jordiluong 18:2b6f41f39a7f 510 calibration_l(); // Calibrate left arm
jordiluong 14:95acac6a07c7 511 currentState = MOVING;
jordiluong 14:95acac6a07c7 512 stateChanged = true;
jordiluong 14:95acac6a07c7 513 }
jordiluong 4:ea7689bf97e1 514 break;
jordiluong 0:80ac024b84cb 515 }
jordiluong 0:80ac024b84cb 516
jordiluong 0:80ac024b84cb 517 case MOVING:
jordiluong 0:80ac024b84cb 518 {
jordiluong 0:80ac024b84cb 519 // State initialization
jordiluong 0:80ac024b84cb 520 if(stateChanged)
jordiluong 0:80ac024b84cb 521 {
jordiluong 15:5d24f832bb7b 522 pc.printf("Entering MOVING \r\n");
jordiluong 0:80ac024b84cb 523 stateChanged = false;
jordiluong 0:80ac024b84cb 524 }
jordiluong 4:ea7689bf97e1 525 break;
jordiluong 0:80ac024b84cb 526 }
jordiluong 0:80ac024b84cb 527
jordiluong 0:80ac024b84cb 528 default:
jordiluong 0:80ac024b84cb 529 {
jordiluong 5:0d3e8694726e 530 TurnMotorsOff(); // Turn motors off for safety
jordiluong 4:ea7689bf97e1 531 break;
jordiluong 0:80ac024b84cb 532 }
jordiluong 0:80ac024b84cb 533 }
jordiluong 0:80ac024b84cb 534 }
jordiluong 13:ec227b229f3d 535
jordiluong 7:757e95b4dc46 536 // MAIN FUNCTION ///////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 537 int main()
jordiluong 0:80ac024b84cb 538 {
jordiluong 0:80ac024b84cb 539 // Serial communication
jordiluong 0:80ac024b84cb 540 pc.baud(115200);
jordiluong 0:80ac024b84cb 541
jordiluong 18:2b6f41f39a7f 542 // Start values of LEDs
jordiluong 14:95acac6a07c7 543 led1.write(1);
jordiluong 14:95acac6a07c7 544 led2.write(1);
jordiluong 14:95acac6a07c7 545 led3.write(1);
jordiluong 15:5d24f832bb7b 546 led4.write(1);
jordiluong 15:5d24f832bb7b 547 led5.write(0);
jordiluong 15:5d24f832bb7b 548
jordiluong 4:ea7689bf97e1 549 pc.printf("START \r\n");
jordiluong 4:ea7689bf97e1 550
jordiluong 18:2b6f41f39a7f 551 bqc.add(&bq1_low ).add(&bq2_high ).add(&bq3_notch ); // Make BiQuad Chain
jordiluong 7:757e95b4dc46 552
jordiluong 18:2b6f41f39a7f 553 processTicker.attach(&JointVelocities, processTs); // Ticker to process EMG
jordiluong 18:2b6f41f39a7f 554 controllerTicker.attach(&MotorController, controllerTs); // Ticker to control motor 1 (PID)
jordiluong 18:2b6f41f39a7f 555 emgRight.attach(&check_emg_r, emgTs); // Ticker to sample EMG of right arm
jordiluong 18:2b6f41f39a7f 556 emgLeft.attach(&check_emg_l, emgTs); // Ticker to sample EMG of left arm
jordiluong 12:12b72bd60fd1 557
jordiluong 18:2b6f41f39a7f 558 motor1MagnitudePin.period_ms(1); // PWM frequency of motor 1 (Should actually be 5 - 10 kHz)
jordiluong 18:2b6f41f39a7f 559 motor2MagnitudePin.period_ms(1); // PWM frequency of motor 2 (Should actually be 5 - 10 kHz)
jordiluong 4:ea7689bf97e1 560
jordiluong 0:80ac024b84cb 561 while(true)
jordiluong 0:80ac024b84cb 562 {
jordiluong 0:80ac024b84cb 563 ProcessStateMachine(); // Execute states function
jordiluong 0:80ac024b84cb 564 }
jordiluong 0:80ac024b84cb 565 }