Marleen van Hoorn / Mbed 2 deprecated Project_script

inverse kinematics toegevoegd en tickers samengevoegd tot 1 ticker

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of Project_script by Marijke Zondag

main.cpp@25:bbef09ff226b, 2018-10-31 (annotated)

Committer:
Marle
Date:
Wed Oct 31 09:28:45 2018 +0000
Revision:
25:bbef09ff226b
Parent:
24:6d63ad38fef7 
inverse kinematics toegevoegd en tickers samengevoegd to 1 ticker

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>
vsluiter 0:c8f15874531b 6
MarijkeZondag 22:5d956c93b3ae 7 //ATTENTION: set mBed to version 151
MarijkeZondag 22:5d956c93b3ae 8 // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
MarijkeZondag 22:5d956c93b3ae 9 // set MODSERIAL to version 44
MarijkeZondag 22:5d956c93b3ae 10 // set HIDScope to version 7
MarijkeZondag 22:5d956c93b3ae 11 // set biquadFilter to version 7
MarijkeZondag 10:39ec51206c8b 12
MarijkeZondag 22:5d956c93b3ae 13 AnalogIn emg0_in (A0); //First raw EMG signal input
MarijkeZondag 22:5d956c93b3ae 14 AnalogIn emg1_in (A1); //Second raw EMG signal input
MarijkeZondag 22:5d956c93b3ae 15 AnalogIn emg2_in (A2); //Third raw EMG signal input
MarijkeZondag 22:5d956c93b3ae 16
MarijkeZondag 22:5d956c93b3ae 17 InterruptIn button1 (D10); //Is this one available? We need to make a map of which pins are used for what.
MarijkeZondag 16:5f7196ddc77b 18 InterruptIn button2 (D11);
MarijkeZondag 6:f4bbb73f3989 19
MarijkeZondag 23:97a976a8f0fc 20 DigitalOut directionpin1 (D7);
MarijkeZondag 23:97a976a8f0fc 21 DigitalOut directionpin2 (D4);
MarijkeZondag 23:97a976a8f0fc 22 PwmOut pwmpin1 (D6);
MarijkeZondag 23:97a976a8f0fc 23 PwmOut pwmpin2 (D5);
MarijkeZondag 22:5d956c93b3ae 24
MarijkeZondag 17:741798018c0d 25 DigitalOut ledr (LED_RED);
MarijkeZondag 17:741798018c0d 26 DigitalOut ledb (LED_BLUE);
MarijkeZondag 17:741798018c0d 27 DigitalOut ledg (LED_GREEN);
MarijkeZondag 13:a3d4b4daf5b4 28
MarijkeZondag 9:c722418997b5 29
MarijkeZondag 23:97a976a8f0fc 30 MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off
MarijkeZondag 6:f4bbb73f3989 31
MarijkeZondag 23:97a976a8f0fc 32 //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
vsluiter 0:c8f15874531b 33
MarijkeZondag 22:5d956c93b3ae 34 //Tickers
Marle 25:bbef09ff226b 35 Ticker ticker;
MarijkeZondag 9:c722418997b5 36
MarijkeZondag 9:c722418997b5 37 //Global variables
MarijkeZondag 22:5d956c93b3ae 38 const float T = 0.002f; //Ticker period
MarijkeZondag 10:39ec51206c8b 39
MarijkeZondag 13:a3d4b4daf5b4 40 //EMG filter
MarijkeZondag 22:5d956c93b3ae 41 double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2
MarijkeZondag 22:5d956c93b3ae 42 double emg0_raw, emg1_raw, emg2_raw;
MarijkeZondag 22:5d956c93b3ae 43 double emg0_filt_x, emg1_filt_x, emg2_filt_x;
MarijkeZondag 22:5d956c93b3ae 44 const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number)
MarijkeZondag 13:a3d4b4daf5b4 45 double sum, sum1, sum2, sum3; //variables used to sum elements in array
MarijkeZondag 22:5d956c93b3ae 46 double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg
MarijkeZondag 22:5d956c93b3ae 47 double movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??)
MarijkeZondag 13:a3d4b4daf5b4 48
MarijkeZondag 22:5d956c93b3ae 49 //Calibration variables
MarijkeZondag 22:5d956c93b3ae 50 int x = -1; //Start switch, colour LED is blue.
MarijkeZondag 22:5d956c93b3ae 51 int emg_cal = 0; //if emg_cal is set to 1, motors can begin to work in this code (!!)
MarijkeZondag 22:5d956c93b3ae 52 const int sizeCal = 1500; //size of the dataset used for calibration, eerst 2000
MarijkeZondag 22:5d956c93b3ae 53 double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //arrays to put the dataset of the calibration in
MarijkeZondag 22:5d956c93b3ae 54 double Mean0,Mean1,Mean2; //average of maximum tightening
MarijkeZondag 22:5d956c93b3ae 55 double Threshold0, Threshold1, Threshold2;
MarijkeZondag 13:a3d4b4daf5b4 56
MarijkeZondag 22:5d956c93b3ae 57 //Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo)
MarijkeZondag 22:5d956c93b3ae 58 BiQuadChain emg0filter;
MarijkeZondag 10:39ec51206c8b 59 BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 60 BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 61 BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 22:5d956c93b3ae 62 BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients
MarijkeZondag 10:39ec51206c8b 63
MarijkeZondag 22:5d956c93b3ae 64 BiQuadChain emg1filter;
MarijkeZondag 10:39ec51206c8b 65 BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 66 BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 67 BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 22:5d956c93b3ae 68 BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 69
MarijkeZondag 22:5d956c93b3ae 70 BiQuadChain emg2filter;
MarijkeZondag 10:39ec51206c8b 71 BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 72 BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 73 BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 14:fa09dae67390 74 BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 75
Marle 25:bbef09ff226b 76 // Inverse Kinematica variabelen
Marle 25:bbef09ff226b 77 const double L1 = 0.208; // Hoogte van tafel tot joint 1
Marle 25:bbef09ff226b 78 const double L2 = 0.288; // Hoogte van tafel tot joint 2
Marle 25:bbef09ff226b 79 const double L3 = 0.212; // Lengte van de arm
Marle 25:bbef09ff226b 80 const double L4 = 0.089; // Afstand van achterkant base tot joint 1
Marle 25:bbef09ff226b 81 const double L5 = 0.030; // Afstand van joint 1 naar joint 2
Marle 25:bbef09ff226b 82 const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation
Marle 25:bbef09ff226b 83 //const double T = 0.002f; // Ticker value
Marle 25:bbef09ff226b 84
Marle 25:bbef09ff226b 85 // Variërende variabelen inverse kinematics:
Marle 25:bbef09ff226b 86 double q1ref = 0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
Marle 25:bbef09ff226b 87 double q2ref = 0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
Marle 25:bbef09ff226b 88 double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals
Marle 25:bbef09ff226b 89 double v_y; // Desired velocity end effector in y direction --> Determined by EMG signals
Marle 25:bbef09ff226b 90
Marle 25:bbef09ff226b 91 double Lq1; // Translatieafstand als gevolg van motor rotation joint 1
Marle 25:bbef09ff226b 92 double Cq2; // Joint angle of the system (corrected for gear ratio 1:5)
Marle 25:bbef09ff226b 93
Marle 25:bbef09ff226b 94 double q1_dot; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
Marle 25:bbef09ff226b 95 double q2_dot; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken
Marle 25:bbef09ff226b 96
Marle 25:bbef09ff226b 97 double q1_ii; // Reference signal for motorangle q1ref
Marle 25:bbef09ff226b 98 double q2_ii; // Reference signal for motorangle q2ref
Marle 25:bbef09ff226b 99
Marle 25:bbef09ff226b 100
MarijkeZondag 9:c722418997b5 101 //Functions
MarijkeZondag 12:eaed305a76c3 102
MarijkeZondag 13:a3d4b4daf5b4 103 void switch_to_calibrate()
MarijkeZondag 13:a3d4b4daf5b4 104 {
MarijkeZondag 22:5d956c93b3ae 105 x++; //Every time function gets called, x increases. Every button press --> new calibration state.
MarijkeZondag 22:5d956c93b3ae 106 //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 107
MarijkeZondag 13:a3d4b4daf5b4 108 if(x==0) //If x = 0, led is red
MarijkeZondag 13:a3d4b4daf5b4 109 {
MarijkeZondag 17:741798018c0d 110 ledr = 0;
MarijkeZondag 17:741798018c0d 111 ledb = 1;
MarijkeZondag 17:741798018c0d 112 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 113 }
MarijkeZondag 13:a3d4b4daf5b4 114 else if (x==1) //If x = 1, led is blue
MarijkeZondag 13:a3d4b4daf5b4 115 {
MarijkeZondag 17:741798018c0d 116 ledr = 1;
MarijkeZondag 17:741798018c0d 117 ledb = 0;
MarijkeZondag 17:741798018c0d 118 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 119 }
MarijkeZondag 22:5d956c93b3ae 120 else if (x==2) //If x = 2, led is green
MarijkeZondag 13:a3d4b4daf5b4 121 {
MarijkeZondag 17:741798018c0d 122 ledr = 1;
MarijkeZondag 17:741798018c0d 123 ledb = 1;
MarijkeZondag 17:741798018c0d 124 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 125 }
MarijkeZondag 22:5d956c93b3ae 126 else //If x = 3 or 4, led is white
MarijkeZondag 13:a3d4b4daf5b4 127 {
MarijkeZondag 17:741798018c0d 128 ledr = 0;
MarijkeZondag 17:741798018c0d 129 ledb = 0;
MarijkeZondag 17:741798018c0d 130 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 131 }
MarijkeZondag 13:a3d4b4daf5b4 132
MarijkeZondag 22:5d956c93b3ae 133 if(x==4) //Reset back to x = -1
MarijkeZondag 13:a3d4b4daf5b4 134 {
MarijkeZondag 16:5f7196ddc77b 135 x = -1;
MarijkeZondag 23:97a976a8f0fc 136 emg_cal=0; //reset, motors off
MarijkeZondag 13:a3d4b4daf5b4 137 }
MarijkeZondag 13:a3d4b4daf5b4 138 }
MarijkeZondag 13:a3d4b4daf5b4 139
MarijkeZondag 13:a3d4b4daf5b4 140
MarijkeZondag 13:a3d4b4daf5b4 141 void calibrate(void)
MarijkeZondag 12:eaed305a76c3 142 {
MarijkeZondag 13:a3d4b4daf5b4 143 switch(x)
MarijkeZondag 13:a3d4b4daf5b4 144 {
MarijkeZondag 22:5d956c93b3ae 145 case 0: //If calibration state 0:
MarijkeZondag 13:a3d4b4daf5b4 146 {
MarijkeZondag 13:a3d4b4daf5b4 147 sum = 0.0;
MarijkeZondag 22:5d956c93b3ae 148 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0
MarijkeZondag 13:a3d4b4daf5b4 149 {
MarijkeZondag 22:5d956c93b3ae 150 StoreCal0[j] = emg0_filt;
MarijkeZondag 13:a3d4b4daf5b4 151 sum+=StoreCal0[j];
MarijkeZondag 22:5d956c93b3ae 152 wait(0.001f); //Does there need to be a wait?
MarijkeZondag 13:a3d4b4daf5b4 153 }
MarijkeZondag 22:5d956c93b3ae 154 Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples)
MarijkeZondag 23:97a976a8f0fc 155 Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean
MarijkeZondag 22:5d956c93b3ae 156 break; //Stop. Threshold is calculated, we will use this further in the code
MarijkeZondag 13:a3d4b4daf5b4 157 }
MarijkeZondag 22:5d956c93b3ae 158 case 1: //If calibration state 1:
MarijkeZondag 13:a3d4b4daf5b4 159 {
MarijkeZondag 22:5d956c93b3ae 160 sum = 0.0;
MarijkeZondag 22:5d956c93b3ae 161 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1
MarijkeZondag 13:a3d4b4daf5b4 162 {
MarijkeZondag 22:5d956c93b3ae 163 StoreCal1[j] = emg1_filt;
MarijkeZondag 13:a3d4b4daf5b4 164 sum+=StoreCal1[j];
MarijkeZondag 21:1da43fdbd254 165 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 166 }
MarijkeZondag 13:a3d4b4daf5b4 167 Mean1 = sum/sizeCal;
MarijkeZondag 23:97a976a8f0fc 168 Threshold1 = Mean1/2;
MarijkeZondag 13:a3d4b4daf5b4 169 break;
MarijkeZondag 13:a3d4b4daf5b4 170 }
MarijkeZondag 22:5d956c93b3ae 171 case 2: //If calibration state 2:
MarijkeZondag 13:a3d4b4daf5b4 172 {
MarijkeZondag 13:a3d4b4daf5b4 173 sum = 0.0;
MarijkeZondag 22:5d956c93b3ae 174 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2
MarijkeZondag 13:a3d4b4daf5b4 175 {
MarijkeZondag 23:97a976a8f0fc 176 StoreCal2[j] = emg2_filt;
MarijkeZondag 13:a3d4b4daf5b4 177 sum+=StoreCal2[j];
MarijkeZondag 21:1da43fdbd254 178 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 179 }
MarijkeZondag 13:a3d4b4daf5b4 180 Mean2 = sum/sizeCal;
MarijkeZondag 23:97a976a8f0fc 181 Threshold2 = Mean2/2;
MarijkeZondag 13:a3d4b4daf5b4 182 break;
MarijkeZondag 13:a3d4b4daf5b4 183 }
MarijkeZondag 22:5d956c93b3ae 184 case 3: //EMG is calibrated, robot can be set to Home position.
MarijkeZondag 13:a3d4b4daf5b4 185 {
MarijkeZondag 22:5d956c93b3ae 186 emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!)
MarijkeZondag 21:1da43fdbd254 187 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 188 break;
MarijkeZondag 13:a3d4b4daf5b4 189 }
MarijkeZondag 22:5d956c93b3ae 190 default: //Ensures nothing happens if x is not 0,1 or 2.
MarijkeZondag 13:a3d4b4daf5b4 191 {
MarijkeZondag 13:a3d4b4daf5b4 192 break;
MarijkeZondag 13:a3d4b4daf5b4 193 }
MarijkeZondag 13:a3d4b4daf5b4 194 }
MarijkeZondag 12:eaed305a76c3 195 }
MarijkeZondag 22:5d956c93b3ae 196
MarijkeZondag 22:5d956c93b3ae 197 void HIDScope_sample()
MarijkeZondag 22:5d956c93b3ae 198 {
MarijkeZondag 22:5d956c93b3ae 199 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
MarijkeZondag 23:97a976a8f0fc 200 //scope.set(0,emg0_raw);
MarijkeZondag 22:5d956c93b3ae 201 //scope.set(1,emg0_filt);
MarijkeZondag 23:97a976a8f0fc 202 //scope.set(1,movAg0); //als moving average werkt
MarijkeZondag 23:97a976a8f0fc 203 //scope.set(2,emg1_raw);
MarijkeZondag 22:5d956c93b3ae 204 //scope.set(3,emg1_filt);
MarijkeZondag 23:97a976a8f0fc 205 //scope.set(3,movAg1); //als moving average werkt
MarijkeZondag 23:97a976a8f0fc 206 //scope.set(4,emg2_raw);
MarijkeZondag 22:5d956c93b3ae 207 //scope.set(5,emg2_filt);
MarijkeZondag 23:97a976a8f0fc 208 //scope.set(5,movAg2); //als moving average werkt
MarijkeZondag 22:5d956c93b3ae 209
MarijkeZondag 23:97a976a8f0fc 210 //scope.send(); //Send data to HIDScope server
MarijkeZondag 22:5d956c93b3ae 211 }
MarijkeZondag 22:5d956c93b3ae 212
MarijkeZondag 22:5d956c93b3ae 213 void EMGFilter0()
MarijkeZondag 22:5d956c93b3ae 214 {
MarijkeZondag 22:5d956c93b3ae 215 emg0_raw = emg0_in.read(); //give name to raw EMG0 data
MarijkeZondag 22:5d956c93b3ae 216 emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 22:5d956c93b3ae 217 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 22:5d956c93b3ae 218 }
MarijkeZondag 22:5d956c93b3ae 219
MarijkeZondag 22:5d956c93b3ae 220 void EMGFilter1()
MarijkeZondag 8:895d941a5910 221 {
MarijkeZondag 22:5d956c93b3ae 222 emg1_raw = emg1_in.read(); //give name to raw EMG1 data
MarijkeZondag 22:5d956c93b3ae 223 emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 22:5d956c93b3ae 224 emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
MarijkeZondag 8:895d941a5910 225 }
MarijkeZondag 8:895d941a5910 226
MarijkeZondag 22:5d956c93b3ae 227 void EMGFilter2()
MarijkeZondag 8:895d941a5910 228 {
MarijkeZondag 22:5d956c93b3ae 229 emg2_raw = emg2_in.read(); //Give name to raw EMG1 data
MarijkeZondag 22:5d956c93b3ae 230 emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 22:5d956c93b3ae 231 emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier.
MarijkeZondag 22:5d956c93b3ae 232 }
MarijkeZondag 22:5d956c93b3ae 233
MarijkeZondag 22:5d956c93b3ae 234 void MovAg() //Calculate moving average (MovAg), klopt nog niet!!
MarijkeZondag 22:5d956c93b3ae 235 {
MarijkeZondag 22:5d956c93b3ae 236 for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals
MarijkeZondag 8:895d941a5910 237 {
MarijkeZondag 22:5d956c93b3ae 238 StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average.
MarijkeZondag 22:5d956c93b3ae 239 StoreArray1[i] = StoreArray1[i-1];
MarijkeZondag 22:5d956c93b3ae 240 StoreArray2[i] = StoreArray2[i-1];
MarijkeZondag 8:895d941a5910 241 }
MarijkeZondag 22:5d956c93b3ae 242
MarijkeZondag 22:5d956c93b3ae 243 StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array
MarijkeZondag 22:5d956c93b3ae 244 StoreArray1[0] = emg1_filt;
MarijkeZondag 22:5d956c93b3ae 245 StoreArray2[0] = emg2_filt;
MarijkeZondag 22:5d956c93b3ae 246
MarijkeZondag 22:5d956c93b3ae 247 sum1 = 0.0;
MarijkeZondag 22:5d956c93b3ae 248 sum2 = 0.0;
MarijkeZondag 22:5d956c93b3ae 249 sum3 = 0.0;
MarijkeZondag 22:5d956c93b3ae 250
MarijkeZondag 22:5d956c93b3ae 251 for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays
MarijkeZondag 8:895d941a5910 252 {
MarijkeZondag 22:5d956c93b3ae 253 sum1 += StoreArray0[a];
MarijkeZondag 22:5d956c93b3ae 254 sum2 += StoreArray1[a];
MarijkeZondag 22:5d956c93b3ae 255 sum3 += StoreArray2[a];
MarijkeZondag 8:895d941a5910 256 }
MarijkeZondag 22:5d956c93b3ae 257
MarijkeZondag 22:5d956c93b3ae 258 movAg0 = sum1/windowsize; //calculates an average in the array
MarijkeZondag 22:5d956c93b3ae 259 movAg1 = sum2/windowsize;
MarijkeZondag 22:5d956c93b3ae 260 movAg2 = sum3/windowsize;
MarijkeZondag 22:5d956c93b3ae 261 //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 8:895d941a5910 262 }
MarijkeZondag 8:895d941a5910 263
MarijkeZondag 22:5d956c93b3ae 264 void emg_filtered() //Call all filter functions
MarijkeZondag 8:895d941a5910 265 {
MarijkeZondag 22:5d956c93b3ae 266 EMGFilter0();
MarijkeZondag 22:5d956c93b3ae 267 EMGFilter1();
MarijkeZondag 22:5d956c93b3ae 268 EMGFilter2();
MarijkeZondag 8:895d941a5910 269 }
MarijkeZondag 8:895d941a5910 270
Marle 25:bbef09ff226b 271 void inverse_kinematics()
Marle 25:bbef09ff226b 272 {
Marle 25:bbef09ff226b 273 Lq1 = q1ref*r_trans;
Marle 25:bbef09ff226b 274 Cq2 = q2ref/5.0;
Marle 25:bbef09ff226b 275
Marle 25:bbef09ff226b 276 q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2));
Marle 25:bbef09ff226b 277 q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2));
Marle 25:bbef09ff226b 278
Marle 25:bbef09ff226b 279 q1_ii = q1ref + q1_dot*T;
Marle 25:bbef09ff226b 280 q2_ii = q2ref + q2_dot*T;
Marle 25:bbef09ff226b 281
Marle 25:bbef09ff226b 282 q1ref = q1_ii;
Marle 25:bbef09ff226b 283 q2ref = q2_ii;
Marle 25:bbef09ff226b 284 }
Marle 25:bbef09ff226b 285
Marle 25:bbef09ff226b 286 void v_des_calculate_qref()
MarijkeZondag 23:97a976a8f0fc 287 {
MarijkeZondag 23:97a976a8f0fc 288 while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
MarijkeZondag 16:5f7196ddc77b 289 {
Marle 25:bbef09ff226b 290 if(movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn
MarijkeZondag 16:5f7196ddc77b 291 {
Marle 25:bbef09ff226b 292 v_x = 1.0; //beweging in +x direction
Marle 25:bbef09ff226b 293 ledr = 0; //red
Marle 25:bbef09ff226b 294 ledb = 1;
Marle 25:bbef09ff226b 295 ledg = 1;
MarijkeZondag 16:5f7196ddc77b 296 }
MarijkeZondag 24:6d63ad38fef7 297 else if(movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
MarijkeZondag 16:5f7196ddc77b 298 {
Marle 25:bbef09ff226b 299 v_y = 1.0; //beweging in +y direction
Marle 25:bbef09ff226b 300 ledr = 1; //green
Marle 25:bbef09ff226b 301 ledb = 1;
Marle 25:bbef09ff226b 302 ledg = 0;
MarijkeZondag 24:6d63ad38fef7 303 }
Marle 25:bbef09ff226b 304
Marle 25:bbef09ff226b 305 else if(movAg0>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
MarijkeZondag 24:6d63ad38fef7 306 {
Marle 25:bbef09ff226b 307 v_x = -v_x;
Marle 25:bbef09ff226b 308 v_y = -v_y;
Marle 25:bbef09ff226b 309 ledr = 1; //Blue
Marle 25:bbef09ff226b 310 ledb = 0;
Marle 25:bbef09ff226b 311 ledg = 1;
Marle 25:bbef09ff226b 312 }
Marle 25:bbef09ff226b 313 else //If not higher than the threshold, motors will not turn at all
Marle 25:bbef09ff226b 314 {
Marle 25:bbef09ff226b 315 v_x = 0;
Marle 25:bbef09ff226b 316 v_y = 0;
MarijkeZondag 24:6d63ad38fef7 317 ledr = 0; //white
MarijkeZondag 24:6d63ad38fef7 318 ledb = 0;
MarijkeZondag 24:6d63ad38fef7 319 ledg = 0;
MarijkeZondag 24:6d63ad38fef7 320 }
Marle 25:bbef09ff226b 321
MarijkeZondag 23:97a976a8f0fc 322 break;
MarijkeZondag 23:97a976a8f0fc 323 }
Marle 25:bbef09ff226b 324 inverse_kinematics();
MarijkeZondag 23:97a976a8f0fc 325 }
MarijkeZondag 23:97a976a8f0fc 326
MarijkeZondag 23:97a976a8f0fc 327
MarijkeZondag 23:97a976a8f0fc 328 int main()
MarijkeZondag 23:97a976a8f0fc 329 {
MarijkeZondag 23:97a976a8f0fc 330 pc.baud(115200);
MarijkeZondag 23:97a976a8f0fc 331 pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
MarijkeZondag 23:97a976a8f0fc 332 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 23:97a976a8f0fc 333
MarijkeZondag 23:97a976a8f0fc 334 emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( &notch1 ); //attach biquad elements to chain
MarijkeZondag 23:97a976a8f0fc 335 emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
MarijkeZondag 23:97a976a8f0fc 336 emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );
MarijkeZondag 10:39ec51206c8b 337
MarijkeZondag 23:97a976a8f0fc 338 while(true)
MarijkeZondag 23:97a976a8f0fc 339 {
Marle 25:bbef09ff226b 340 ticker.attach(&emg_filtered,T); //EMG signals filtered every T sec.
Marle 25:bbef09ff226b 341 ticker.attach(&MovAg,T); //Moving average calculation every T sec.
Marle 25:bbef09ff226b 342 ticker.attach(&v_des_calculate_qref,T); //v_des determined every T
Marle 25:bbef09ff226b 343
MarijkeZondag 23:97a976a8f0fc 344 // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
MarijkeZondag 23:97a976a8f0fc 345
MarijkeZondag 23:97a976a8f0fc 346 button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
MarijkeZondag 24:6d63ad38fef7 347 wait(0.2f); //Wait to avoid bouncing of button
MarijkeZondag 23:97a976a8f0fc 348 button2.rise(calibrate); //Calibrate threshold for 3 muscles
MarijkeZondag 24:6d63ad38fef7 349 wait(0.2f); //Wait to avoid bouncing of button
MarijkeZondag 23:97a976a8f0fc 350
MarijkeZondag 23:97a976a8f0fc 351 pc.printf("x is %i\n\r",x);
MarijkeZondag 23:97a976a8f0fc 352 pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2);
MarijkeZondag 23:97a976a8f0fc 353 pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
MarijkeZondag 23:97a976a8f0fc 354 //wait(2.0f);
MarijkeZondag 23:97a976a8f0fc 355 }
vsluiter 0:c8f15874531b 356 }