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@30:f04a35f2a06d, 2018-11-01 (annotated)

Committer:
MarijkeZondag
Date:
Thu Nov 01 18:02:30 2018 +0000
Revision:
30:f04a35f2a06d
Parent:
29:6cd4f5ac57c4
Child:
31:0418ce58af56
Switch motor control aangepast, RKI klopt nog niet, Jorine zet het erbij.

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 23:97a976a8f0fc 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 23:97a976a8f0fc 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 30:f04a35f2a06d 46 const float T = 0.002f; //Ticker period EMG, engine control
MarijkeZondag 30:f04a35f2a06d 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 22:5d956c93b3ae 53 const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number)
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 22:5d956c93b3ae 56 double movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??)
MarijkeZondag 13:a3d4b4daf5b4 57
MarijkeZondag 22:5d956c93b3ae 58 //Calibration variables
MarijkeZondag 22:5d956c93b3ae 59 int x = -1; //Start switch, colour LED is blue.
MarijkeZondag 22:5d956c93b3ae 60 int emg_cal = 0; //if emg_cal is set to 1, motors can begin to work in this code (!!)
MarijkeZondag 22:5d956c93b3ae 61 const int sizeCal = 1500; //size of the dataset used for calibration, eerst 2000
MarijkeZondag 22:5d956c93b3ae 62 double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //arrays to put the dataset of the calibration in
MarijkeZondag 22:5d956c93b3ae 63 double Mean0,Mean1,Mean2; //average of maximum tightening
MarijkeZondag 22:5d956c93b3ae 64 double Threshold0, Threshold1, Threshold2;
MarijkeZondag 13:a3d4b4daf5b4 65
MarijkeZondag 22:5d956c93b3ae 66 //Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo)
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 22:5d956c93b3ae 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 22:5d956c93b3ae 77 BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
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 14:fa09dae67390 83 BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 84
MarijkeZondag 28:5e54cd4525de 85 //Global variables
MarijkeZondag 28:5e54cd4525de 86 const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders???
MarijkeZondag 28:5e54cd4525de 87 const float T2 = 0.002f;
MarijkeZondag 28:5e54cd4525de 88
MarijkeZondag 26:ac5656aa35c7 89 // Inverse Kinematica variables
Marle 25:bbef09ff226b 90 const double L1 = 0.208; // Hoogte van tafel tot joint 1
MarijkeZondag 28:5e54cd4525de 91 //const double L2 = 0.288; // Hoogte van tafel tot joint 2
Marle 25:bbef09ff226b 92 const double L3 = 0.212; // Lengte van de arm
Marle 25:bbef09ff226b 93 const double L4 = 0.089; // Afstand van achterkant base tot joint 1
MarijkeZondag 28:5e54cd4525de 94 //const double L5 = 0.030; // Afstand van joint 1 naar joint 2
Marle 25:bbef09ff226b 95 const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation
Marle 25:bbef09ff226b 96
Marle 25:bbef09ff226b 97 // Variërende variabelen inverse kinematics:
MarijkeZondag 28:5e54cd4525de 98 double q1ref = 0.0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
MarijkeZondag 28:5e54cd4525de 99 double q2ref = 0.0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
Marle 25:bbef09ff226b 100 double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals
Marle 25:bbef09ff226b 101 double v_y; // Desired velocity end effector in y direction --> Determined by EMG signals
Marle 25:bbef09ff226b 102
Marle 25:bbef09ff226b 103 double Lq1; // Translatieafstand als gevolg van motor rotation joint 1
Marle 25:bbef09ff226b 104 double Cq2; // Joint angle of the system (corrected for gear ratio 1:5)
Marle 25:bbef09ff226b 105
MarijkeZondag 28:5e54cd4525de 106 double q1_dot=0.0; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
MarijkeZondag 28:5e54cd4525de 107 double q2_dot=0.0; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken
Marle 25:bbef09ff226b 108
MarijkeZondag 28:5e54cd4525de 109 double q1_ii=0.0; // Reference signal for motorangle q1ref
MarijkeZondag 28:5e54cd4525de 110 double q2_ii=0.0; // Reference signal for motorangle q2ref
Marle 25:bbef09ff226b 111
MarijkeZondag 26:ac5656aa35c7 112 //Variables PID controller
MarijkeZondag 26:ac5656aa35c7 113 double PI = 3.14159;
MarijkeZondag 30:f04a35f2a06d 114 double Kp1 = 20.0; //Motor 1
MarijkeZondag 26:ac5656aa35c7 115 double Ki1 = 1.02;
MarijkeZondag 28:5e54cd4525de 116 double Kd1 = 1.0;
MarijkeZondag 26:ac5656aa35c7 117 double encoder_radians1=0;
MarijkeZondag 28:5e54cd4525de 118 double err_integral1 = 0;
MarijkeZondag 28:5e54cd4525de 119 double err_prev1, err_prev2;
MarijkeZondag 28:5e54cd4525de 120 double err1, err2;
MarijkeZondag 30:f04a35f2a06d 121 BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //sample frequency 500 Hz, cutoff 20Hz low pass
Marle 25:bbef09ff226b 122
MarijkeZondag 30:f04a35f2a06d 123 double Kp2 = 20.0; //Motor 2
MarijkeZondag 26:ac5656aa35c7 124 double Ki2 = 1.02;
MarijkeZondag 28:5e54cd4525de 125 double Kd2 = 1.0;
MarijkeZondag 26:ac5656aa35c7 126 double encoder_radians2=0;
MarijkeZondag 28:5e54cd4525de 127 double err_integral2 = 0;
MarijkeZondag 28:5e54cd4525de 128 double u1, u2;
MarijkeZondag 30:f04a35f2a06d 129 BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );
MarijkeZondag 26:ac5656aa35c7 130
MarijkeZondag 28:5e54cd4525de 131
MarijkeZondag 28:5e54cd4525de 132 int start_control = 0;
MarijkeZondag 28:5e54cd4525de 133 double emg_cal = 1;
MarijkeZondag 27:fa493551be99 134
MarijkeZondag 26:ac5656aa35c7 135
MarijkeZondag 26:ac5656aa35c7 136 //--------------Functions----------------------------------------------------------------------------------------------------------------------------//
MarijkeZondag 26:ac5656aa35c7 137
MarijkeZondag 26:ac5656aa35c7 138
MarijkeZondag 26:ac5656aa35c7 139 //------------------ Filter EMG + Calibration EMG --------------------------------//
MarijkeZondag 26:ac5656aa35c7 140
MarijkeZondag 13:a3d4b4daf5b4 141 void switch_to_calibrate()
MarijkeZondag 13:a3d4b4daf5b4 142 {
MarijkeZondag 22:5d956c93b3ae 143 x++; //Every time function gets called, x increases. Every button press --> new calibration state.
MarijkeZondag 22:5d956c93b3ae 144 //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1.
MarijkeZondag 22:5d956c93b3ae 145
MarijkeZondag 13:a3d4b4daf5b4 146 if(x==0) //If x = 0, led is red
MarijkeZondag 13:a3d4b4daf5b4 147 {
MarijkeZondag 17:741798018c0d 148 ledr = 0;
MarijkeZondag 17:741798018c0d 149 ledb = 1;
MarijkeZondag 17:741798018c0d 150 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 151 }
MarijkeZondag 13:a3d4b4daf5b4 152 else if (x==1) //If x = 1, led is blue
MarijkeZondag 13:a3d4b4daf5b4 153 {
MarijkeZondag 17:741798018c0d 154 ledr = 1;
MarijkeZondag 17:741798018c0d 155 ledb = 0;
MarijkeZondag 17:741798018c0d 156 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 157 }
MarijkeZondag 22:5d956c93b3ae 158 else if (x==2) //If x = 2, led is green
MarijkeZondag 13:a3d4b4daf5b4 159 {
MarijkeZondag 17:741798018c0d 160 ledr = 1;
MarijkeZondag 17:741798018c0d 161 ledb = 1;
MarijkeZondag 17:741798018c0d 162 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 163 }
MarijkeZondag 22:5d956c93b3ae 164 else //If x = 3 or 4, led is white
MarijkeZondag 13:a3d4b4daf5b4 165 {
MarijkeZondag 17:741798018c0d 166 ledr = 0;
MarijkeZondag 17:741798018c0d 167 ledb = 0;
MarijkeZondag 17:741798018c0d 168 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 169 }
MarijkeZondag 13:a3d4b4daf5b4 170
MarijkeZondag 22:5d956c93b3ae 171 if(x==4) //Reset back to x = -1
MarijkeZondag 13:a3d4b4daf5b4 172 {
MarijkeZondag 16:5f7196ddc77b 173 x = -1;
MarijkeZondag 23:97a976a8f0fc 174 emg_cal=0; //reset, motors off
MarijkeZondag 13:a3d4b4daf5b4 175 }
MarijkeZondag 13:a3d4b4daf5b4 176 }
MarijkeZondag 13:a3d4b4daf5b4 177
MarijkeZondag 13:a3d4b4daf5b4 178
MarijkeZondag 13:a3d4b4daf5b4 179 void calibrate(void)
MarijkeZondag 12:eaed305a76c3 180 {
MarijkeZondag 13:a3d4b4daf5b4 181 switch(x)
MarijkeZondag 13:a3d4b4daf5b4 182 {
MarijkeZondag 22:5d956c93b3ae 183 case 0: //If calibration state 0:
MarijkeZondag 13:a3d4b4daf5b4 184 {
MarijkeZondag 13:a3d4b4daf5b4 185 sum = 0.0;
MarijkeZondag 22:5d956c93b3ae 186 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0
MarijkeZondag 13:a3d4b4daf5b4 187 {
MarijkeZondag 22:5d956c93b3ae 188 StoreCal0[j] = emg0_filt;
MarijkeZondag 13:a3d4b4daf5b4 189 sum+=StoreCal0[j];
MarijkeZondag 22:5d956c93b3ae 190 wait(0.001f); //Does there need to be a wait?
MarijkeZondag 13:a3d4b4daf5b4 191 }
MarijkeZondag 22:5d956c93b3ae 192 Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples)
MarijkeZondag 23:97a976a8f0fc 193 Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean
MarijkeZondag 22:5d956c93b3ae 194 break; //Stop. Threshold is calculated, we will use this further in the code
MarijkeZondag 13:a3d4b4daf5b4 195 }
MarijkeZondag 22:5d956c93b3ae 196 case 1: //If calibration state 1:
MarijkeZondag 13:a3d4b4daf5b4 197 {
MarijkeZondag 22:5d956c93b3ae 198 sum = 0.0;
MarijkeZondag 22:5d956c93b3ae 199 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1
MarijkeZondag 13:a3d4b4daf5b4 200 {
MarijkeZondag 22:5d956c93b3ae 201 StoreCal1[j] = emg1_filt;
MarijkeZondag 13:a3d4b4daf5b4 202 sum+=StoreCal1[j];
MarijkeZondag 21:1da43fdbd254 203 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 204 }
MarijkeZondag 13:a3d4b4daf5b4 205 Mean1 = sum/sizeCal;
MarijkeZondag 23:97a976a8f0fc 206 Threshold1 = Mean1/2;
MarijkeZondag 13:a3d4b4daf5b4 207 break;
MarijkeZondag 13:a3d4b4daf5b4 208 }
MarijkeZondag 22:5d956c93b3ae 209 case 2: //If calibration state 2:
MarijkeZondag 13:a3d4b4daf5b4 210 {
MarijkeZondag 13:a3d4b4daf5b4 211 sum = 0.0;
MarijkeZondag 22:5d956c93b3ae 212 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2
MarijkeZondag 13:a3d4b4daf5b4 213 {
MarijkeZondag 23:97a976a8f0fc 214 StoreCal2[j] = emg2_filt;
MarijkeZondag 13:a3d4b4daf5b4 215 sum+=StoreCal2[j];
MarijkeZondag 21:1da43fdbd254 216 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 217 }
MarijkeZondag 13:a3d4b4daf5b4 218 Mean2 = sum/sizeCal;
MarijkeZondag 23:97a976a8f0fc 219 Threshold2 = Mean2/2;
MarijkeZondag 13:a3d4b4daf5b4 220 break;
MarijkeZondag 13:a3d4b4daf5b4 221 }
MarijkeZondag 22:5d956c93b3ae 222 case 3: //EMG is calibrated, robot can be set to Home position.
MarijkeZondag 13:a3d4b4daf5b4 223 {
MarijkeZondag 22:5d956c93b3ae 224 emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!)
MarijkeZondag 27:fa493551be99 225
MarijkeZondag 21:1da43fdbd254 226 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 227 break;
MarijkeZondag 13:a3d4b4daf5b4 228 }
MarijkeZondag 22:5d956c93b3ae 229 default: //Ensures nothing happens if x is not 0,1 or 2.
MarijkeZondag 13:a3d4b4daf5b4 230 {
MarijkeZondag 13:a3d4b4daf5b4 231 break;
MarijkeZondag 13:a3d4b4daf5b4 232 }
MarijkeZondag 13:a3d4b4daf5b4 233 }
MarijkeZondag 12:eaed305a76c3 234 }
MarijkeZondag 28:5e54cd4525de 235
MarijkeZondag 28:5e54cd4525de 236 void EMGFilter0()
MarijkeZondag 28:5e54cd4525de 237 {
MarijkeZondag 28:5e54cd4525de 238 emg0_raw = emg0_in.read(); //give name to raw EMG0 data
MarijkeZondag 28:5e54cd4525de 239 emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 28:5e54cd4525de 240 emg0_filt = abs(emg0_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken.
MarijkeZondag 28:5e54cd4525de 241 }
MarijkeZondag 28:5e54cd4525de 242
MarijkeZondag 28:5e54cd4525de 243 void EMGFilter1()
MarijkeZondag 28:5e54cd4525de 244 {
MarijkeZondag 28:5e54cd4525de 245 emg1_raw = emg1_in.read(); //give name to raw EMG1 data
MarijkeZondag 28:5e54cd4525de 246 emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 28:5e54cd4525de 247 emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
MarijkeZondag 28:5e54cd4525de 248 }
MarijkeZondag 28:5e54cd4525de 249
MarijkeZondag 28:5e54cd4525de 250 void EMGFilter2()
MarijkeZondag 28:5e54cd4525de 251 {
MarijkeZondag 28:5e54cd4525de 252 emg2_raw = emg2_in.read(); //Give name to raw EMG1 data
MarijkeZondag 28:5e54cd4525de 253 emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 28:5e54cd4525de 254 emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier.
MarijkeZondag 28:5e54cd4525de 255 }
MarijkeZondag 28:5e54cd4525de 256
MarijkeZondag 28:5e54cd4525de 257 void MovAg() //Calculate moving average (MovAg), klopt nog niet!!
MarijkeZondag 28:5e54cd4525de 258 {
MarijkeZondag 28:5e54cd4525de 259 for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals
MarijkeZondag 28:5e54cd4525de 260 {
MarijkeZondag 28:5e54cd4525de 261 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 262 StoreArray1[i] = StoreArray1[i-1];
MarijkeZondag 28:5e54cd4525de 263 StoreArray2[i] = StoreArray2[i-1];
MarijkeZondag 28:5e54cd4525de 264 }
MarijkeZondag 28:5e54cd4525de 265
MarijkeZondag 28:5e54cd4525de 266 StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array
MarijkeZondag 28:5e54cd4525de 267 StoreArray1[0] = emg1_filt;
MarijkeZondag 28:5e54cd4525de 268 StoreArray2[0] = emg2_filt;
MarijkeZondag 28:5e54cd4525de 269
MarijkeZondag 28:5e54cd4525de 270 sum1 = 0.0;
MarijkeZondag 28:5e54cd4525de 271 sum2 = 0.0;
MarijkeZondag 28:5e54cd4525de 272 sum3 = 0.0;
MarijkeZondag 28:5e54cd4525de 273
MarijkeZondag 28:5e54cd4525de 274 for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays
MarijkeZondag 28:5e54cd4525de 275 {
MarijkeZondag 28:5e54cd4525de 276 sum1 += StoreArray0[a];
MarijkeZondag 28:5e54cd4525de 277 sum2 += StoreArray1[a];
MarijkeZondag 28:5e54cd4525de 278 sum3 += StoreArray2[a];
MarijkeZondag 28:5e54cd4525de 279 }
MarijkeZondag 28:5e54cd4525de 280
MarijkeZondag 28:5e54cd4525de 281 movAg0 = sum1/windowsize; //calculates an average in the array
MarijkeZondag 28:5e54cd4525de 282 movAg1 = sum2/windowsize;
MarijkeZondag 28:5e54cd4525de 283 movAg2 = sum3/windowsize;
MarijkeZondag 28:5e54cd4525de 284 //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout.
MarijkeZondag 28:5e54cd4525de 285 }
MarijkeZondag 28:5e54cd4525de 286
MarijkeZondag 28:5e54cd4525de 287 void emg_filtered() //Call all filter functions
MarijkeZondag 28:5e54cd4525de 288 {
MarijkeZondag 28:5e54cd4525de 289 EMGFilter0();
MarijkeZondag 28:5e54cd4525de 290 EMGFilter1();
MarijkeZondag 28:5e54cd4525de 291 EMGFilter2();
MarijkeZondag 28:5e54cd4525de 292 }
MarijkeZondag 28:5e54cd4525de 293
MarijkeZondag 26:ac5656aa35c7 294 /*
MarijkeZondag 22:5d956c93b3ae 295 void HIDScope_sample()
MarijkeZondag 22:5d956c93b3ae 296 {
MarijkeZondag 26:ac5656aa35c7 297 scope.set(0,emg0_raw);
MarijkeZondag 26:ac5656aa35c7 298 scope.set(1,emg0_filt);
MarijkeZondag 26:ac5656aa35c7 299 scope.set(1,movAg0); //als moving average werkt
MarijkeZondag 26:ac5656aa35c7 300 scope.set(2,emg1_raw);
MarijkeZondag 26:ac5656aa35c7 301 scope.set(3,emg1_filt);
MarijkeZondag 26:ac5656aa35c7 302 scope.set(3,movAg1); //als moving average werkt
MarijkeZondag 26:ac5656aa35c7 303 scope.set(4,emg2_raw);
MarijkeZondag 26:ac5656aa35c7 304 scope.set(5,emg2_filt);
MarijkeZondag 26:ac5656aa35c7 305 scope.set(5,movAg2); //als moving average werkt
MarijkeZondag 22:5d956c93b3ae 306
MarijkeZondag 26:ac5656aa35c7 307 scope.send(); //Send data to HIDScope server
MarijkeZondag 22:5d956c93b3ae 308 }
MarijkeZondag 26:ac5656aa35c7 309 */
MarijkeZondag 8:895d941a5910 310
MarijkeZondag 28:5e54cd4525de 311
MarijkeZondag 28:5e54cd4525de 312 //---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------//
MarijkeZondag 28:5e54cd4525de 313 void PID_control1()
MarijkeZondag 28:5e54cd4525de 314 {
MarijkeZondag 28:5e54cd4525de 315 //pc.printf("ik doe het, PDI \n\r");
MarijkeZondag 28:5e54cd4525de 316
MarijkeZondag 28:5e54cd4525de 317 // Proportional part:
MarijkeZondag 28:5e54cd4525de 318 double u_k1 = Kp1 * err1;
MarijkeZondag 28:5e54cd4525de 319
MarijkeZondag 28:5e54cd4525de 320 //Integral part
MarijkeZondag 28:5e54cd4525de 321 err_integral1 = err_integral1 + err1 * T;
MarijkeZondag 28:5e54cd4525de 322 double u_i1 = Ki1 * err_integral1;
MarijkeZondag 28:5e54cd4525de 323
MarijkeZondag 28:5e54cd4525de 324 // Derivative part
MarijkeZondag 28:5e54cd4525de 325 double err_derivative1 = (err1 - err_prev1)/T;
MarijkeZondag 28:5e54cd4525de 326 double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1);
MarijkeZondag 28:5e54cd4525de 327 double u_d1 = Kd1 * filtered_err_derivative1;
MarijkeZondag 28:5e54cd4525de 328 err_prev1 = err1;
MarijkeZondag 28:5e54cd4525de 329
MarijkeZondag 28:5e54cd4525de 330
MarijkeZondag 28:5e54cd4525de 331 // Sum all parts and return it
MarijkeZondag 28:5e54cd4525de 332 u1 = u_k1 + u_i1 + u_d1;
MarijkeZondag 28:5e54cd4525de 333 }
MarijkeZondag 28:5e54cd4525de 334
MarijkeZondag 28:5e54cd4525de 335 void PID_control2()
MarijkeZondag 28:5e54cd4525de 336 {
MarijkeZondag 28:5e54cd4525de 337 //pc.printf("ik doe het, PDI \n\r");
MarijkeZondag 28:5e54cd4525de 338
MarijkeZondag 28:5e54cd4525de 339 // Proportional part:
MarijkeZondag 28:5e54cd4525de 340 double u_k2 = Kp2 * err2;
MarijkeZondag 28:5e54cd4525de 341
MarijkeZondag 28:5e54cd4525de 342 //Integral part
MarijkeZondag 28:5e54cd4525de 343 err_integral2 = err_integral2 + err2 * T;
MarijkeZondag 28:5e54cd4525de 344 double u_i2 = Ki2 * err_integral2;
MarijkeZondag 28:5e54cd4525de 345
MarijkeZondag 28:5e54cd4525de 346 // Derivative part
MarijkeZondag 28:5e54cd4525de 347 double err_derivative2 = (err2 - err_prev2)/T;
MarijkeZondag 28:5e54cd4525de 348 double filtered_err_derivative2 = LowPassFilterDer2.step(err_derivative2);
MarijkeZondag 28:5e54cd4525de 349 double u_d2 = Kd2 * filtered_err_derivative2;
MarijkeZondag 28:5e54cd4525de 350 err_prev2 = err2;
MarijkeZondag 28:5e54cd4525de 351
MarijkeZondag 28:5e54cd4525de 352
MarijkeZondag 28:5e54cd4525de 353 // Sum all parts and return it
MarijkeZondag 28:5e54cd4525de 354 u2 = u_k2 + u_i2 + u_d2;
MarijkeZondag 28:5e54cd4525de 355 }
MarijkeZondag 28:5e54cd4525de 356
MarijkeZondag 28:5e54cd4525de 357 void engine_control1() //Engine 1 is translational engine, connected with left side pins
MarijkeZondag 28:5e54cd4525de 358 {
MarijkeZondag 28:5e54cd4525de 359 //pc.printf("ik doe het, engine control 1\n\r");
MarijkeZondag 28:5e54cd4525de 360 encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0;
MarijkeZondag 28:5e54cd4525de 361 //pc.printf("encoder1 %f \n\r", (float)encoder1.getPulses());
MarijkeZondag 28:5e54cd4525de 362 //pc.printf("encoder_radians1 %f \n\r",(float) encoder_radians1);
MarijkeZondag 28:5e54cd4525de 363 err1 = q1ref - encoder_radians1;
MarijkeZondag 28:5e54cd4525de 364 //pc.printf("err1 = %f\n\r",err1);
MarijkeZondag 28:5e54cd4525de 365 PID_control1(); //PID controller function call
MarijkeZondag 28:5e54cd4525de 366
MarijkeZondag 28:5e54cd4525de 367 //pc.printf("u1 = %f\n\r",u1);
MarijkeZondag 28:5e54cd4525de 368
MarijkeZondag 28:5e54cd4525de 369 //if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600
MarijkeZondag 28:5e54cd4525de 370 //{
MarijkeZondag 28:5e54cd4525de 371 pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
MarijkeZondag 28:5e54cd4525de 372 directionpin1.write(u1<0);
MarijkeZondag 28:5e54cd4525de 373 //}
MarijkeZondag 28:5e54cd4525de 374 //else
MarijkeZondag 28:5e54cd4525de 375 // {
MarijkeZondag 28:5e54cd4525de 376 // pwmpin1 = 0;
MarijkeZondag 28:5e54cd4525de 377 // }
MarijkeZondag 28:5e54cd4525de 378 }
MarijkeZondag 28:5e54cd4525de 379
MarijkeZondag 28:5e54cd4525de 380 void engine_control2() //Engine 2 is rotational engine, connected with right side wires
MarijkeZondag 28:5e54cd4525de 381 {
MarijkeZondag 28:5e54cd4525de 382 encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0;
MarijkeZondag 28:5e54cd4525de 383 //pc.printf("encoder2 %f \n\r",(float)encoder2.getPulses());
MarijkeZondag 28:5e54cd4525de 384 //pc.printf("encoder_radians2 %f \n\r",(float)encoder_radians2);
MarijkeZondag 28:5e54cd4525de 385 err2 = q2ref - encoder_radians2;
MarijkeZondag 28:5e54cd4525de 386 //pc.printf("err2 = %f\n\r",err2);
MarijkeZondag 28:5e54cd4525de 387 PID_control2(); //PID controller function call
MarijkeZondag 28:5e54cd4525de 388 //pc.printf("u2 = %f\n\r",u2);
MarijkeZondag 28:5e54cd4525de 389
MarijkeZondag 28:5e54cd4525de 390 //if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250) //limits rotation, in counts
MarijkeZondag 28:5e54cd4525de 391 // {
MarijkeZondag 28:5e54cd4525de 392 pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
MarijkeZondag 28:5e54cd4525de 393 directionpin2.write(u2>0);
MarijkeZondag 28:5e54cd4525de 394 // }
MarijkeZondag 28:5e54cd4525de 395 //else
MarijkeZondag 28:5e54cd4525de 396 // {
MarijkeZondag 28:5e54cd4525de 397 // pwmpin2 = 0;
MarijkeZondag 28:5e54cd4525de 398 // }
MarijkeZondag 28:5e54cd4525de 399 }
MarijkeZondag 28:5e54cd4525de 400
MarijkeZondag 26:ac5656aa35c7 401 //------------------ Inversed Kinematics --------------------------------//
MarijkeZondag 8:895d941a5910 402
Marle 25:bbef09ff226b 403 void inverse_kinematics()
Marle 25:bbef09ff226b 404 {
MarijkeZondag 28:5e54cd4525de 405
MarijkeZondag 28:5e54cd4525de 406 //pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y);
MarijkeZondag 28:5e54cd4525de 407
Marle 25:bbef09ff226b 408 Lq1 = q1ref*r_trans;
Marle 25:bbef09ff226b 409 Cq2 = q2ref/5.0;
Marle 25:bbef09ff226b 410
MarijkeZondag 28:5e54cd4525de 411 q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2)); //RKI systeem
MarijkeZondag 28:5e54cd4525de 412 q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2)); //
Marle 25:bbef09ff226b 413
MarijkeZondag 28:5e54cd4525de 414 q1_ii = q1ref + (q1_dot/r_trans)*T; //Omgezet naar motorhoeken
MarijkeZondag 28:5e54cd4525de 415 q2_ii = q2ref + (q2_dot*5.0)*T;
Marle 25:bbef09ff226b 416
Marle 25:bbef09ff226b 417 q1ref = q1_ii;
Marle 25:bbef09ff226b 418 q2ref = q2_ii;
MarijkeZondag 27:fa493551be99 419
MarijkeZondag 28:5e54cd4525de 420
MarijkeZondag 28:5e54cd4525de 421 //pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref);
MarijkeZondag 28:5e54cd4525de 422
MarijkeZondag 28:5e54cd4525de 423
MarijkeZondag 28:5e54cd4525de 424 //start_control = 1;
MarijkeZondag 28:5e54cd4525de 425 engine_control1();
MarijkeZondag 28:5e54cd4525de 426 engine_control2();
Marle 25:bbef09ff226b 427 }
Marle 25:bbef09ff226b 428
Marle 25:bbef09ff226b 429 void v_des_calculate_qref()
MarijkeZondag 23:97a976a8f0fc 430 {
MarijkeZondag 30:f04a35f2a06d 431 while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
MarijkeZondag 16:5f7196ddc77b 432 {
MarijkeZondag 30:f04a35f2a06d 433 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 434 {
MarijkeZondag 30:f04a35f2a06d 435 v_x = 0.5; //movement in +x direction
MarijkeZondag 28:5e54cd4525de 436 v_y = 0.0;
MarijkeZondag 28:5e54cd4525de 437
MarijkeZondag 30:f04a35f2a06d 438 ledr = 0; //red
Marle 25:bbef09ff226b 439 ledb = 1;
Marle 25:bbef09ff226b 440 ledg = 1;
MarijkeZondag 16:5f7196ddc77b 441 }
MarijkeZondag 30:f04a35f2a06d 442 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 443 {
MarijkeZondag 30:f04a35f2a06d 444 v_y = 0.5; //Movement in +y direction
MarijkeZondag 28:5e54cd4525de 445 v_x = 0.0;
MarijkeZondag 28:5e54cd4525de 446
MarijkeZondag 30:f04a35f2a06d 447 ledr = 1; //Green
Marle 25:bbef09ff226b 448 ledb = 1;
Marle 25:bbef09ff226b 449 ledg = 0;
MarijkeZondag 24:6d63ad38fef7 450 }
Marle 25:bbef09ff226b 451
MarijkeZondag 30:f04a35f2a06d 452 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 453 {
MarijkeZondag 30:f04a35f2a06d 454 v_y = 0.0; //Movement in -x direction
MarijkeZondag 30:f04a35f2a06d 455 v_x = -0.5;
MarijkeZondag 28:5e54cd4525de 456
MarijkeZondag 30:f04a35f2a06d 457 ledr = 0; //Purple
Marle 25:bbef09ff226b 458 ledb = 0;
MarijkeZondag 30:f04a35f2a06d 459 ledg = 1;
MarijkeZondag 30:f04a35f2a06d 460 }
MarijkeZondag 28:5e54cd4525de 461
MarijkeZondag 30:f04a35f2a06d 462 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 463 {
MarijkeZondag 30:f04a35f2a06d 464 v_y = -0.5; //Movement in -y direction
MarijkeZondag 30:f04a35f2a06d 465 v_x = 0.0;
MarijkeZondag 30:f04a35f2a06d 466
MarijkeZondag 30:f04a35f2a06d 467 ledr = 1; //Blue
MarijkeZondag 30:f04a35f2a06d 468 ledb = 0;
MarijkeZondag 30:f04a35f2a06d 469 ledg = 1;
MarijkeZondag 30:f04a35f2a06d 470 }
MarijkeZondag 30:f04a35f2a06d 471 else //If not higher than any threshold, motors will not turn at all
Marle 25:bbef09ff226b 472 {
Marle 25:bbef09ff226b 473 v_x = 0;
Marle 25:bbef09ff226b 474 v_y = 0;
MarijkeZondag 28:5e54cd4525de 475
MarijkeZondag 30:f04a35f2a06d 476 ledr = 0; //White
MarijkeZondag 24:6d63ad38fef7 477 ledb = 0;
MarijkeZondag 24:6d63ad38fef7 478 ledg = 0;
MarijkeZondag 24:6d63ad38fef7 479 }
MarijkeZondag 27:fa493551be99 480
MarijkeZondag 30:f04a35f2a06d 481 inverse_kinematics(); //Call inverse kinematics function
MarijkeZondag 27:fa493551be99 482
MarijkeZondag 28:5e54cd4525de 483 break;
MarijkeZondag 23:97a976a8f0fc 484 }
MarijkeZondag 26:ac5656aa35c7 485 }
MarijkeZondag 26:ac5656aa35c7 486
MarijkeZondag 30:f04a35f2a06d 487 void printFunction()
MarijkeZondag 30:f04a35f2a06d 488 {
MarijkeZondag 30:f04a35f2a06d 489 pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2);
MarijkeZondag 30:f04a35f2a06d 490 pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
MarijkeZondag 30:f04a35f2a06d 491 }
MarijkeZondag 30:f04a35f2a06d 492
MarijkeZondag 26:ac5656aa35c7 493
MarijkeZondag 26:ac5656aa35c7 494
MarijkeZondag 26:ac5656aa35c7 495 //------------------ Start main function --------------------------//
MarijkeZondag 26:ac5656aa35c7 496
MarijkeZondag 26:ac5656aa35c7 497
MarijkeZondag 23:97a976a8f0fc 498 int main()
MarijkeZondag 23:97a976a8f0fc 499 {
MarijkeZondag 23:97a976a8f0fc 500 pc.baud(115200);
MarijkeZondag 30:f04a35f2a06d 501 pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
MarijkeZondag 30:f04a35f2a06d 502 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 23:97a976a8f0fc 503
MarijkeZondag 23:97a976a8f0fc 504 emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( &notch1 ); //attach biquad elements to chain
MarijkeZondag 23:97a976a8f0fc 505 emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
MarijkeZondag 23:97a976a8f0fc 506 emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );
MarijkeZondag 10:39ec51206c8b 507
MarijkeZondag 30:f04a35f2a06d 508 emg_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
MarijkeZondag 28:5e54cd4525de 509 movag_tick.attach(&MovAg,T);
MarijkeZondag 30:f04a35f2a06d 510 func_tick.attach(&v_des_calculate_qref,T); //v_des determined every T
MarijkeZondag 30:f04a35f2a06d 511 print_tick.attach(&printFunction,T2);
MarijkeZondag 30:f04a35f2a06d 512 // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
MarijkeZondag 28:5e54cd4525de 513
MarijkeZondag 30:f04a35f2a06d 514 button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
MarijkeZondag 30:f04a35f2a06d 515 //wait(0.2f); //Wait to avoid bouncing of button
MarijkeZondag 30:f04a35f2a06d 516 button2.rise(calibrate); //Calibrate threshold for 3 muscles
MarijkeZondag 30:f04a35f2a06d 517 //wait(0.2f); //Wait to avoid bouncing of button
MarijkeZondag 28:5e54cd4525de 518
MarijkeZondag 23:97a976a8f0fc 519 while(true)
MarijkeZondag 23:97a976a8f0fc 520 {
MarijkeZondag 30:f04a35f2a06d 521 ;
MarijkeZondag 23:97a976a8f0fc 522 }
vsluiter 0:c8f15874531b 523 }