Jordi Luong / Mbed 2 deprecated Project_BioRobotics

Project BioRobotics Group 19

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@14:95acac6a07c7, 2017-11-02 (annotated)

Committer:
jordiluong
Date:
Thu Nov 02 12:19:54 2017 +0000
Revision:
14:95acac6a07c7
Parent:
13:ec227b229f3d
Child:
15:5d24f832bb7b
Added EMG, not really working

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 14:95acac6a07c7 16 enum states{MOTORS_OFF, CALIBRATING, MOVING, HITTING};
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 5:0d3e8694726e 21 QEI Encoder1(D10,D11,NC,32); // CONNECT ENC1A TO D10, ENC1B TO D11
jordiluong 5:0d3e8694726e 22 QEI Encoder2(D12,D13,NC,32); // CONNECT ENC2A TO D12, ENC2B TO D13
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 3:5c3edcd29448 27 DigitalOut motor2DirectionPin(D7); // Value 0: CW or CCW?; 1: CW or 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 14:95acac6a07c7 38 AnalogIn emg_r(A2); // CONNECT EMG TO A2
jordiluong 14:95acac6a07c7 39 AnalogIn emg_l(A3); // CONNECT EMG TO A3
jordiluong 13:ec227b229f3d 40
jordiluong 7:757e95b4dc46 41 // MOTOR CONTROL ///////////////////////////////////////////////////////////////
jordiluong 14:95acac6a07c7 42 Ticker controllerTicker; // Ticker for the controller
jordiluong 14:95acac6a07c7 43 const double controller_Ts = 1/200.1; // Time step for controllerTicker [s]
jordiluong 5:0d3e8694726e 44 const double motorRatio = 131.25; // Ratio of the gearbox in the motors []
jordiluong 7:757e95b4dc46 45 const double radPerPulse = 2*pi/(32*motorRatio); // Amount of radians the motor rotates per encoder pulse [rad/pulse]
jordiluong 14:95acac6a07c7 46 volatile double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start
jordiluong 14:95acac6a07c7 47 double velocity = 0.01; // X and Y velocity of the end effector when desired
jordiluong 13:ec227b229f3d 48
jordiluong 7:757e95b4dc46 49 // MOTOR 1
jordiluong 14:95acac6a07c7 50 volatile double position1; // Position of motor 1 [rad]
jordiluong 14:95acac6a07c7 51 volatile double reference1 = 0; // Desired rotation of motor 1 [rad]
jordiluong 14:95acac6a07c7 52 double motor1Max = 0; // Maximum rotation of motor 1 [rad]
jordiluong 14:95acac6a07c7 53 double motor1Min = 2*pi*-40/360; // Minimum rotation of motor 1 [rad]
jordiluong 5:0d3e8694726e 54 // Controller gains
jordiluong 14:95acac6a07c7 55 const double motor1_KP = 13; // Position gain []
jordiluong 13:ec227b229f3d 56 const double motor1_KI = 7; // Integral gain []
jordiluong 13:ec227b229f3d 57 const double motor1_KD = 0.3; // Derivative gain []
jordiluong 5:0d3e8694726e 58 double motor1_err_int = 0, motor1_prev_err = 0;
jordiluong 5:0d3e8694726e 59 // Derivative filter coefficients
jordiluong 14:95acac6a07c7 60 const double motor1_f_a1 = 0.25, motor1_f_a2 = 0.8; // Derivative filter coefficients []
jordiluong 14:95acac6a07c7 61 const double motor1_f_b0 = 1, motor1_f_b1 = 2, motor1_f_b2 = 0.8; // Derivative filter coefficients []
jordiluong 5:0d3e8694726e 62 // Filter variables
jordiluong 5:0d3e8694726e 63 double motor1_f_v1 = 0, motor1_f_v2 = 0;
jordiluong 13:ec227b229f3d 64
jordiluong 7:757e95b4dc46 65 // MOTOR 2
jordiluong 14:95acac6a07c7 66 volatile double position2; // Position of motor 2 [rad]
jordiluong 14:95acac6a07c7 67 volatile double reference2 = 0; // Desired rotation of motor 2 [rad]
jordiluong 13:ec227b229f3d 68 double motor2Max = 2*pi*25/360; // Maximum rotation of motor 2 [rad]
jordiluong 13:ec227b229f3d 69 double motor2Min = 2*pi*-28/360; // Minimum rotation of motor 2 [rad]
jordiluong 5:0d3e8694726e 70 // Controller gains
jordiluong 14:95acac6a07c7 71 //const double motor2_KP = potmeter1*10; // Position gain []
jordiluong 14:95acac6a07c7 72 //const double motor2_KI = potmeter2*20; // Integral gain []
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 14:95acac6a07c7 83 // EMG //////////////////////////////////////////////////////////////////
jordiluong 14:95acac6a07c7 84 Ticker emgLeft;
jordiluong 14:95acac6a07c7 85 Ticker emgRight;
jordiluong 14:95acac6a07c7 86 double emgTs = 0.5;
jordiluong 14:95acac6a07c7 87 // Filters
jordiluong 14:95acac6a07c7 88 BiQuadChain bqc;
jordiluong 14:95acac6a07c7 89 BiQuad bq2_high(0.875182, -1.750364, 0.87518, -1.73472, 0.766004);
jordiluong 14:95acac6a07c7 90 BiQuad bq3_notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780);
jordiluong 14:95acac6a07c7 91 BiQuad bq1_low(3.65747e-2, 7.31495e-2, 3.65747e-2, -1.390892, 0.537191);
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 14:95acac6a07c7 97 volatile bool check_calibration_r=0;
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 14:95acac6a07c7 105 volatile bool check_calibration_l=0;
jordiluong 14:95acac6a07c7 106 volatile double avg_emg_l;
jordiluong 14:95acac6a07c7 107 volatile bool active_l = false;
jordiluong 13:ec227b229f3d 108
jordiluong 14:95acac6a07c7 109 Ticker sampleTicker;
jordiluong 14:95acac6a07c7 110 const double sampleTs = 0.05;
jordiluong 13:ec227b229f3d 111
jordiluong 7:757e95b4dc46 112 // FUNCTIONS ///////////////////////////////////////////////////////////////////
jordiluong 7:757e95b4dc46 113 // BIQUAD FILTER FOR DERIVATIVE OF ERROR ///////////////////////////////////////
jordiluong 5:0d3e8694726e 114 double biquad(double u, double &v1, double &v2, const double a1,
jordiluong 5:0d3e8694726e 115 const double a2, const double b0, const double b1, const double b2)
jordiluong 0:80ac024b84cb 116 {
jordiluong 5:0d3e8694726e 117 double v = u - a1*v1 - a2*v2;
jordiluong 5:0d3e8694726e 118 double y = b0*v + b1*v1 + b2*v2;
jordiluong 5:0d3e8694726e 119 v2 = v1; v1 = v;
jordiluong 5:0d3e8694726e 120 return y;
jordiluong 0:80ac024b84cb 121 }
jordiluong 13:ec227b229f3d 122
jordiluong 7:757e95b4dc46 123 // PID-CONTROLLER WITH FILTER //////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 124 double PID_Controller(double e, const double Kp, const double Ki,
jordiluong 5:0d3e8694726e 125 const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1,
jordiluong 5:0d3e8694726e 126 double &f_v2, const double f_a1, const double f_a2, const double f_b0,
jordiluong 5:0d3e8694726e 127 const double f_b1, const double f_b2)
jordiluong 0:80ac024b84cb 128 {
jordiluong 5:0d3e8694726e 129 // Derivative
jordiluong 8:78d8ccf84a38 130 double e_der = (e - e_prev)/Ts; // Calculate the derivative of error
jordiluong 12:12b72bd60fd1 131 e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); // Filter the derivative of error
jordiluong 12:12b72bd60fd1 132 //e_der = bq1.step(e_der);
jordiluong 5:0d3e8694726e 133 e_prev = e;
jordiluong 5:0d3e8694726e 134 // Integral
jordiluong 8:78d8ccf84a38 135 e_int = e_int + Ts*e; // Calculate the integral of error
jordiluong 5:0d3e8694726e 136 // PID
jordiluong 12:12b72bd60fd1 137 //pc.printf("%f --> %f \r\n", e_der, e_derf);
jordiluong 11:5c06c97c3673 138 return Kp*e + Ki*e_int + Kd*e_der;
jordiluong 3:5c3edcd29448 139 }
jordiluong 13:ec227b229f3d 140
jordiluong 7:757e95b4dc46 141 // MOTOR 1 /////////////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 142 void RotateMotor1(double motor1Value)
jordiluong 3:5c3edcd29448 143 {
jordiluong 10:a9e344e440b8 144 if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING
jordiluong 5:0d3e8694726e 145 {
jordiluong 10:a9e344e440b8 146 if(motor1Value >= 0) motor1DirectionPin = 0; // Rotate motor 1 CW
jordiluong 10:a9e344e440b8 147 else motor1DirectionPin = 1; // Rotate motor 1 CCW
jordiluong 5:0d3e8694726e 148
jordiluong 5:0d3e8694726e 149 if(fabs(motor1Value) > 1) motor1MagnitudePin = 1;
jordiluong 5:0d3e8694726e 150 else motor1MagnitudePin = fabs(motor1Value);
jordiluong 5:0d3e8694726e 151 }
jordiluong 5:0d3e8694726e 152 else motor1MagnitudePin = 0;
jordiluong 3:5c3edcd29448 153 }
jordiluong 13:ec227b229f3d 154
jordiluong 10:a9e344e440b8 155 // MOTOR 2 /////////////////////////////////////////////////////////////////////
jordiluong 10:a9e344e440b8 156 void RotateMotor2(double motor2Value)
jordiluong 10:a9e344e440b8 157 {
jordiluong 10:a9e344e440b8 158 if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING
jordiluong 10:a9e344e440b8 159 {
jordiluong 10:a9e344e440b8 160 if(motor2Value >= 0) motor2DirectionPin = 1; // Rotate motor 1 CW
jordiluong 10:a9e344e440b8 161 else motor2DirectionPin = 0; // Rotate motor 1 CCW
jordiluong 10:a9e344e440b8 162
jordiluong 10:a9e344e440b8 163 if(fabs(motor2Value) > 1) motor2MagnitudePin = 1;
jordiluong 10:a9e344e440b8 164 else motor2MagnitudePin = fabs(motor2Value);
jordiluong 10:a9e344e440b8 165 }
jordiluong 10:a9e344e440b8 166 else motor2MagnitudePin = 0;
jordiluong 10:a9e344e440b8 167 }
jordiluong 13:ec227b229f3d 168
jordiluong 7:757e95b4dc46 169 // MOTOR 1 PID-CONTROLLER //////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 170 void Motor1Controller()
jordiluong 5:0d3e8694726e 171 {
jordiluong 14:95acac6a07c7 172 if(currentState == MOVING)
jordiluong 14:95acac6a07c7 173 {
jordiluong 14:95acac6a07c7 174 position1 = radPerPulse * Encoder1.getPulses();
jordiluong 14:95acac6a07c7 175 position2 = radPerPulse * Encoder2.getPulses();
jordiluong 14:95acac6a07c7 176 //pc.printf("error %f \r\n", reference1 - position1);
jordiluong 14:95acac6a07c7 177 double motor1Value = PID_Controller(reference1 - position1, motor1_KP,
jordiluong 14:95acac6a07c7 178 motor1_KI, motor1_KD, controller_Ts, motor1_err_int, motor1_prev_err,
jordiluong 14:95acac6a07c7 179 motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0,
jordiluong 14:95acac6a07c7 180 motor1_f_b1, motor1_f_b2);
jordiluong 14:95acac6a07c7 181 //pc.printf("motor value %f \r\n",motor1Value);
jordiluong 14:95acac6a07c7 182
jordiluong 14:95acac6a07c7 183 double motor2Value = PID_Controller(reference2 - position2, motor2_KP,
jordiluong 14:95acac6a07c7 184 motor2_KI, motor2_KD, controller_Ts, 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 14:95acac6a07c7 188 //pc.printf("%f, %f \r\n", motor1Value, motor2Value);
jordiluong 14:95acac6a07c7 189
jordiluong 14:95acac6a07c7 190 RotateMotor1(motor1Value);
jordiluong 14:95acac6a07c7 191 RotateMotor2(motor2Value);
jordiluong 14:95acac6a07c7 192 }
jordiluong 10:a9e344e440b8 193 }
jordiluong 13:ec227b229f3d 194
jordiluong 12:12b72bd60fd1 195 // MOTOR 2 PID-CONTROLLER //////////////////////////////////////////////////////
jordiluong 12:12b72bd60fd1 196 /*void Motor2Controller()
jordiluong 12:12b72bd60fd1 197 {
jordiluong 12:12b72bd60fd1 198 position2 = radPerPulse * Encoder2.getPulses();
jordiluong 12:12b72bd60fd1 199 double motor2Value = PID_Controller(reference2 - position2, motor2_KP,
jordiluong 12:12b72bd60fd1 200 motor2_KI, motor2_KD, controller2_Ts, motor2_err_int, motor2_prev_err,
jordiluong 12:12b72bd60fd1 201 motor2_f_v1, motor2_f_v2, motor2_f_a1, motor2_f_a2, motor2_f_b0,
jordiluong 12:12b72bd60fd1 202 motor2_f_b1, motor2_f_b2);
jordiluong 12:12b72bd60fd1 203 RotateMotor2(motor2Value);
jordiluong 12:12b72bd60fd1 204 }
jordiluong 12:12b72bd60fd1 205 */
jordiluong 7:757e95b4dc46 206 // TURN OFF MOTORS /////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 207 void TurnMotorsOff()
jordiluong 5:0d3e8694726e 208 {
jordiluong 5:0d3e8694726e 209 motor1MagnitudePin = 0;
jordiluong 5:0d3e8694726e 210 motor2MagnitudePin = 0;
jordiluong 5:0d3e8694726e 211 }
jordiluong 13:ec227b229f3d 212
jordiluong 14:95acac6a07c7 213
jordiluong 14:95acac6a07c7 214 // EMG /////////////////////////////////////////////////////////////////////////
jordiluong 14:95acac6a07c7 215 // Filter EMG signal of right arm
jordiluong 13:ec227b229f3d 216
jordiluong 14:95acac6a07c7 217 void filter_r(){
jordiluong 14:95acac6a07c7 218 if(check_calibration_r==1){
jordiluong 14:95acac6a07c7 219 emg_value_r = emg_r.read();
jordiluong 14:95acac6a07c7 220 emgFiltered_r = bqc.step( emg_value_r );
jordiluong 14:95acac6a07c7 221 filteredAbs_r = abs( emgFiltered_r );
jordiluong 14:95acac6a07c7 222 if (avg_emg_r != 0){
jordiluong 14:95acac6a07c7 223 onoffsignal_r=filteredAbs_r/avg_emg_r; //divide the emg_r signal by the max emg_r to calibrate the signal per person
jordiluong 14:95acac6a07c7 224 }
jordiluong 14:95acac6a07c7 225 }
jordiluong 14:95acac6a07c7 226 }
jordiluong 14:95acac6a07c7 227
jordiluong 14:95acac6a07c7 228 // Filter EMG signal of left arm
jordiluong 14:95acac6a07c7 229 void filter_l(){
jordiluong 14:95acac6a07c7 230 if(check_calibration_l==1){
jordiluong 14:95acac6a07c7 231 emg_value_l = emg_l.read();
jordiluong 14:95acac6a07c7 232 emgFiltered_l = bqc.step( emg_value_l );
jordiluong 14:95acac6a07c7 233 filteredAbs_l = abs( emgFiltered_l );
jordiluong 14:95acac6a07c7 234 if (avg_emg_l != 0){
jordiluong 14:95acac6a07c7 235 onoffsignal_l=filteredAbs_l/avg_emg_l; //divide the emg_r signal by the avg_emg_l wat staat voor avg emg in gespannen toestand
jordiluong 14:95acac6a07c7 236 }
jordiluong 10:a9e344e440b8 237 }
jordiluong 14:95acac6a07c7 238 }
jordiluong 14:95acac6a07c7 239
jordiluong 14:95acac6a07c7 240 // Check threshold right arm
jordiluong 14:95acac6a07c7 241 void check_emg_r(){
jordiluong 14:95acac6a07c7 242 double filteredAbs_temp_r;
jordiluong 14:95acac6a07c7 243
jordiluong 14:95acac6a07c7 244 if((check_calibration_l==1) &&(check_calibration_r==1)){
jordiluong 14:95acac6a07c7 245 for( int i = 0; i<250;i++){
jordiluong 14:95acac6a07c7 246 filter_r();
jordiluong 14:95acac6a07c7 247 filteredAbs_temp_r = filteredAbs_temp_r + onoffsignal_r;
jordiluong 14:95acac6a07c7 248 wait(0.0004); // 0.0004
jordiluong 14:95acac6a07c7 249 }
jordiluong 14:95acac6a07c7 250 filteredAbs_temp_r = filteredAbs_temp_r/250;
jordiluong 14:95acac6a07c7 251 if(filteredAbs_temp_r<=0.3){ //if signal is lower then 0.5 the blue light goes on
jordiluong 14:95acac6a07c7 252 led1.write(1); //led 1 is rood en uit
jordiluong 14:95acac6a07c7 253
jordiluong 14:95acac6a07c7 254 active_r = false;
jordiluong 14:95acac6a07c7 255 }
jordiluong 14:95acac6a07c7 256 else if(filteredAbs_temp_r > 0.3){ //if signal does not pass threshold value, blue light goes on
jordiluong 14:95acac6a07c7 257 led1.write(0);
jordiluong 14:95acac6a07c7 258 active_r = true;
jordiluong 14:95acac6a07c7 259 }
jordiluong 10:a9e344e440b8 260 }
jordiluong 14:95acac6a07c7 261 }
jordiluong 14:95acac6a07c7 262 // Check threshold left arm
jordiluong 14:95acac6a07c7 263 void check_emg_l(){
jordiluong 14:95acac6a07c7 264 double filteredAbs_temp_l;
jordiluong 14:95acac6a07c7 265
jordiluong 14:95acac6a07c7 266 if((check_calibration_l)==1 &&(check_calibration_r==1) ){
jordiluong 14:95acac6a07c7 267 for( int i = 0; i<250;i++){
jordiluong 14:95acac6a07c7 268 filter_l();
jordiluong 14:95acac6a07c7 269 filteredAbs_temp_l = filteredAbs_temp_l + onoffsignal_l;
jordiluong 14:95acac6a07c7 270 wait(0.0004); // 0.0004
jordiluong 14:95acac6a07c7 271 }
jordiluong 14:95acac6a07c7 272 filteredAbs_temp_l = filteredAbs_temp_l/250;
jordiluong 14:95acac6a07c7 273 if(filteredAbs_temp_l<=0.3){ //if signal is lower then 0.5 the blue light goes on
jordiluong 14:95acac6a07c7 274 // led1.write(1); //led 1 is rood en uit
jordiluong 14:95acac6a07c7 275 led2.write(1); //led 2 is blauw en aan
jordiluong 14:95acac6a07c7 276 active_l = false;
jordiluong 14:95acac6a07c7 277 }
jordiluong 14:95acac6a07c7 278 else if(filteredAbs_temp_l > 0.3){ //if signal does not pass threshold value, blue light goes on
jordiluong 14:95acac6a07c7 279 // led1.write(0);
jordiluong 14:95acac6a07c7 280 led2.write(0);
jordiluong 14:95acac6a07c7 281 active_l = true;
jordiluong 14:95acac6a07c7 282 }
jordiluong 14:95acac6a07c7 283 }
jordiluong 14:95acac6a07c7 284
jordiluong 10:a9e344e440b8 285 }
jordiluong 13:ec227b229f3d 286
jordiluong 14:95acac6a07c7 287 // Calibrate right arm
jordiluong 14:95acac6a07c7 288 int calibration_r(){
jordiluong 14:95acac6a07c7 289 led3.write(0);
jordiluong 14:95acac6a07c7 290
jordiluong 14:95acac6a07c7 291 double signal_verzameling_r = 0;
jordiluong 14:95acac6a07c7 292 for(int n =0; n<5000;n++){
jordiluong 14:95acac6a07c7 293 filter_r();
jordiluong 14:95acac6a07c7 294 //read for 5000 samples as calibration
jordiluong 14:95acac6a07c7 295 emg_value_r = emg_r.read();
jordiluong 14:95acac6a07c7 296 emgFiltered_r = bqc.step( emg_value_r );
jordiluong 14:95acac6a07c7 297 filteredAbs_r = abs(emgFiltered_r);
jordiluong 14:95acac6a07c7 298
jordiluong 14:95acac6a07c7 299
jordiluong 14:95acac6a07c7 300 // signal_verzameling[n]= filteredAbs_r;
jordiluong 14:95acac6a07c7 301 signal_verzameling_r = signal_verzameling_r + filteredAbs_r ;
jordiluong 14:95acac6a07c7 302 wait(0.0004);
jordiluong 14:95acac6a07c7 303
jordiluong 14:95acac6a07c7 304 if (n == 4999){
jordiluong 14:95acac6a07c7 305 avg_emg_r = signal_verzameling_r / n;
jordiluong 14:95acac6a07c7 306
jordiluong 14:95acac6a07c7 307 }
jordiluong 14:95acac6a07c7 308 }
jordiluong 14:95acac6a07c7 309
jordiluong 14:95acac6a07c7 310 led3.write(1);
jordiluong 14:95acac6a07c7 311 //pc.printf("calibratie rechts compleet\n\r");
jordiluong 14:95acac6a07c7 312
jordiluong 14:95acac6a07c7 313 check_calibration_r=1;
jordiluong 14:95acac6a07c7 314 return 0;
jordiluong 14:95acac6a07c7 315 }
jordiluong 14:95acac6a07c7 316
jordiluong 14:95acac6a07c7 317 // Calibrate left arm
jordiluong 14:95acac6a07c7 318 int calibration_l(){
jordiluong 14:95acac6a07c7 319 led3.write(0);
jordiluong 14:95acac6a07c7 320
jordiluong 14:95acac6a07c7 321 double signal_verzameling_l= 0;
jordiluong 14:95acac6a07c7 322 for(int n =0; n<5000;n++){
jordiluong 14:95acac6a07c7 323 filter_l();
jordiluong 14:95acac6a07c7 324 //read for 5000 samples as calibration
jordiluong 14:95acac6a07c7 325 emg_value_l = emg_l.read();
jordiluong 14:95acac6a07c7 326 emgFiltered_l = bqc.step( emg_value_l );
jordiluong 14:95acac6a07c7 327 filteredAbs_l = abs(emgFiltered_l);
jordiluong 14:95acac6a07c7 328
jordiluong 14:95acac6a07c7 329
jordiluong 14:95acac6a07c7 330 // signal_verzameling[n]= filteredAbs_r;
jordiluong 14:95acac6a07c7 331 signal_verzameling_l = signal_verzameling_l + filteredAbs_l ;
jordiluong 14:95acac6a07c7 332 wait(0.0004);
jordiluong 14:95acac6a07c7 333
jordiluong 14:95acac6a07c7 334 if (n == 4999){
jordiluong 14:95acac6a07c7 335 avg_emg_l = signal_verzameling_l / n;
jordiluong 14:95acac6a07c7 336
jordiluong 14:95acac6a07c7 337 }
jordiluong 14:95acac6a07c7 338 }
jordiluong 14:95acac6a07c7 339 led3.write(1);
jordiluong 14:95acac6a07c7 340 wait(1);
jordiluong 14:95acac6a07c7 341 check_calibration_l=1;
jordiluong 14:95acac6a07c7 342
jordiluong 14:95acac6a07c7 343 //pc.printf("calibratie links compleet\n\r");
jordiluong 14:95acac6a07c7 344 // }
jordiluong 14:95acac6a07c7 345 return 0;
jordiluong 14:95acac6a07c7 346 }
jordiluong 14:95acac6a07c7 347
jordiluong 14:95acac6a07c7 348 // DETERMINE JOINT VELOCITIES //////////////////////////////////////////////////
jordiluong 14:95acac6a07c7 349 void JointVelocities()
jordiluong 10:a9e344e440b8 350 {
jordiluong 10:a9e344e440b8 351 if(currentState == MOVING)
jordiluong 10:a9e344e440b8 352 {
jordiluong 14:95acac6a07c7 353 position1 = radPerPulse * Encoder1.getPulses();
jordiluong 14:95acac6a07c7 354 position2 = radPerPulse * Encoder2.getPulses();
jordiluong 14:95acac6a07c7 355
jordiluong 14:95acac6a07c7 356 //check_emg_r(), check_emg_l();
jordiluong 14:95acac6a07c7 357
jordiluong 14:95acac6a07c7 358 if(active_r && reference1 > motor1Min && reference2 < motor2Max)
jordiluong 14:95acac6a07c7 359 {
jordiluong 14:95acac6a07c7 360 xVelocity = velocity;
jordiluong 14:95acac6a07c7 361 //pc.printf("button1 \r\n");
jordiluong 14:95acac6a07c7 362 }
jordiluong 14:95acac6a07c7 363 else if(active_l && reference1 < motor1Max && reference2 > motor2Min)
jordiluong 14:95acac6a07c7 364 {
jordiluong 14:95acac6a07c7 365 xVelocity = -velocity;
jordiluong 14:95acac6a07c7 366 //pc.printf(" button2 \r\n");
jordiluong 14:95acac6a07c7 367 }
jordiluong 14:95acac6a07c7 368 else xVelocity = 0;
jordiluong 14:95acac6a07c7 369
jordiluong 14:95acac6a07c7 370 if(!button3 && reference2 < motor2Max )//&& reference1 > motor2Min)
jordiluong 14:95acac6a07c7 371 {
jordiluong 14:95acac6a07c7 372 yVelocity = velocity;
jordiluong 14:95acac6a07c7 373 //pc.printf(" button3 \r\n");
jordiluong 14:95acac6a07c7 374 }
jordiluong 14:95acac6a07c7 375 else if(!button4 && reference2 > motor2Min )//&& reference1 > motor2Min)
jordiluong 14:95acac6a07c7 376 {
jordiluong 14:95acac6a07c7 377 yVelocity = -velocity;
jordiluong 14:95acac6a07c7 378 //pc.printf(" button4 \r\n");
jordiluong 14:95acac6a07c7 379 }
jordiluong 14:95acac6a07c7 380 else yVelocity = 0;
jordiluong 14:95acac6a07c7 381
jordiluong 14:95acac6a07c7 382 //pc.printf("x %f, y %f \r\n", xVelocity, yVelocity);
jordiluong 14:95acac6a07c7 383
jordiluong 14:95acac6a07c7 384 // Construct Jacobian
jordiluong 14:95acac6a07c7 385 double q[4];
jordiluong 14:95acac6a07c7 386 q[0] = position1, q[1] = -position1;
jordiluong 14:95acac6a07c7 387 q[2] = position2, q[3] = -position2;
jordiluong 14:95acac6a07c7 388
jordiluong 14:95acac6a07c7 389 double T2[3]; // Second column of the jacobian
jordiluong 14:95acac6a07c7 390 double T3[3]; // Third column of the jacobian
jordiluong 14:95acac6a07c7 391 double T4[3]; // Fourth column of the jacobian
jordiluong 14:95acac6a07c7 392 double T1[6];
jordiluong 14:95acac6a07c7 393 static const signed char b_T1[3] = { 1, 0, 0 };
jordiluong 14:95acac6a07c7 394 double J_data[6];
jordiluong 14:95acac6a07c7 395
jordiluong 14:95acac6a07c7 396 T2[0] = 1.0;
jordiluong 14:95acac6a07c7 397 T2[1] = 0.365 * cos(q[0]);
jordiluong 14:95acac6a07c7 398 T2[2] = 0.365 * sin(q[0]);
jordiluong 14:95acac6a07c7 399 T3[0] = 1.0;
jordiluong 14:95acac6a07c7 400 T3[1] = 0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 +
jordiluong 14:95acac6a07c7 401 q[0]) + q[1]);
jordiluong 14:95acac6a07c7 402 T3[2] = 0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 +
jordiluong 14:95acac6a07c7 403 q[0]) + q[1]);
jordiluong 14:95acac6a07c7 404 T4[0] = 1.0;
jordiluong 14:95acac6a07c7 405 T4[1] = (0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 +
jordiluong 14:95acac6a07c7 406 q[0]) + q[1])) + 0.265 * sin((q[0] + q[1]) + q[2]);
jordiluong 14:95acac6a07c7 407 T4[2] = (0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 +
jordiluong 14:95acac6a07c7 408 q[0]) + q[1])) - 0.265 * cos((q[0] + q[1]) + q[2]);
jordiluong 14:95acac6a07c7 409
jordiluong 14:95acac6a07c7 410 for (int i = 0; i < 3; i++)
jordiluong 14:95acac6a07c7 411 {
jordiluong 14:95acac6a07c7 412 T1[i] = (double)b_T1[i] - T2[i];
jordiluong 14:95acac6a07c7 413 T1[3 + i] = T3[i] - T4[i];
jordiluong 14:95acac6a07c7 414 }
jordiluong 14:95acac6a07c7 415
jordiluong 14:95acac6a07c7 416 for (int i = 0; i < 2; i++)
jordiluong 14:95acac6a07c7 417 {
jordiluong 14:95acac6a07c7 418 for (int j = 0; j < 3; j++)
jordiluong 14:95acac6a07c7 419 {
jordiluong 14:95acac6a07c7 420 J_data[j + 3 * i] = T1[j + 3 * i];
jordiluong 14:95acac6a07c7 421 }
jordiluong 14:95acac6a07c7 422 }
jordiluong 14:95acac6a07c7 423
jordiluong 14:95acac6a07c7 424 // Here the first row of the Jacobian is cut off, so we do not take rotation into consideration
jordiluong 14:95acac6a07c7 425 // Note: the matrices from now on will we constructed rowwise
jordiluong 14:95acac6a07c7 426 double Jvelocity[4];
jordiluong 14:95acac6a07c7 427 Jvelocity[0] = J_data[1];
jordiluong 14:95acac6a07c7 428 Jvelocity[1] = J_data[4];
jordiluong 14:95acac6a07c7 429 Jvelocity[2] = J_data[2];
jordiluong 14:95acac6a07c7 430 Jvelocity[3] = J_data[5];
jordiluong 14:95acac6a07c7 431
jordiluong 14:95acac6a07c7 432 // Creating the inverse Jacobian
jordiluong 14:95acac6a07c7 433 double Jvelocity_inv[4]; // The inverse matrix of the jacobian
jordiluong 14:95acac6a07c7 434 double determ = Jvelocity[0]*Jvelocity[3]-Jvelocity[1]*Jvelocity[2]; // The determinant of the matrix
jordiluong 14:95acac6a07c7 435 Jvelocity_inv[0] = Jvelocity[3]/determ;
jordiluong 14:95acac6a07c7 436 Jvelocity_inv[1] = -Jvelocity[1]/determ;
jordiluong 14:95acac6a07c7 437 Jvelocity_inv[2] = -Jvelocity[2]/determ;
jordiluong 14:95acac6a07c7 438 Jvelocity_inv[3] = Jvelocity[0]/determ;
jordiluong 14:95acac6a07c7 439
jordiluong 14:95acac6a07c7 440 // Now the velocity of the joints are found by giving the velocity of the end-effector and the inverse jacobian
jordiluong 14:95acac6a07c7 441 double msh[2]; // This is the velocity the joints have to have
jordiluong 14:95acac6a07c7 442 msh[0] = xVelocity*Jvelocity_inv[0] + yVelocity*Jvelocity_inv[1];
jordiluong 14:95acac6a07c7 443 msh[1] = xVelocity*Jvelocity_inv[2] + yVelocity*Jvelocity_inv[3];
jordiluong 14:95acac6a07c7 444
jordiluong 14:95acac6a07c7 445 if(reference1 + msh[0]*sampleTs > motor1Max) reference1 = motor1Max;
jordiluong 14:95acac6a07c7 446 else if(reference1 + msh[0]*sampleTs < motor1Min) reference1 = motor1Min;
jordiluong 14:95acac6a07c7 447 else reference1 = reference1 + msh[0]*sampleTs;
jordiluong 14:95acac6a07c7 448
jordiluong 14:95acac6a07c7 449 if(reference2 + msh[1]*sampleTs > motor2Max) reference2 = motor2Max;
jordiluong 14:95acac6a07c7 450 else if(reference2 + msh[1]*sampleTs < motor2Min) reference2 = motor2Min;
jordiluong 14:95acac6a07c7 451 else reference2 = reference2 + msh[1]*sampleTs;
jordiluong 14:95acac6a07c7 452
jordiluong 14:95acac6a07c7 453
jordiluong 14:95acac6a07c7 454 scope.set(0,reference1);
jordiluong 14:95acac6a07c7 455 scope.set(1,position1);
jordiluong 14:95acac6a07c7 456 scope.set(2,reference2);
jordiluong 14:95acac6a07c7 457 scope.set(3,position2);
jordiluong 14:95acac6a07c7 458 scope.send();
jordiluong 14:95acac6a07c7 459
jordiluong 14:95acac6a07c7 460 pc.printf("position 1 %f, 2 %f \r\n", position1/2/pi*360, position2/2/pi*360);
jordiluong 14:95acac6a07c7 461 pc.printf("reference 1 %f, 2 %f \r\n", reference1/2/pi*360, reference2/2/pi*360);
jordiluong 14:95acac6a07c7 462 //pc.printf("msh*Ts 1 %f, 2 %f \r\n\n", msh[0]*emg_Ts, msh[1]*emg_Ts);
jordiluong 14:95acac6a07c7 463
jordiluong 10:a9e344e440b8 464 }
jordiluong 10:a9e344e440b8 465 }
jordiluong 13:ec227b229f3d 466
jordiluong 7:757e95b4dc46 467 // STATES //////////////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 468 void ProcessStateMachine()
jordiluong 0:80ac024b84cb 469 {
jordiluong 0:80ac024b84cb 470 switch(currentState)
jordiluong 0:80ac024b84cb 471 {
jordiluong 0:80ac024b84cb 472 case MOTORS_OFF:
jordiluong 0:80ac024b84cb 473 {
jordiluong 0:80ac024b84cb 474 // State initialization
jordiluong 0:80ac024b84cb 475 if(stateChanged)
jordiluong 0:80ac024b84cb 476 {
jordiluong 12:12b72bd60fd1 477 //pc.printf("Entering MOTORS_OFF \r\n"
jordiluong 14:95acac6a07c7 478 //"Press button 1 to enter CALIBRATING \r\n");
jordiluong 5:0d3e8694726e 479 TurnMotorsOff(); // Turn motors off
jordiluong 0:80ac024b84cb 480 stateChanged = false;
jordiluong 0:80ac024b84cb 481 }
jordiluong 0:80ac024b84cb 482
jordiluong 0:80ac024b84cb 483 // Home command
jordiluong 4:ea7689bf97e1 484 if(!button1)
jordiluong 0:80ac024b84cb 485 {
jordiluong 14:95acac6a07c7 486 currentState = CALIBRATING;
jordiluong 14:95acac6a07c7 487 stateChanged = true;
jordiluong 14:95acac6a07c7 488 break;
jordiluong 14:95acac6a07c7 489 }
jordiluong 14:95acac6a07c7 490 break;
jordiluong 14:95acac6a07c7 491 }
jordiluong 14:95acac6a07c7 492
jordiluong 14:95acac6a07c7 493 case CALIBRATING:
jordiluong 14:95acac6a07c7 494 {
jordiluong 14:95acac6a07c7 495 // State initialization
jordiluong 14:95acac6a07c7 496 if(stateChanged)
jordiluong 14:95acac6a07c7 497 {
jordiluong 14:95acac6a07c7 498 //pc.printf("Entering CALIBRATING \r\n"
jordiluong 14:95acac6a07c7 499 //"Press button 1 to enter MOVING \r\n");
jordiluong 14:95acac6a07c7 500 stateChanged = false;
jordiluong 14:95acac6a07c7 501 calibration_r();
jordiluong 14:95acac6a07c7 502 calibration_l();
jordiluong 14:95acac6a07c7 503 currentState = MOVING;
jordiluong 14:95acac6a07c7 504 stateChanged = true;
jordiluong 14:95acac6a07c7 505 }
jordiluong 14:95acac6a07c7 506 /*
jordiluong 14:95acac6a07c7 507 // Home command
jordiluong 14:95acac6a07c7 508 if(!button1)
jordiluong 14:95acac6a07c7 509 {
jordiluong 0:80ac024b84cb 510 currentState = MOVING;
jordiluong 0:80ac024b84cb 511 stateChanged = true;
jordiluong 0:80ac024b84cb 512 break;
jordiluong 0:80ac024b84cb 513 }
jordiluong 14:95acac6a07c7 514 */
jordiluong 4:ea7689bf97e1 515 break;
jordiluong 0:80ac024b84cb 516 }
jordiluong 0:80ac024b84cb 517
jordiluong 0:80ac024b84cb 518 case MOVING:
jordiluong 0:80ac024b84cb 519 {
jordiluong 0:80ac024b84cb 520 // State initialization
jordiluong 0:80ac024b84cb 521 if(stateChanged)
jordiluong 0:80ac024b84cb 522 {
jordiluong 14:95acac6a07c7 523 //pc.printf("Entering MOVING \r\n");
jordiluong 12:12b72bd60fd1 524 //"Press button 2 to enter HITTING \r\n");
jordiluong 0:80ac024b84cb 525 stateChanged = false;
jordiluong 0:80ac024b84cb 526 }
jordiluong 0:80ac024b84cb 527
jordiluong 0:80ac024b84cb 528 // Hit command
jordiluong 10:a9e344e440b8 529 /*if(!button2)
jordiluong 0:80ac024b84cb 530 {
jordiluong 0:80ac024b84cb 531 currentState = HITTING;
jordiluong 0:80ac024b84cb 532 stateChanged = true;
jordiluong 0:80ac024b84cb 533 break;
jordiluong 0:80ac024b84cb 534 }
jordiluong 10:a9e344e440b8 535 */
jordiluong 4:ea7689bf97e1 536 break;
jordiluong 0:80ac024b84cb 537 }
jordiluong 0:80ac024b84cb 538
jordiluong 0:80ac024b84cb 539 case HITTING:
jordiluong 0:80ac024b84cb 540 {
jordiluong 0:80ac024b84cb 541 // State initialization
jordiluong 0:80ac024b84cb 542 if(stateChanged)
jordiluong 0:80ac024b84cb 543 {
jordiluong 12:12b72bd60fd1 544 //pc.printf("Entering HITTING \r\n");
jordiluong 5:0d3e8694726e 545 stateChanged = false;
jordiluong 4:ea7689bf97e1 546 //HitBall(); // Hit the ball
jordiluong 0:80ac024b84cb 547 currentState = MOVING;
jordiluong 0:80ac024b84cb 548 stateChanged = true;
jordiluong 0:80ac024b84cb 549 break;
jordiluong 0:80ac024b84cb 550 }
jordiluong 4:ea7689bf97e1 551 break;
jordiluong 0:80ac024b84cb 552 }
jordiluong 0:80ac024b84cb 553
jordiluong 0:80ac024b84cb 554 default:
jordiluong 0:80ac024b84cb 555 {
jordiluong 5:0d3e8694726e 556 TurnMotorsOff(); // Turn motors off for safety
jordiluong 4:ea7689bf97e1 557 break;
jordiluong 0:80ac024b84cb 558 }
jordiluong 0:80ac024b84cb 559 }
jordiluong 0:80ac024b84cb 560 }
jordiluong 13:ec227b229f3d 561
jordiluong 7:757e95b4dc46 562 // MAIN FUNCTION ///////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 563 int main()
jordiluong 0:80ac024b84cb 564 {
jordiluong 0:80ac024b84cb 565 // Serial communication
jordiluong 0:80ac024b84cb 566 pc.baud(115200);
jordiluong 0:80ac024b84cb 567
jordiluong 14:95acac6a07c7 568 led1.write(1);
jordiluong 14:95acac6a07c7 569 led2.write(1);
jordiluong 14:95acac6a07c7 570 led3.write(1);
jordiluong 4:ea7689bf97e1 571 pc.printf("START \r\n");
jordiluong 4:ea7689bf97e1 572
jordiluong 14:95acac6a07c7 573 bqc.add( &bq1_low ).add( &bq2_high ).add( &bq3_notch );
jordiluong 7:757e95b4dc46 574
jordiluong 14:95acac6a07c7 575 sampleTicker.attach(&JointVelocities, sampleTs); // Ticker to sample EMG
jordiluong 12:12b72bd60fd1 576 controllerTicker.attach(&Motor1Controller, controller_Ts); // Ticker to control motor 1 (PID)
jordiluong 14:95acac6a07c7 577 emgRight.attach(&check_emg_r, emgTs); //continously execute the motor controller
jordiluong 14:95acac6a07c7 578 emgLeft.attach(&check_emg_l, emgTs);
jordiluong 12:12b72bd60fd1 579
jordiluong 12:12b72bd60fd1 580 motor1MagnitudePin.period_ms(1);
jordiluong 12:12b72bd60fd1 581 motor2MagnitudePin.period_ms(1);
jordiluong 12:12b72bd60fd1 582 TurnMotorsOff();
jordiluong 4:ea7689bf97e1 583
jordiluong 0:80ac024b84cb 584 while(true)
jordiluong 0:80ac024b84cb 585 {
jordiluong 0:80ac024b84cb 586 ProcessStateMachine(); // Execute states function
jordiluong 0:80ac024b84cb 587 }
jordiluong 0:80ac024b84cb 588 }