Marijke Zondag / Mbed 2 deprecated Project_script_union_final_version

final version

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Project_script_union_final by Jorine Oosting

main.cpp@35:63c890ac71ff, 2018-11-05 (annotated)

Committer:
MarijkeZondag
Date:
Mon Nov 05 16:45:17 2018 +0000
Revision:
35:63c890ac71ff
Parent:
34:b8b18ba0c336
Child:
36:650a9245bc44
Code mooi gemaakt + comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:c8f15874531b 1 #include "mbed.h"
vsluiter 0:c8f15874531b 2 #include "MODSERIAL.h"
MarijkeZondag 10:39ec51206c8b 3 #include "BiQuad.h"
MarijkeZondag 10:39ec51206c8b 4 #include "HIDScope.h"
MarijkeZondag 10:39ec51206c8b 5 #include <math.h>
MarijkeZondag 28:5e54cd4525de 6 #include "QEI.h"
vsluiter 0:c8f15874531b 7
MarijkeZondag 22:5d956c93b3ae 8 //ATTENTION: set mBed to version 151
MarijkeZondag 22:5d956c93b3ae 9 // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
MarijkeZondag 22:5d956c93b3ae 10 // set MODSERIAL to version 44
MarijkeZondag 22:5d956c93b3ae 11 // set HIDScope to version 7
MarijkeZondag 22:5d956c93b3ae 12 // set biquadFilter to version 7
MarijkeZondag 10:39ec51206c8b 13
MarijkeZondag 22:5d956c93b3ae 14 AnalogIn emg0_in (A0); //First raw EMG signal input
MarijkeZondag 22:5d956c93b3ae 15 AnalogIn emg1_in (A1); //Second raw EMG signal input
MarijkeZondag 22:5d956c93b3ae 16 AnalogIn emg2_in (A2); //Third raw EMG signal input
MarijkeZondag 22:5d956c93b3ae 17
MarijkeZondag 26:ac5656aa35c7 18 InterruptIn button1 (D10);
MarijkeZondag 16:5f7196ddc77b 19 InterruptIn button2 (D11);
MarijkeZondag 6:f4bbb73f3989 20
MarijkeZondag 28:5e54cd4525de 21 DigitalOut directionpin1 (D4);
MarijkeZondag 28:5e54cd4525de 22 DigitalOut directionpin2 (D7);
MarijkeZondag 26:ac5656aa35c7 23
MarijkeZondag 28:5e54cd4525de 24 PwmOut pwmpin1 (D5);
MarijkeZondag 28:5e54cd4525de 25 PwmOut pwmpin2 (D6);
MarijkeZondag 22:5d956c93b3ae 26
MarijkeZondag 17:741798018c0d 27 DigitalOut ledr (LED_RED);
MarijkeZondag 17:741798018c0d 28 DigitalOut ledb (LED_BLUE);
MarijkeZondag 17:741798018c0d 29 DigitalOut ledg (LED_GREEN);
MarijkeZondag 13:a3d4b4daf5b4 30
MarijkeZondag 9:c722418997b5 31
MarijkeZondag 35:63c890ac71ff 32 MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off
MarijkeZondag 28:5e54cd4525de 33 QEI encoder2 (D9, D8, NC, 8400,QEI::X4_ENCODING);
MarijkeZondag 28:5e54cd4525de 34 QEI encoder1 (D12, D13, NC, 8400,QEI::X4_ENCODING);
MarijkeZondag 6:f4bbb73f3989 35
MarijkeZondag 35:63c890ac71ff 36 //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
vsluiter 0:c8f15874531b 37
MarijkeZondag 22:5d956c93b3ae 38 //Tickers
MarijkeZondag 28:5e54cd4525de 39 Ticker func_tick;
MarijkeZondag 28:5e54cd4525de 40 Ticker movag_tick;
MarijkeZondag 30:f04a35f2a06d 41 Ticker emg_tick;
MarijkeZondag 30:f04a35f2a06d 42 Ticker print_tick;
MarijkeZondag 30:f04a35f2a06d 43
MarijkeZondag 9:c722418997b5 44
MarijkeZondag 9:c722418997b5 45 //Global variables
MarijkeZondag 35:63c890ac71ff 46 const float T = 0.002f; //Ticker period EMG, engine control
MarijkeZondag 35:63c890ac71ff 47 const float T2 = 0.2f; //Ticker print function
MarijkeZondag 10:39ec51206c8b 48
MarijkeZondag 13:a3d4b4daf5b4 49 //EMG filter
MarijkeZondag 22:5d956c93b3ae 50 double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2
MarijkeZondag 22:5d956c93b3ae 51 double emg0_raw, emg1_raw, emg2_raw;
MarijkeZondag 22:5d956c93b3ae 52 double emg0_filt_x, emg1_filt_x, emg2_filt_x;
MarijkeZondag 35:63c890ac71ff 53 const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated
MarijkeZondag 13:a3d4b4daf5b4 54 double sum, sum1, sum2, sum3; //variables used to sum elements in array
MarijkeZondag 22:5d956c93b3ae 55 double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg
MarijkeZondag 35:63c890ac71ff 56 double movAg0, movAg1, movAg2; //Outcome of MovAg
MarijkeZondag 13:a3d4b4daf5b4 57
MarijkeZondag 22:5d956c93b3ae 58 //Calibration variables
MarijkeZondag 22:5d956c93b3ae 59 int x = -1; //Start switch, colour LED is blue.
MarijkeZondag 35:63c890ac71ff 60 int emg_cal = 0; //If emg_cal is set to 1, motors can begin to work in this code
MarijkeZondag 35:63c890ac71ff 61 const int sizeCal = 1500; //Size of the dataset used for calibration
MarijkeZondag 35:63c890ac71ff 62 double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //Arrays to put the dataset of the calibration in
MarijkeZondag 35:63c890ac71ff 63 double Mean0,Mean1,Mean2; //Average of maximum contraction
MarijkeZondag 22:5d956c93b3ae 64 double Threshold0, Threshold1, Threshold2;
MarijkeZondag 13:a3d4b4daf5b4 65
MarijkeZondag 35:63c890ac71ff 66 //Biquad //Variables for the biquad band filters
MarijkeZondag 22:5d956c93b3ae 67 BiQuadChain emg0filter;
MarijkeZondag 10:39ec51206c8b 68 BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 69 BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 70 BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 35:63c890ac71ff 71 BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients
MarijkeZondag 10:39ec51206c8b 72
MarijkeZondag 22:5d956c93b3ae 73 BiQuadChain emg1filter;
MarijkeZondag 10:39ec51206c8b 74 BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 75 BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 76 BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 35:63c890ac71ff 77 BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
MarijkeZondag 10:39ec51206c8b 78
MarijkeZondag 22:5d956c93b3ae 79 BiQuadChain emg2filter;
MarijkeZondag 10:39ec51206c8b 80 BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 81 BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 82 BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 35:63c890ac71ff 83 BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
MarijkeZondag 10:39ec51206c8b 84
Marle 25:bbef09ff226b 85
MarijkeZondag 26:ac5656aa35c7 86 //Variables PID controller
MarijkeZondag 35:63c890ac71ff 87 double PI = 3.14159; //Pi value
Marle 25:bbef09ff226b 88
MarijkeZondag 35:63c890ac71ff 89 double Kp1 = 20.0; //Proportional gain motor 1
MarijkeZondag 35:63c890ac71ff 90 double Ki1 = 1.02; //Integrative term motor 1
MarijkeZondag 35:63c890ac71ff 91 double Kd1 = 1.0; //Differential term motor 1
MarijkeZondag 35:63c890ac71ff 92 double encoder_radians1=0; //Inital encoder value motor 1
MarijkeZondag 35:63c890ac71ff 93 double err_integral1 = 0; //Initial error integral value motor 1
MarijkeZondag 35:63c890ac71ff 94 double err_prev1, err_prev2; //Variables called previous error motor 1 and motor 2
MarijkeZondag 35:63c890ac71ff 95 double err1, err2; //Variables called error motor 1 and motor 2
MarijkeZondag 35:63c890ac71ff 96 BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //Low_pass differential term Sample frequency 500 Hz, cutoff 20Hz low pass
MarijkeZondag 35:63c890ac71ff 97
MarijkeZondag 35:63c890ac71ff 98 double Kp2 = 20.0; / //Motor 2
MarijkeZondag 26:ac5656aa35c7 99 double Ki2 = 1.02;
MarijkeZondag 28:5e54cd4525de 100 double Kd2 = 1.0;
MarijkeZondag 26:ac5656aa35c7 101 double encoder_radians2=0;
MarijkeZondag 28:5e54cd4525de 102 double err_integral2 = 0;
MarijkeZondag 28:5e54cd4525de 103 double u1, u2;
MarijkeZondag 30:f04a35f2a06d 104 BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );
MarijkeZondag 26:ac5656aa35c7 105
MarijkeZondag 32:56a8bd82e971 106 // Inverse Kinematica variables
MarijkeZondag 35:63c890ac71ff 107 //const double L1 = 0.208; // Height table to joint 1
MarijkeZondag 35:63c890ac71ff 108 //const double L2 = 0.288; // Height table to joint 2
MarijkeZondag 35:63c890ac71ff 109 const double L3 = 0.212; // Length arm
MarijkeZondag 35:63c890ac71ff 110 //const double L4 = 0.089; // Distance backside base to joint 1
MarijkeZondag 35:63c890ac71ff 111 //const double L5 = 0.030; // Distance from joint 1 to joint 2
MarijkeZondag 35:63c890ac71ff 112 const double r_trans = 0.035; // Calculate translation to shaft rotation
MarijkeZondag 32:56a8bd82e971 113
MarijkeZondag 32:56a8bd82e971 114 // Variërende variabelen inverse kinematics:
MarijkeZondag 35:63c890ac71ff 115 double q1ref = 0.0; // Current motor angle of joint 1 as determined out of reference signal
MarijkeZondag 35:63c890ac71ff 116 double q2ref = 0.0; // Current motor angle of joint 2 as determined out of reference signal
MarijkeZondag 35:63c890ac71ff 117 double v_x; // Desired velocity of end effector in x direction --> Determined by EMG signals
MarijkeZondag 35:63c890ac71ff 118 double v_y; // Desired velocity of end effector in y direction --> Determined by EMG signals
MarijkeZondag 32:56a8bd82e971 119
MarijkeZondag 35:63c890ac71ff 120 //double Lq1; // Translational distance due to motor rotation joint 1
MarijkeZondag 35:63c890ac71ff 121 //double Cq2; // Joint angle of the system (corrected for gear ratio 1:5)
MarijkeZondag 32:56a8bd82e971 122
MarijkeZondag 35:63c890ac71ff 123 double q1_dot=0.0; // Required angular velocity of motor 1 to reach v_des
MarijkeZondag 35:63c890ac71ff 124 double q2_dot=0.0; // Required angular velocity of motor 2 to reach v_des
MarijkeZondag 32:56a8bd82e971 125
MarijkeZondag 35:63c890ac71ff 126 double q1_ii=0.0; // Reference signal for motorangle q1ref
MarijkeZondag 35:63c890ac71ff 127 double q2_ii=0.0; // Reference signal for motorangle q2ref
MarijkeZondag 32:56a8bd82e971 128
MarijkeZondag 32:56a8bd82e971 129 double q1_motor;
MarijkeZondag 32:56a8bd82e971 130 double q2_motor;
MarijkeZondag 28:5e54cd4525de 131
MarijkeZondag 26:ac5656aa35c7 132 //--------------Functions----------------------------------------------------------------------------------------------------------------------------//
MarijkeZondag 26:ac5656aa35c7 133
MarijkeZondag 26:ac5656aa35c7 134
MarijkeZondag 35:63c890ac71ff 135 //------------------ Filter EMG + Calibration EMG --------------------------------------------------------------------------------------------------//
MarijkeZondag 26:ac5656aa35c7 136
MarijkeZondag 13:a3d4b4daf5b4 137 void switch_to_calibrate()
MarijkeZondag 13:a3d4b4daf5b4 138 {
MarijkeZondag 35:63c890ac71ff 139 //Every time function gets called, x increases. Every button press --> new calibration state.
MarijkeZondag 35:63c890ac71ff 140 //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1.
MarijkeZondag 35:63c890ac71ff 141
MarijkeZondag 35:63c890ac71ff 142 x++;
MarijkeZondag 35:63c890ac71ff 143
MarijkeZondag 22:5d956c93b3ae 144
MarijkeZondag 35:63c890ac71ff 145 if(x==0) //If x = 0, led is red
MarijkeZondag 13:a3d4b4daf5b4 146 {
MarijkeZondag 17:741798018c0d 147 ledr = 0;
MarijkeZondag 17:741798018c0d 148 ledb = 1;
MarijkeZondag 17:741798018c0d 149 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 150 }
MarijkeZondag 35:63c890ac71ff 151 else if (x==1) //If x = 1, led is blue
MarijkeZondag 13:a3d4b4daf5b4 152 {
MarijkeZondag 17:741798018c0d 153 ledr = 1;
MarijkeZondag 17:741798018c0d 154 ledb = 0;
MarijkeZondag 17:741798018c0d 155 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 156 }
MarijkeZondag 35:63c890ac71ff 157 else if (x==2) //If x = 2, led is green
MarijkeZondag 13:a3d4b4daf5b4 158 {
MarijkeZondag 17:741798018c0d 159 ledr = 1;
MarijkeZondag 17:741798018c0d 160 ledb = 1;
MarijkeZondag 17:741798018c0d 161 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 162 }
MarijkeZondag 35:63c890ac71ff 163 else //If x = 3 or 4, led is white
MarijkeZondag 13:a3d4b4daf5b4 164 {
MarijkeZondag 17:741798018c0d 165 ledr = 0;
MarijkeZondag 17:741798018c0d 166 ledb = 0;
MarijkeZondag 17:741798018c0d 167 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 168 }
MarijkeZondag 13:a3d4b4daf5b4 169
MarijkeZondag 35:63c890ac71ff 170 if(x==4) //Reset back to x = -1
MarijkeZondag 13:a3d4b4daf5b4 171 {
MarijkeZondag 16:5f7196ddc77b 172 x = -1;
MarijkeZondag 35:63c890ac71ff 173 emg_cal=0; //Reset, motors off
MarijkeZondag 13:a3d4b4daf5b4 174 }
MarijkeZondag 13:a3d4b4daf5b4 175 }
MarijkeZondag 13:a3d4b4daf5b4 176
MarijkeZondag 13:a3d4b4daf5b4 177
MarijkeZondag 13:a3d4b4daf5b4 178 void calibrate(void)
MarijkeZondag 12:eaed305a76c3 179 {
MarijkeZondag 13:a3d4b4daf5b4 180 switch(x)
MarijkeZondag 13:a3d4b4daf5b4 181 {
MarijkeZondag 35:63c890ac71ff 182 case 0: //If calibration state 0:
MarijkeZondag 13:a3d4b4daf5b4 183 {
MarijkeZondag 13:a3d4b4daf5b4 184 sum = 0.0;
MarijkeZondag 35:63c890ac71ff 185 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0
MarijkeZondag 13:a3d4b4daf5b4 186 {
MarijkeZondag 22:5d956c93b3ae 187 StoreCal0[j] = emg0_filt;
MarijkeZondag 13:a3d4b4daf5b4 188 sum+=StoreCal0[j];
MarijkeZondag 35:63c890ac71ff 189 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 190 }
MarijkeZondag 35:63c890ac71ff 191 Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set
MarijkeZondag 35:63c890ac71ff 192 Threshold0 = Mean0*0.5; //Threshold calculation calve = 0.8*mean
MarijkeZondag 35:63c890ac71ff 193 break; //Stop. Threshold is calculated.
MarijkeZondag 13:a3d4b4daf5b4 194 }
MarijkeZondag 35:63c890ac71ff 195 case 1: //If calibration state 1:
MarijkeZondag 13:a3d4b4daf5b4 196 {
MarijkeZondag 22:5d956c93b3ae 197 sum = 0.0;
MarijkeZondag 35:63c890ac71ff 198 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1
MarijkeZondag 13:a3d4b4daf5b4 199 {
MarijkeZondag 22:5d956c93b3ae 200 StoreCal1[j] = emg1_filt;
MarijkeZondag 13:a3d4b4daf5b4 201 sum+=StoreCal1[j];
MarijkeZondag 21:1da43fdbd254 202 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 203 }
MarijkeZondag 13:a3d4b4daf5b4 204 Mean1 = sum/sizeCal;
MarijkeZondag 23:97a976a8f0fc 205 Threshold1 = Mean1/2;
MarijkeZondag 13:a3d4b4daf5b4 206 break;
MarijkeZondag 35:63c890ac71ff 207 }
MarijkeZondag 35:63c890ac71ff 208 case 2: //If calibration state 2:
MarijkeZondag 13:a3d4b4daf5b4 209 {
MarijkeZondag 13:a3d4b4daf5b4 210 sum = 0.0;
MarijkeZondag 35:63c890ac71ff 211 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2
MarijkeZondag 13:a3d4b4daf5b4 212 {
MarijkeZondag 23:97a976a8f0fc 213 StoreCal2[j] = emg2_filt;
MarijkeZondag 13:a3d4b4daf5b4 214 sum+=StoreCal2[j];
MarijkeZondag 21:1da43fdbd254 215 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 216 }
MarijkeZondag 13:a3d4b4daf5b4 217 Mean2 = sum/sizeCal;
MarijkeZondag 23:97a976a8f0fc 218 Threshold2 = Mean2/2;
MarijkeZondag 13:a3d4b4daf5b4 219 break;
MarijkeZondag 13:a3d4b4daf5b4 220 }
MarijkeZondag 35:63c890ac71ff 221 case 3: //EMG is calibrated, robot can be set to Home position.
MarijkeZondag 13:a3d4b4daf5b4 222 {
MarijkeZondag 35:63c890ac71ff 223 emg_cal = 1; //This is the setting for which the motors can begin turning in this code
MarijkeZondag 27:fa493551be99 224
MarijkeZondag 21:1da43fdbd254 225 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 226 break;
MarijkeZondag 13:a3d4b4daf5b4 227 }
MarijkeZondag 35:63c890ac71ff 228 default: //Ensures nothing happens if x is not 0,1 or 2
MarijkeZondag 13:a3d4b4daf5b4 229 {
MarijkeZondag 13:a3d4b4daf5b4 230 break;
MarijkeZondag 13:a3d4b4daf5b4 231 }
MarijkeZondag 13:a3d4b4daf5b4 232 }
MarijkeZondag 12:eaed305a76c3 233 }
MarijkeZondag 28:5e54cd4525de 234
MarijkeZondag 28:5e54cd4525de 235 void EMGFilter0()
MarijkeZondag 28:5e54cd4525de 236 {
MarijkeZondag 35:63c890ac71ff 237 emg0_raw = emg0_in.read(); //Give name to raw EMG0 data
MarijkeZondag 35:63c890ac71ff 238 emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 35:63c890ac71ff 239 emg0_filt = abs(emg0_filt_x); //Rectifier
MarijkeZondag 28:5e54cd4525de 240 }
MarijkeZondag 28:5e54cd4525de 241
MarijkeZondag 28:5e54cd4525de 242 void EMGFilter1()
MarijkeZondag 28:5e54cd4525de 243 {
MarijkeZondag 35:63c890ac71ff 244 emg1_raw = emg1_in.read(); //Give name to raw EMG1 data
MarijkeZondag 35:63c890ac71ff 245 emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 35:63c890ac71ff 246 emg1_filt = abs(emg1_filt_x); //Rectifier
MarijkeZondag 28:5e54cd4525de 247 }
MarijkeZondag 28:5e54cd4525de 248
MarijkeZondag 28:5e54cd4525de 249 void EMGFilter2()
MarijkeZondag 28:5e54cd4525de 250 {
MarijkeZondag 35:63c890ac71ff 251 emg2_raw = emg2_in.read(); //Give name to raw EMG1 data
MarijkeZondag 35:63c890ac71ff 252 emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 35:63c890ac71ff 253 emg2_filt = abs(emg2_filt_x); //Rectifier
MarijkeZondag 28:5e54cd4525de 254 }
MarijkeZondag 28:5e54cd4525de 255
MarijkeZondag 35:63c890ac71ff 256 void MovAg() //Calculate moving average (MovAg)
MarijkeZondag 28:5e54cd4525de 257 {
MarijkeZondag 35:63c890ac71ff 258 for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals
MarijkeZondag 28:5e54cd4525de 259 {
MarijkeZondag 35:63c890ac71ff 260 StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average.
MarijkeZondag 28:5e54cd4525de 261 StoreArray1[i] = StoreArray1[i-1];
MarijkeZondag 28:5e54cd4525de 262 StoreArray2[i] = StoreArray2[i-1];
MarijkeZondag 28:5e54cd4525de 263 }
MarijkeZondag 28:5e54cd4525de 264
MarijkeZondag 35:63c890ac71ff 265 StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array
MarijkeZondag 28:5e54cd4525de 266 StoreArray1[0] = emg1_filt;
MarijkeZondag 28:5e54cd4525de 267 StoreArray2[0] = emg2_filt;
MarijkeZondag 28:5e54cd4525de 268
MarijkeZondag 28:5e54cd4525de 269 sum1 = 0.0;
MarijkeZondag 28:5e54cd4525de 270 sum2 = 0.0;
MarijkeZondag 28:5e54cd4525de 271 sum3 = 0.0;
MarijkeZondag 28:5e54cd4525de 272
MarijkeZondag 35:63c890ac71ff 273 for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays
MarijkeZondag 28:5e54cd4525de 274 {
MarijkeZondag 28:5e54cd4525de 275 sum1 += StoreArray0[a];
MarijkeZondag 28:5e54cd4525de 276 sum2 += StoreArray1[a];
MarijkeZondag 28:5e54cd4525de 277 sum3 += StoreArray2[a];
MarijkeZondag 28:5e54cd4525de 278 }
MarijkeZondag 28:5e54cd4525de 279
MarijkeZondag 35:63c890ac71ff 280 movAg0 = sum1/windowsize; //calculates an average in the array
MarijkeZondag 28:5e54cd4525de 281 movAg1 = sum2/windowsize;
MarijkeZondag 28:5e54cd4525de 282 movAg2 = sum3/windowsize;
MarijkeZondag 28:5e54cd4525de 283 }
MarijkeZondag 28:5e54cd4525de 284
MarijkeZondag 35:63c890ac71ff 285 void emg_filtered() //Call all filter functions
MarijkeZondag 28:5e54cd4525de 286 {
MarijkeZondag 28:5e54cd4525de 287 EMGFilter0();
MarijkeZondag 28:5e54cd4525de 288 EMGFilter1();
MarijkeZondag 28:5e54cd4525de 289 EMGFilter2();
MarijkeZondag 28:5e54cd4525de 290 }
MarijkeZondag 28:5e54cd4525de 291
MarijkeZondag 35:63c890ac71ff 292 //---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------------------------------------------//
MarijkeZondag 28:5e54cd4525de 293 void PID_control1()
MarijkeZondag 28:5e54cd4525de 294 {
MarijkeZondag 28:5e54cd4525de 295 // Proportional part:
MarijkeZondag 35:63c890ac71ff 296 double u_k1 = Kp1 * err1; //Proportional gain times calculated error
MarijkeZondag 28:5e54cd4525de 297
MarijkeZondag 28:5e54cd4525de 298 //Integral part
MarijkeZondag 35:63c890ac71ff 299 err_integral1 = err_integral1 + err1 * T; //Adds the error*T
MarijkeZondag 35:63c890ac71ff 300 double u_i1 = Ki1 * err_integral1; //Integral term times the integral
MarijkeZondag 28:5e54cd4525de 301
MarijkeZondag 28:5e54cd4525de 302 // Derivative part
MarijkeZondag 35:63c890ac71ff 303 double err_derivative1 = (err1 - err_prev1)/T; //error - previous error /T
MarijkeZondag 35:63c890ac71ff 304 double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1); //Filter the derivative term for stabilization
MarijkeZondag 35:63c890ac71ff 305 double u_d1 = Kd1 * filtered_err_derivative1; //Derivative term times the derivative error
MarijkeZondag 35:63c890ac71ff 306 err_prev1 = err1; //Sets the current error to previous error (remember)
MarijkeZondag 28:5e54cd4525de 307
MarijkeZondag 35:63c890ac71ff 308 // Sum all parts and return it
MarijkeZondag 35:63c890ac71ff 309 u1 = u_k1 + u_i1 + u_d1;
MarijkeZondag 28:5e54cd4525de 310 }
MarijkeZondag 28:5e54cd4525de 311
MarijkeZondag 28:5e54cd4525de 312 void PID_control2()
MarijkeZondag 28:5e54cd4525de 313 {
MarijkeZondag 28:5e54cd4525de 314 // Proportional part:
MarijkeZondag 28:5e54cd4525de 315 double u_k2 = Kp2 * err2;
MarijkeZondag 28:5e54cd4525de 316
MarijkeZondag 28:5e54cd4525de 317 //Integral part
MarijkeZondag 28:5e54cd4525de 318 err_integral2 = err_integral2 + err2 * T;
MarijkeZondag 28:5e54cd4525de 319 double u_i2 = Ki2 * err_integral2;
MarijkeZondag 28:5e54cd4525de 320
MarijkeZondag 28:5e54cd4525de 321 // Derivative part
MarijkeZondag 28:5e54cd4525de 322 double err_derivative2 = (err2 - err_prev2)/T;
MarijkeZondag 28:5e54cd4525de 323 double filtered_err_derivative2 = LowPassFilterDer2.step(err_derivative2);
MarijkeZondag 28:5e54cd4525de 324 double u_d2 = Kd2 * filtered_err_derivative2;
MarijkeZondag 28:5e54cd4525de 325 err_prev2 = err2;
MarijkeZondag 28:5e54cd4525de 326
MarijkeZondag 35:63c890ac71ff 327 // Sum all parts and return it
MarijkeZondag 28:5e54cd4525de 328 u2 = u_k2 + u_i2 + u_d2;
MarijkeZondag 28:5e54cd4525de 329 }
MarijkeZondag 35:63c890ac71ff 330 void engine_control1() //Engine 1 is translational engine, connected with left side pins
MarijkeZondag 28:5e54cd4525de 331 {
MarijkeZondag 28:5e54cd4525de 332 encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0;
MarijkeZondag 35:63c890ac71ff 333 err1 = q1_motor - encoder_radians1; //Calculate error between desired angle 1 and current angle 1
MarijkeZondag 35:63c890ac71ff 334 PID_control1(); //PID 1 controller function call
MarijkeZondag 35:63c890ac71ff 335 pwmpin1 = fabs(u1); //Motor 1 speed set
MarijkeZondag 35:63c890ac71ff 336 directionpin1.write(u1<0); //Direction motor 1 set
MarijkeZondag 28:5e54cd4525de 337 }
MarijkeZondag 28:5e54cd4525de 338
MarijkeZondag 35:63c890ac71ff 339 void engine_control2() //Engine 2 is rotational engine, connected with right side wires
MarijkeZondag 28:5e54cd4525de 340 {
MarijkeZondag 28:5e54cd4525de 341 encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0;
MarijkeZondag 35:63c890ac71ff 342 err2 = q2_motor - encoder_radians2; //Calculate error between desired angle 2 and current angle 2
MarijkeZondag 35:63c890ac71ff 343 PID_control2(); //PID 2 controller function call
MarijkeZondag 35:63c890ac71ff 344 pwmpin2 = fabs(u2); //Motor 2 speed set
MarijkeZondag 35:63c890ac71ff 345 directionpin2.write(u2>0); //Direction motor 2 set
MarijkeZondag 28:5e54cd4525de 346 }
MarijkeZondag 28:5e54cd4525de 347
Esmee 33:976be2825a23 348
MarijkeZondag 35:63c890ac71ff 349 //------------------ Inversed Kinematics -----------------------------------------------------------------------------------------------------------//
MarijkeZondag 8:895d941a5910 350
Marle 25:bbef09ff226b 351 void inverse_kinematics()
Esmee 34:b8b18ba0c336 352 {
Marle 25:bbef09ff226b 353
MarijkeZondag 35:63c890ac71ff 354 q1_dot = (v_x*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref); //Calculate desired angular velocity of motor 1
MarijkeZondag 35:63c890ac71ff 355 q2_dot = v_y/(L3*cos(q2ref)); //Calculate desired angular velocity of motor 2
MarijkeZondag 35:63c890ac71ff 356
MarijkeZondag 35:63c890ac71ff 357 q1_ii = q1ref + q1_dot*T; //Adds the desired angle of motor 1 to the reference angle
MarijkeZondag 35:63c890ac71ff 358 q2_ii = q2ref + q2_dot*T; //Adds the desired angle of motor 2 to the reference angle
Marle 25:bbef09ff226b 359
MarijkeZondag 35:63c890ac71ff 360 q1ref = q1_ii; //Makes new qref
MarijkeZondag 35:63c890ac71ff 361 q2ref = q2_ii; //Makes new qref
MarijkeZondag 32:56a8bd82e971 362
MarijkeZondag 35:63c890ac71ff 363 q1_motor = -q1ref/r_trans; //Sets the angle at which motor 1 needs to go, with ratio rotation/translation
MarijkeZondag 35:63c890ac71ff 364 q2_motor = q2ref*5.0; //Sets the angle at which motor 2 needs to go, scaled by 5
MarijkeZondag 28:5e54cd4525de 365
MarijkeZondag 35:63c890ac71ff 366 engine_control1(); //Call engine_control 1 function
MarijkeZondag 35:63c890ac71ff 367 engine_control2(); //Call engine_control 2 function
Esmee 33:976be2825a23 368
Marle 25:bbef09ff226b 369 }
Marle 25:bbef09ff226b 370
Marle 25:bbef09ff226b 371 void v_des_calculate_qref()
MarijkeZondag 23:97a976a8f0fc 372 {
MarijkeZondag 35:63c890ac71ff 373 while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
MarijkeZondag 16:5f7196ddc77b 374 {
MarijkeZondag 35:63c890ac71ff 375 if(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0)
MarijkeZondag 16:5f7196ddc77b 376 {
MarijkeZondag 35:63c890ac71ff 377 v_x = 0.05; //Movement in +x direction
MarijkeZondag 28:5e54cd4525de 378 v_y = 0.0;
MarijkeZondag 28:5e54cd4525de 379
MarijkeZondag 35:63c890ac71ff 380 ledr = 0; //Led is red
Marle 25:bbef09ff226b 381 ledb = 1;
Marle 25:bbef09ff226b 382 ledg = 1;
MarijkeZondag 16:5f7196ddc77b 383 }
MarijkeZondag 35:63c890ac71ff 384 else if(movAg2>Threshold2 && movAg0<Threshold0) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is off (movAg0)
MarijkeZondag 16:5f7196ddc77b 385 {
MarijkeZondag 35:63c890ac71ff 386 v_y = 0.05; //Movement in +y direction
MarijkeZondag 28:5e54cd4525de 387 v_x = 0.0;
MarijkeZondag 28:5e54cd4525de 388
MarijkeZondag 35:63c890ac71ff 389 ledr = 1; //Led is green
Marle 25:bbef09ff226b 390 ledb = 1;
Marle 25:bbef09ff226b 391 ledg = 0;
MarijkeZondag 24:6d63ad38fef7 392 }
Marle 25:bbef09ff226b 393
MarijkeZondag 35:63c890ac71ff 394 else if(movAg0>Threshold0 && movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is on (movAg0)
MarijkeZondag 24:6d63ad38fef7 395 {
MarijkeZondag 35:63c890ac71ff 396 v_y = 0.0; //Movement in -x direction
Esmee 33:976be2825a23 397 v_x = -0.05;
MarijkeZondag 28:5e54cd4525de 398
MarijkeZondag 35:63c890ac71ff 399 ledr = 0; //Led is purple
Marle 25:bbef09ff226b 400 ledb = 0;
MarijkeZondag 30:f04a35f2a06d 401 ledg = 1;
MarijkeZondag 30:f04a35f2a06d 402 }
MarijkeZondag 28:5e54cd4525de 403
MarijkeZondag 35:63c890ac71ff 404 else if(movAg0>Threshold0 && movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is on (movAg0)
MarijkeZondag 30:f04a35f2a06d 405 {
MarijkeZondag 35:63c890ac71ff 406 v_y = -0.05; //Movement in -y direction
MarijkeZondag 30:f04a35f2a06d 407 v_x = 0.0;
MarijkeZondag 30:f04a35f2a06d 408
MarijkeZondag 35:63c890ac71ff 409 ledr = 1; //Led is blue
MarijkeZondag 30:f04a35f2a06d 410 ledb = 0;
MarijkeZondag 30:f04a35f2a06d 411 ledg = 1;
MarijkeZondag 30:f04a35f2a06d 412 }
MarijkeZondag 35:63c890ac71ff 413 else //If not higher than any threshold, motors will not turn at all
Marle 25:bbef09ff226b 414 {
Marle 25:bbef09ff226b 415 v_x = 0;
Marle 25:bbef09ff226b 416 v_y = 0;
MarijkeZondag 28:5e54cd4525de 417
MarijkeZondag 35:63c890ac71ff 418 ledr = 0; //Led is white
MarijkeZondag 24:6d63ad38fef7 419 ledb = 0;
MarijkeZondag 24:6d63ad38fef7 420 ledg = 0;
MarijkeZondag 24:6d63ad38fef7 421 }
MarijkeZondag 27:fa493551be99 422
MarijkeZondag 35:63c890ac71ff 423 inverse_kinematics(); //Call inverse kinematics function
MarijkeZondag 27:fa493551be99 424
MarijkeZondag 28:5e54cd4525de 425 break;
MarijkeZondag 23:97a976a8f0fc 426 }
MarijkeZondag 26:ac5656aa35c7 427 }
MarijkeZondag 26:ac5656aa35c7 428
MarijkeZondag 35:63c890ac71ff 429 //------------------ Start main function -----------------------------------------------------------------------------------------------------------//
MarijkeZondag 26:ac5656aa35c7 430
MarijkeZondag 23:97a976a8f0fc 431 int main()
MarijkeZondag 23:97a976a8f0fc 432 {
MarijkeZondag 35:63c890ac71ff 433 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 23:97a976a8f0fc 434
MarijkeZondag 35:63c890ac71ff 435 emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( &notch1 ); //Attach biquad elements to chain
MarijkeZondag 23:97a976a8f0fc 436 emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
MarijkeZondag 23:97a976a8f0fc 437 emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );
MarijkeZondag 10:39ec51206c8b 438
MarijkeZondag 35:63c890ac71ff 439 emg_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
MarijkeZondag 28:5e54cd4525de 440 movag_tick.attach(&MovAg,T);
MarijkeZondag 35:63c890ac71ff 441 func_tick.attach(&v_des_calculate_qref,T); //v_des determined every T
MarijkeZondag 28:5e54cd4525de 442
MarijkeZondag 35:63c890ac71ff 443 button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
MarijkeZondag 35:63c890ac71ff 444 button2.rise(calibrate); //Calibrate threshold for 3 muscles
MarijkeZondag 28:5e54cd4525de 445
MarijkeZondag 23:97a976a8f0fc 446 while(true)
MarijkeZondag 23:97a976a8f0fc 447 {
MarijkeZondag 30:f04a35f2a06d 448 ;
MarijkeZondag 23:97a976a8f0fc 449 }
vsluiter 0:c8f15874531b 450 }