Jorine Oosting / Mbed 2 deprecated Project_script_union_final

Final Jorine Aangepast

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Project_script_union_final by Marijke Zondag

main.cpp@27:fa493551be99, 2018-10-31 (annotated)

Committer:
MarijkeZondag
Date:
Wed Oct 31 12:38:00 2018 +0000
Revision:
27:fa493551be99
Parent:
26:ac5656aa35c7
Child:
28:5e54cd4525de
Aanpassing om 13:37 uur;

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 26:ac5656aa35c7 17 InterruptIn encoderA1 (D9);
MarijkeZondag 26:ac5656aa35c7 18 InterruptIn encoderB1 (D8);
MarijkeZondag 26:ac5656aa35c7 19 InterruptIn encoderA2 (D12);
MarijkeZondag 26:ac5656aa35c7 20 InterruptIn encoderB2 (D13);
MarijkeZondag 26:ac5656aa35c7 21
MarijkeZondag 26:ac5656aa35c7 22 InterruptIn button1 (D10);
MarijkeZondag 16:5f7196ddc77b 23 InterruptIn button2 (D11);
MarijkeZondag 6:f4bbb73f3989 24
MarijkeZondag 23:97a976a8f0fc 25 DigitalOut directionpin1 (D7);
MarijkeZondag 23:97a976a8f0fc 26 DigitalOut directionpin2 (D4);
MarijkeZondag 26:ac5656aa35c7 27
MarijkeZondag 23:97a976a8f0fc 28 PwmOut pwmpin1 (D6);
MarijkeZondag 23:97a976a8f0fc 29 PwmOut pwmpin2 (D5);
MarijkeZondag 22:5d956c93b3ae 30
MarijkeZondag 17:741798018c0d 31 DigitalOut ledr (LED_RED);
MarijkeZondag 17:741798018c0d 32 DigitalOut ledb (LED_BLUE);
MarijkeZondag 17:741798018c0d 33 DigitalOut ledg (LED_GREEN);
MarijkeZondag 13:a3d4b4daf5b4 34
MarijkeZondag 9:c722418997b5 35
MarijkeZondag 23:97a976a8f0fc 36 MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off
MarijkeZondag 6:f4bbb73f3989 37
MarijkeZondag 23:97a976a8f0fc 38 //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
vsluiter 0:c8f15874531b 39
MarijkeZondag 22:5d956c93b3ae 40 //Tickers
Marle 25:bbef09ff226b 41 Ticker ticker;
MarijkeZondag 9:c722418997b5 42
MarijkeZondag 9:c722418997b5 43 //Global variables
MarijkeZondag 27:fa493551be99 44 const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders???
MarijkeZondag 10:39ec51206c8b 45
MarijkeZondag 13:a3d4b4daf5b4 46 //EMG filter
MarijkeZondag 22:5d956c93b3ae 47 double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2
MarijkeZondag 22:5d956c93b3ae 48 double emg0_raw, emg1_raw, emg2_raw;
MarijkeZondag 22:5d956c93b3ae 49 double emg0_filt_x, emg1_filt_x, emg2_filt_x;
MarijkeZondag 22:5d956c93b3ae 50 const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number)
MarijkeZondag 13:a3d4b4daf5b4 51 double sum, sum1, sum2, sum3; //variables used to sum elements in array
MarijkeZondag 22:5d956c93b3ae 52 double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg
MarijkeZondag 22:5d956c93b3ae 53 double movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??)
MarijkeZondag 13:a3d4b4daf5b4 54
MarijkeZondag 22:5d956c93b3ae 55 //Calibration variables
MarijkeZondag 22:5d956c93b3ae 56 int x = -1; //Start switch, colour LED is blue.
MarijkeZondag 22:5d956c93b3ae 57 int emg_cal = 0; //if emg_cal is set to 1, motors can begin to work in this code (!!)
MarijkeZondag 22:5d956c93b3ae 58 const int sizeCal = 1500; //size of the dataset used for calibration, eerst 2000
MarijkeZondag 22:5d956c93b3ae 59 double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //arrays to put the dataset of the calibration in
MarijkeZondag 22:5d956c93b3ae 60 double Mean0,Mean1,Mean2; //average of maximum tightening
MarijkeZondag 22:5d956c93b3ae 61 double Threshold0, Threshold1, Threshold2;
MarijkeZondag 13:a3d4b4daf5b4 62
MarijkeZondag 22:5d956c93b3ae 63 //Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo)
MarijkeZondag 22:5d956c93b3ae 64 BiQuadChain emg0filter;
MarijkeZondag 10:39ec51206c8b 65 BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 66 BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 67 BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 22:5d956c93b3ae 68 BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients
MarijkeZondag 10:39ec51206c8b 69
MarijkeZondag 22:5d956c93b3ae 70 BiQuadChain emg1filter;
MarijkeZondag 10:39ec51206c8b 71 BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 72 BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 73 BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 22:5d956c93b3ae 74 BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 75
MarijkeZondag 22:5d956c93b3ae 76 BiQuadChain emg2filter;
MarijkeZondag 10:39ec51206c8b 77 BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 78 BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 79 BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 14:fa09dae67390 80 BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 81
MarijkeZondag 26:ac5656aa35c7 82 // Inverse Kinematica variables
Marle 25:bbef09ff226b 83 const double L1 = 0.208; // Hoogte van tafel tot joint 1
Marle 25:bbef09ff226b 84 const double L2 = 0.288; // Hoogte van tafel tot joint 2
Marle 25:bbef09ff226b 85 const double L3 = 0.212; // Lengte van de arm
Marle 25:bbef09ff226b 86 const double L4 = 0.089; // Afstand van achterkant base tot joint 1
Marle 25:bbef09ff226b 87 const double L5 = 0.030; // Afstand van joint 1 naar joint 2
Marle 25:bbef09ff226b 88 const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation
Marle 25:bbef09ff226b 89
Marle 25:bbef09ff226b 90 // Variërende variabelen inverse kinematics:
Marle 25:bbef09ff226b 91 double q1ref = 0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
Marle 25:bbef09ff226b 92 double q2ref = 0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
Marle 25:bbef09ff226b 93 double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals
Marle 25:bbef09ff226b 94 double v_y; // Desired velocity end effector in y direction --> Determined by EMG signals
Marle 25:bbef09ff226b 95
Marle 25:bbef09ff226b 96 double Lq1; // Translatieafstand als gevolg van motor rotation joint 1
Marle 25:bbef09ff226b 97 double Cq2; // Joint angle of the system (corrected for gear ratio 1:5)
Marle 25:bbef09ff226b 98
Marle 25:bbef09ff226b 99 double q1_dot; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
Marle 25:bbef09ff226b 100 double q2_dot; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken
Marle 25:bbef09ff226b 101
Marle 25:bbef09ff226b 102 double q1_ii; // Reference signal for motorangle q1ref
Marle 25:bbef09ff226b 103 double q2_ii; // Reference signal for motorangle q2ref
Marle 25:bbef09ff226b 104
MarijkeZondag 26:ac5656aa35c7 105 //Variables PID controller
MarijkeZondag 26:ac5656aa35c7 106 double PI = 3.14159;
MarijkeZondag 26:ac5656aa35c7 107 double Kp1 = 17.5; //Motor 1
MarijkeZondag 26:ac5656aa35c7 108 double Ki1 = 1.02;
MarijkeZondag 26:ac5656aa35c7 109 double Kd1 = 23.2;
MarijkeZondag 26:ac5656aa35c7 110 double encoder1 = 0;
MarijkeZondag 26:ac5656aa35c7 111 double encoder_radians1=0;
Marle 25:bbef09ff226b 112
MarijkeZondag 26:ac5656aa35c7 113 double Kp2 = 17.5; //Motor 2
MarijkeZondag 26:ac5656aa35c7 114 double Ki2 = 1.02;
MarijkeZondag 26:ac5656aa35c7 115 double Kd2 = 23.2;
MarijkeZondag 26:ac5656aa35c7 116 double encoder2 = 0;
MarijkeZondag 26:ac5656aa35c7 117 double encoder_radians2=0;
MarijkeZondag 26:ac5656aa35c7 118
MarijkeZondag 27:fa493551be99 119 double start_control = 0;
MarijkeZondag 27:fa493551be99 120
MarijkeZondag 26:ac5656aa35c7 121
MarijkeZondag 26:ac5656aa35c7 122 //--------------Functions----------------------------------------------------------------------------------------------------------------------------//
MarijkeZondag 26:ac5656aa35c7 123
MarijkeZondag 26:ac5656aa35c7 124
MarijkeZondag 26:ac5656aa35c7 125 //------------------ Encoder motor 1 --------------------------------//
MarijkeZondag 26:ac5656aa35c7 126
MarijkeZondag 26:ac5656aa35c7 127 void encoderA1_rise()
MarijkeZondag 26:ac5656aa35c7 128 {
MarijkeZondag 26:ac5656aa35c7 129 if(encoderB1==false)
MarijkeZondag 26:ac5656aa35c7 130 {
MarijkeZondag 26:ac5656aa35c7 131 encoder1++;
MarijkeZondag 26:ac5656aa35c7 132 }
MarijkeZondag 26:ac5656aa35c7 133 else
MarijkeZondag 26:ac5656aa35c7 134 {
MarijkeZondag 26:ac5656aa35c7 135 encoder1--;
MarijkeZondag 26:ac5656aa35c7 136 }
MarijkeZondag 26:ac5656aa35c7 137 }
MarijkeZondag 26:ac5656aa35c7 138
MarijkeZondag 26:ac5656aa35c7 139 void encoderA1_fall()
MarijkeZondag 26:ac5656aa35c7 140 {
MarijkeZondag 26:ac5656aa35c7 141 if(encoderB1==true)
MarijkeZondag 26:ac5656aa35c7 142 {
MarijkeZondag 26:ac5656aa35c7 143 encoder1++;
MarijkeZondag 26:ac5656aa35c7 144 }
MarijkeZondag 26:ac5656aa35c7 145 else
MarijkeZondag 26:ac5656aa35c7 146 {
MarijkeZondag 26:ac5656aa35c7 147 encoder1--;
MarijkeZondag 26:ac5656aa35c7 148 }
MarijkeZondag 26:ac5656aa35c7 149 }
MarijkeZondag 26:ac5656aa35c7 150
MarijkeZondag 26:ac5656aa35c7 151 void encoderB1_rise()
MarijkeZondag 26:ac5656aa35c7 152 {
MarijkeZondag 26:ac5656aa35c7 153 if(encoderA1==true)
MarijkeZondag 26:ac5656aa35c7 154 {
MarijkeZondag 26:ac5656aa35c7 155 encoder1++;
MarijkeZondag 26:ac5656aa35c7 156 }
MarijkeZondag 26:ac5656aa35c7 157 else
MarijkeZondag 26:ac5656aa35c7 158 {
MarijkeZondag 26:ac5656aa35c7 159 encoder1--;
MarijkeZondag 26:ac5656aa35c7 160 }
MarijkeZondag 26:ac5656aa35c7 161 }
MarijkeZondag 26:ac5656aa35c7 162
MarijkeZondag 26:ac5656aa35c7 163 void encoderB1_fall()
MarijkeZondag 26:ac5656aa35c7 164 {
MarijkeZondag 26:ac5656aa35c7 165 if(encoderA1==false)
MarijkeZondag 26:ac5656aa35c7 166 {
MarijkeZondag 26:ac5656aa35c7 167 encoder1++;
MarijkeZondag 26:ac5656aa35c7 168 }
MarijkeZondag 26:ac5656aa35c7 169 else
MarijkeZondag 26:ac5656aa35c7 170 {
MarijkeZondag 26:ac5656aa35c7 171 encoder1--;
MarijkeZondag 26:ac5656aa35c7 172 }
MarijkeZondag 26:ac5656aa35c7 173 }
MarijkeZondag 26:ac5656aa35c7 174
MarijkeZondag 26:ac5656aa35c7 175 void encoder_count1()
MarijkeZondag 26:ac5656aa35c7 176 {
MarijkeZondag 26:ac5656aa35c7 177 encoderA1.rise(&encoderA1_rise);
MarijkeZondag 26:ac5656aa35c7 178 encoderA1.fall(&encoderA1_fall);
MarijkeZondag 26:ac5656aa35c7 179 encoderB1.rise(&encoderB1_rise);
MarijkeZondag 26:ac5656aa35c7 180 encoderB1.fall(&encoderB1_fall);
MarijkeZondag 26:ac5656aa35c7 181 }
MarijkeZondag 26:ac5656aa35c7 182
MarijkeZondag 26:ac5656aa35c7 183 //------------------ Encoder motor 2 --------------------------------//
MarijkeZondag 26:ac5656aa35c7 184
MarijkeZondag 26:ac5656aa35c7 185 void encoderA2_rise()
MarijkeZondag 26:ac5656aa35c7 186 {
MarijkeZondag 26:ac5656aa35c7 187 if(encoderB2==false)
MarijkeZondag 26:ac5656aa35c7 188 {
MarijkeZondag 26:ac5656aa35c7 189 encoder2++;
MarijkeZondag 26:ac5656aa35c7 190 }
MarijkeZondag 26:ac5656aa35c7 191 else
MarijkeZondag 26:ac5656aa35c7 192 {
MarijkeZondag 26:ac5656aa35c7 193 encoder2--;
MarijkeZondag 26:ac5656aa35c7 194 }
MarijkeZondag 26:ac5656aa35c7 195 }
MarijkeZondag 12:eaed305a76c3 196
MarijkeZondag 26:ac5656aa35c7 197 void encoderA2_fall()
MarijkeZondag 26:ac5656aa35c7 198 {
MarijkeZondag 26:ac5656aa35c7 199 if(encoderB2==true)
MarijkeZondag 26:ac5656aa35c7 200 {
MarijkeZondag 26:ac5656aa35c7 201 encoder2++;
MarijkeZondag 26:ac5656aa35c7 202 }
MarijkeZondag 26:ac5656aa35c7 203 else
MarijkeZondag 26:ac5656aa35c7 204 {
MarijkeZondag 26:ac5656aa35c7 205 encoder2--;
MarijkeZondag 26:ac5656aa35c7 206 }
MarijkeZondag 26:ac5656aa35c7 207 }
MarijkeZondag 26:ac5656aa35c7 208
MarijkeZondag 26:ac5656aa35c7 209 void encoderB2_rise()
MarijkeZondag 26:ac5656aa35c7 210 {
MarijkeZondag 26:ac5656aa35c7 211 if(encoderA2==true)
MarijkeZondag 26:ac5656aa35c7 212 {
MarijkeZondag 26:ac5656aa35c7 213 encoder2++;
MarijkeZondag 26:ac5656aa35c7 214 }
MarijkeZondag 26:ac5656aa35c7 215 else
MarijkeZondag 26:ac5656aa35c7 216 {
MarijkeZondag 26:ac5656aa35c7 217 encoder2--;
MarijkeZondag 26:ac5656aa35c7 218 }
MarijkeZondag 26:ac5656aa35c7 219 }
MarijkeZondag 26:ac5656aa35c7 220
MarijkeZondag 26:ac5656aa35c7 221 void encoderB2_fall()
MarijkeZondag 26:ac5656aa35c7 222 {
MarijkeZondag 26:ac5656aa35c7 223 if(encoderA2==false)
MarijkeZondag 26:ac5656aa35c7 224 {
MarijkeZondag 26:ac5656aa35c7 225 encoder2++;
MarijkeZondag 26:ac5656aa35c7 226 }
MarijkeZondag 26:ac5656aa35c7 227 else
MarijkeZondag 26:ac5656aa35c7 228 {
MarijkeZondag 26:ac5656aa35c7 229 encoder2--;
MarijkeZondag 26:ac5656aa35c7 230 }
MarijkeZondag 26:ac5656aa35c7 231 }
MarijkeZondag 26:ac5656aa35c7 232
MarijkeZondag 26:ac5656aa35c7 233 void encoder_count2()
MarijkeZondag 26:ac5656aa35c7 234 {
MarijkeZondag 26:ac5656aa35c7 235 encoderA2.rise(&encoderA2_rise);
MarijkeZondag 26:ac5656aa35c7 236 encoderA2.fall(&encoderA2_fall);
MarijkeZondag 26:ac5656aa35c7 237 encoderB2.rise(&encoderB2_rise);
MarijkeZondag 26:ac5656aa35c7 238 encoderB2.fall(&encoderB2_fall);
MarijkeZondag 26:ac5656aa35c7 239 }
MarijkeZondag 26:ac5656aa35c7 240
MarijkeZondag 26:ac5656aa35c7 241 //------------------ Filter EMG + Calibration EMG --------------------------------//
MarijkeZondag 26:ac5656aa35c7 242
MarijkeZondag 26:ac5656aa35c7 243 void EMGFilter0()
MarijkeZondag 26:ac5656aa35c7 244 {
MarijkeZondag 26:ac5656aa35c7 245 emg0_raw = emg0_in.read(); //give name to raw EMG0 data calve
MarijkeZondag 26:ac5656aa35c7 246 emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 26:ac5656aa35c7 247 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 26:ac5656aa35c7 248 }
MarijkeZondag 26:ac5656aa35c7 249
MarijkeZondag 26:ac5656aa35c7 250 void EMGFilter1()
MarijkeZondag 26:ac5656aa35c7 251 {
MarijkeZondag 26:ac5656aa35c7 252 emg1_raw = emg1_in.read(); //give name to raw EMG1 data bicep 1
MarijkeZondag 26:ac5656aa35c7 253 emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 26:ac5656aa35c7 254 emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
MarijkeZondag 26:ac5656aa35c7 255 }
MarijkeZondag 26:ac5656aa35c7 256
MarijkeZondag 26:ac5656aa35c7 257 void EMGFilter2()
MarijkeZondag 26:ac5656aa35c7 258 {
MarijkeZondag 26:ac5656aa35c7 259 emg2_raw = emg2_in.read(); //Give name to raw EMG1 data bicep 2
MarijkeZondag 26:ac5656aa35c7 260 emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 26:ac5656aa35c7 261 emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier.
MarijkeZondag 26:ac5656aa35c7 262 }
MarijkeZondag 26:ac5656aa35c7 263
MarijkeZondag 26:ac5656aa35c7 264 void MovAg() //Calculate moving average (MovAg)
MarijkeZondag 26:ac5656aa35c7 265 {
MarijkeZondag 26:ac5656aa35c7 266 for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals
MarijkeZondag 26:ac5656aa35c7 267 {
MarijkeZondag 26:ac5656aa35c7 268 StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average.
MarijkeZondag 26:ac5656aa35c7 269 StoreArray1[i] = StoreArray1[i-1];
MarijkeZondag 26:ac5656aa35c7 270 StoreArray2[i] = StoreArray2[i-1];
MarijkeZondag 26:ac5656aa35c7 271 }
MarijkeZondag 26:ac5656aa35c7 272
MarijkeZondag 26:ac5656aa35c7 273 StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array
MarijkeZondag 26:ac5656aa35c7 274 StoreArray1[0] = emg1_filt;
MarijkeZondag 26:ac5656aa35c7 275 StoreArray2[0] = emg2_filt;
MarijkeZondag 26:ac5656aa35c7 276
MarijkeZondag 26:ac5656aa35c7 277 sum1 = 0.0;
MarijkeZondag 26:ac5656aa35c7 278 sum2 = 0.0;
MarijkeZondag 26:ac5656aa35c7 279 sum3 = 0.0;
MarijkeZondag 26:ac5656aa35c7 280
MarijkeZondag 26:ac5656aa35c7 281 for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays
MarijkeZondag 26:ac5656aa35c7 282 {
MarijkeZondag 26:ac5656aa35c7 283 sum1 += StoreArray0[a];
MarijkeZondag 26:ac5656aa35c7 284 sum2 += StoreArray1[a];
MarijkeZondag 26:ac5656aa35c7 285 sum3 += StoreArray2[a];
MarijkeZondag 26:ac5656aa35c7 286 }
MarijkeZondag 26:ac5656aa35c7 287
MarijkeZondag 26:ac5656aa35c7 288 movAg0 = sum1/windowsize; //calculates an average in the array
MarijkeZondag 26:ac5656aa35c7 289 movAg1 = sum2/windowsize;
MarijkeZondag 26:ac5656aa35c7 290 movAg2 = sum3/windowsize;
MarijkeZondag 26:ac5656aa35c7 291 }
MarijkeZondag 26:ac5656aa35c7 292
MarijkeZondag 26:ac5656aa35c7 293 void emg_filtered() //Call all filter functions
MarijkeZondag 26:ac5656aa35c7 294 {
MarijkeZondag 26:ac5656aa35c7 295 EMGFilter0();
MarijkeZondag 26:ac5656aa35c7 296 EMGFilter1();
MarijkeZondag 26:ac5656aa35c7 297 EMGFilter2();
MarijkeZondag 27:fa493551be99 298 MovAg();
MarijkeZondag 26:ac5656aa35c7 299 }
MarijkeZondag 13:a3d4b4daf5b4 300 void switch_to_calibrate()
MarijkeZondag 13:a3d4b4daf5b4 301 {
MarijkeZondag 22:5d956c93b3ae 302 x++; //Every time function gets called, x increases. Every button press --> new calibration state.
MarijkeZondag 22:5d956c93b3ae 303 //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 304
MarijkeZondag 13:a3d4b4daf5b4 305 if(x==0) //If x = 0, led is red
MarijkeZondag 13:a3d4b4daf5b4 306 {
MarijkeZondag 17:741798018c0d 307 ledr = 0;
MarijkeZondag 17:741798018c0d 308 ledb = 1;
MarijkeZondag 17:741798018c0d 309 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 310 }
MarijkeZondag 13:a3d4b4daf5b4 311 else if (x==1) //If x = 1, led is blue
MarijkeZondag 13:a3d4b4daf5b4 312 {
MarijkeZondag 17:741798018c0d 313 ledr = 1;
MarijkeZondag 17:741798018c0d 314 ledb = 0;
MarijkeZondag 17:741798018c0d 315 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 316 }
MarijkeZondag 22:5d956c93b3ae 317 else if (x==2) //If x = 2, led is green
MarijkeZondag 13:a3d4b4daf5b4 318 {
MarijkeZondag 17:741798018c0d 319 ledr = 1;
MarijkeZondag 17:741798018c0d 320 ledb = 1;
MarijkeZondag 17:741798018c0d 321 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 322 }
MarijkeZondag 22:5d956c93b3ae 323 else //If x = 3 or 4, led is white
MarijkeZondag 13:a3d4b4daf5b4 324 {
MarijkeZondag 17:741798018c0d 325 ledr = 0;
MarijkeZondag 17:741798018c0d 326 ledb = 0;
MarijkeZondag 17:741798018c0d 327 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 328 }
MarijkeZondag 13:a3d4b4daf5b4 329
MarijkeZondag 22:5d956c93b3ae 330 if(x==4) //Reset back to x = -1
MarijkeZondag 13:a3d4b4daf5b4 331 {
MarijkeZondag 16:5f7196ddc77b 332 x = -1;
MarijkeZondag 23:97a976a8f0fc 333 emg_cal=0; //reset, motors off
MarijkeZondag 13:a3d4b4daf5b4 334 }
MarijkeZondag 13:a3d4b4daf5b4 335 }
MarijkeZondag 13:a3d4b4daf5b4 336
MarijkeZondag 13:a3d4b4daf5b4 337
MarijkeZondag 13:a3d4b4daf5b4 338 void calibrate(void)
MarijkeZondag 12:eaed305a76c3 339 {
MarijkeZondag 13:a3d4b4daf5b4 340 switch(x)
MarijkeZondag 13:a3d4b4daf5b4 341 {
MarijkeZondag 22:5d956c93b3ae 342 case 0: //If calibration state 0:
MarijkeZondag 13:a3d4b4daf5b4 343 {
MarijkeZondag 13:a3d4b4daf5b4 344 sum = 0.0;
MarijkeZondag 22:5d956c93b3ae 345 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0
MarijkeZondag 13:a3d4b4daf5b4 346 {
MarijkeZondag 22:5d956c93b3ae 347 StoreCal0[j] = emg0_filt;
MarijkeZondag 13:a3d4b4daf5b4 348 sum+=StoreCal0[j];
MarijkeZondag 22:5d956c93b3ae 349 wait(0.001f); //Does there need to be a wait?
MarijkeZondag 13:a3d4b4daf5b4 350 }
MarijkeZondag 22:5d956c93b3ae 351 Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples)
MarijkeZondag 23:97a976a8f0fc 352 Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean
MarijkeZondag 22:5d956c93b3ae 353 break; //Stop. Threshold is calculated, we will use this further in the code
MarijkeZondag 13:a3d4b4daf5b4 354 }
MarijkeZondag 22:5d956c93b3ae 355 case 1: //If calibration state 1:
MarijkeZondag 13:a3d4b4daf5b4 356 {
MarijkeZondag 22:5d956c93b3ae 357 sum = 0.0;
MarijkeZondag 22:5d956c93b3ae 358 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1
MarijkeZondag 13:a3d4b4daf5b4 359 {
MarijkeZondag 22:5d956c93b3ae 360 StoreCal1[j] = emg1_filt;
MarijkeZondag 13:a3d4b4daf5b4 361 sum+=StoreCal1[j];
MarijkeZondag 21:1da43fdbd254 362 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 363 }
MarijkeZondag 13:a3d4b4daf5b4 364 Mean1 = sum/sizeCal;
MarijkeZondag 23:97a976a8f0fc 365 Threshold1 = Mean1/2;
MarijkeZondag 13:a3d4b4daf5b4 366 break;
MarijkeZondag 13:a3d4b4daf5b4 367 }
MarijkeZondag 22:5d956c93b3ae 368 case 2: //If calibration state 2:
MarijkeZondag 13:a3d4b4daf5b4 369 {
MarijkeZondag 13:a3d4b4daf5b4 370 sum = 0.0;
MarijkeZondag 22:5d956c93b3ae 371 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2
MarijkeZondag 13:a3d4b4daf5b4 372 {
MarijkeZondag 23:97a976a8f0fc 373 StoreCal2[j] = emg2_filt;
MarijkeZondag 13:a3d4b4daf5b4 374 sum+=StoreCal2[j];
MarijkeZondag 21:1da43fdbd254 375 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 376 }
MarijkeZondag 13:a3d4b4daf5b4 377 Mean2 = sum/sizeCal;
MarijkeZondag 23:97a976a8f0fc 378 Threshold2 = Mean2/2;
MarijkeZondag 13:a3d4b4daf5b4 379 break;
MarijkeZondag 13:a3d4b4daf5b4 380 }
MarijkeZondag 22:5d956c93b3ae 381 case 3: //EMG is calibrated, robot can be set to Home position.
MarijkeZondag 13:a3d4b4daf5b4 382 {
MarijkeZondag 22:5d956c93b3ae 383 emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!)
MarijkeZondag 27:fa493551be99 384
MarijkeZondag 21:1da43fdbd254 385 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 386 break;
MarijkeZondag 13:a3d4b4daf5b4 387 }
MarijkeZondag 22:5d956c93b3ae 388 default: //Ensures nothing happens if x is not 0,1 or 2.
MarijkeZondag 13:a3d4b4daf5b4 389 {
MarijkeZondag 13:a3d4b4daf5b4 390 break;
MarijkeZondag 13:a3d4b4daf5b4 391 }
MarijkeZondag 13:a3d4b4daf5b4 392 }
MarijkeZondag 12:eaed305a76c3 393 }
MarijkeZondag 26:ac5656aa35c7 394 /*
MarijkeZondag 22:5d956c93b3ae 395 void HIDScope_sample()
MarijkeZondag 22:5d956c93b3ae 396 {
MarijkeZondag 26:ac5656aa35c7 397 scope.set(0,emg0_raw);
MarijkeZondag 26:ac5656aa35c7 398 scope.set(1,emg0_filt);
MarijkeZondag 26:ac5656aa35c7 399 scope.set(1,movAg0); //als moving average werkt
MarijkeZondag 26:ac5656aa35c7 400 scope.set(2,emg1_raw);
MarijkeZondag 26:ac5656aa35c7 401 scope.set(3,emg1_filt);
MarijkeZondag 26:ac5656aa35c7 402 scope.set(3,movAg1); //als moving average werkt
MarijkeZondag 26:ac5656aa35c7 403 scope.set(4,emg2_raw);
MarijkeZondag 26:ac5656aa35c7 404 scope.set(5,emg2_filt);
MarijkeZondag 26:ac5656aa35c7 405 scope.set(5,movAg2); //als moving average werkt
MarijkeZondag 22:5d956c93b3ae 406
MarijkeZondag 26:ac5656aa35c7 407 scope.send(); //Send data to HIDScope server
MarijkeZondag 22:5d956c93b3ae 408 }
MarijkeZondag 26:ac5656aa35c7 409 */
MarijkeZondag 8:895d941a5910 410
MarijkeZondag 26:ac5656aa35c7 411 //------------------ Inversed Kinematics --------------------------------//
MarijkeZondag 8:895d941a5910 412
Marle 25:bbef09ff226b 413 void inverse_kinematics()
Marle 25:bbef09ff226b 414 {
Marle 25:bbef09ff226b 415 Lq1 = q1ref*r_trans;
Marle 25:bbef09ff226b 416 Cq2 = q2ref/5.0;
Marle 25:bbef09ff226b 417
Marle 25:bbef09ff226b 418 q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2));
Marle 25:bbef09ff226b 419 q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2));
Marle 25:bbef09ff226b 420
Marle 25:bbef09ff226b 421 q1_ii = q1ref + q1_dot*T;
Marle 25:bbef09ff226b 422 q2_ii = q2ref + q2_dot*T;
Marle 25:bbef09ff226b 423
Marle 25:bbef09ff226b 424 q1ref = q1_ii;
Marle 25:bbef09ff226b 425 q2ref = q2_ii;
MarijkeZondag 27:fa493551be99 426
MarijkeZondag 27:fa493551be99 427 start_control = 1;
Marle 25:bbef09ff226b 428 }
Marle 25:bbef09ff226b 429
Marle 25:bbef09ff226b 430 void v_des_calculate_qref()
MarijkeZondag 23:97a976a8f0fc 431 {
MarijkeZondag 27:fa493551be99 432 if(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
MarijkeZondag 16:5f7196ddc77b 433 {
MarijkeZondag 26:ac5656aa35c7 434 if(movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn
MarijkeZondag 16:5f7196ddc77b 435 {
MarijkeZondag 26:ac5656aa35c7 436 v_x = 1.0; //beweging in +x direction
MarijkeZondag 26:ac5656aa35c7 437 ledr = 0; //red
Marle 25:bbef09ff226b 438 ledb = 1;
Marle 25:bbef09ff226b 439 ledg = 1;
MarijkeZondag 16:5f7196ddc77b 440 }
MarijkeZondag 26:ac5656aa35c7 441 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 442 {
MarijkeZondag 26:ac5656aa35c7 443 v_y = 1.0; //beweging in +y direction
MarijkeZondag 26:ac5656aa35c7 444 ledr = 1; //green
Marle 25:bbef09ff226b 445 ledb = 1;
Marle 25:bbef09ff226b 446 ledg = 0;
MarijkeZondag 24:6d63ad38fef7 447 }
Marle 25:bbef09ff226b 448
MarijkeZondag 26:ac5656aa35c7 449 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 450 {
Marle 25:bbef09ff226b 451 v_x = -v_x;
Marle 25:bbef09ff226b 452 v_y = -v_y;
MarijkeZondag 26:ac5656aa35c7 453 ledr = 1; //Blue
Marle 25:bbef09ff226b 454 ledb = 0;
Marle 25:bbef09ff226b 455 ledg = 1;
Marle 25:bbef09ff226b 456 }
MarijkeZondag 26:ac5656aa35c7 457 else //If not higher than the threshold, motors will not turn at all
Marle 25:bbef09ff226b 458 {
Marle 25:bbef09ff226b 459 v_x = 0;
Marle 25:bbef09ff226b 460 v_y = 0;
MarijkeZondag 26:ac5656aa35c7 461 ledr = 0; //white
MarijkeZondag 24:6d63ad38fef7 462 ledb = 0;
MarijkeZondag 24:6d63ad38fef7 463 ledg = 0;
MarijkeZondag 24:6d63ad38fef7 464 }
MarijkeZondag 27:fa493551be99 465
MarijkeZondag 27:fa493551be99 466 inverse_kinematics(); //Call inverse kinematics function
MarijkeZondag 27:fa493551be99 467
MarijkeZondag 23:97a976a8f0fc 468 }
MarijkeZondag 26:ac5656aa35c7 469 }
MarijkeZondag 26:ac5656aa35c7 470
MarijkeZondag 26:ac5656aa35c7 471 //---------PID controller motor 1 + start motor 1 -----------------------------------------------------------//
MarijkeZondag 26:ac5656aa35c7 472 double PID_controller1(double err1)
MarijkeZondag 26:ac5656aa35c7 473 {
MarijkeZondag 26:ac5656aa35c7 474 static double err_integral1 = 0;
MarijkeZondag 26:ac5656aa35c7 475 static double err_prev1 = err1; // initialization with this value only done once!
MarijkeZondag 26:ac5656aa35c7 476
MarijkeZondag 26:ac5656aa35c7 477 static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MarijkeZondag 26:ac5656aa35c7 478
MarijkeZondag 26:ac5656aa35c7 479 // Proportional part:
MarijkeZondag 26:ac5656aa35c7 480 double u_k1 = Kp1 * err1;
MarijkeZondag 26:ac5656aa35c7 481
MarijkeZondag 27:fa493551be99 482 // Integral part
MarijkeZondag 26:ac5656aa35c7 483 err_integral1 = err_integral1 + err1 * T;
MarijkeZondag 26:ac5656aa35c7 484 double u_i1 = Ki1 * err_integral1;
MarijkeZondag 26:ac5656aa35c7 485
MarijkeZondag 26:ac5656aa35c7 486 // Derivative part
MarijkeZondag 26:ac5656aa35c7 487 double err_derivative1 = (err1 - err_prev1)/T;
MarijkeZondag 26:ac5656aa35c7 488 double filtered_err_derivative1 = LowPassFilter1.step(err_derivative1);
MarijkeZondag 26:ac5656aa35c7 489 double u_d1 = Kd1 * filtered_err_derivative1;
MarijkeZondag 26:ac5656aa35c7 490 err_prev1 = err1;
MarijkeZondag 26:ac5656aa35c7 491
MarijkeZondag 26:ac5656aa35c7 492 // Sum all parts and return it
MarijkeZondag 26:ac5656aa35c7 493 return u_k1 + u_i1 + u_d1;
MarijkeZondag 26:ac5656aa35c7 494 }
MarijkeZondag 26:ac5656aa35c7 495
MarijkeZondag 26:ac5656aa35c7 496 void start_your_engines1(double u1)
MarijkeZondag 26:ac5656aa35c7 497 {
MarijkeZondag 26:ac5656aa35c7 498 if(encoder1<5250 && encoder1>-5250) //limits rotation, in counts
MarijkeZondag 26:ac5656aa35c7 499 {
MarijkeZondag 26:ac5656aa35c7 500 pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
MarijkeZondag 26:ac5656aa35c7 501 directionpin1.write(u1 < 0.0f);
MarijkeZondag 26:ac5656aa35c7 502 }
MarijkeZondag 26:ac5656aa35c7 503 else
MarijkeZondag 26:ac5656aa35c7 504 {
MarijkeZondag 26:ac5656aa35c7 505 pwmpin1 = 0;
MarijkeZondag 26:ac5656aa35c7 506 }
MarijkeZondag 26:ac5656aa35c7 507 }
MarijkeZondag 26:ac5656aa35c7 508
MarijkeZondag 27:fa493551be99 509 void engine_control1() //Engine 1 is rotational engine, connected with left side pins
MarijkeZondag 26:ac5656aa35c7 510 {
MarijkeZondag 27:fa493551be99 511 while(start_control == 1)
MarijkeZondag 27:fa493551be99 512 {
MarijkeZondag 27:fa493551be99 513 encoder_radians1 = encoder1*(2*PI)/8400;
MarijkeZondag 27:fa493551be99 514 double err1 = q1ref - encoder_radians1;
MarijkeZondag 27:fa493551be99 515 double u1 = PID_controller1(err1); //PID controller function call
MarijkeZondag 27:fa493551be99 516 start_your_engines1(u1); //Call start_your_engines function
MarijkeZondag 27:fa493551be99 517
MarijkeZondag 27:fa493551be99 518 break;
MarijkeZondag 27:fa493551be99 519 }
MarijkeZondag 23:97a976a8f0fc 520 }
MarijkeZondag 23:97a976a8f0fc 521
MarijkeZondag 23:97a976a8f0fc 522
MarijkeZondag 26:ac5656aa35c7 523
MarijkeZondag 26:ac5656aa35c7 524 //---------PID controller motor 1 + start motor 1 -----------------------------------------------------------//
MarijkeZondag 26:ac5656aa35c7 525 double PID_controller2(double err2)
MarijkeZondag 26:ac5656aa35c7 526 {
MarijkeZondag 26:ac5656aa35c7 527 static double err_integral2 = 0;
MarijkeZondag 26:ac5656aa35c7 528 static double err_prev2 = err2; // initialization with this value only done once!
MarijkeZondag 26:ac5656aa35c7 529
MarijkeZondag 26:ac5656aa35c7 530 static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MarijkeZondag 26:ac5656aa35c7 531
MarijkeZondag 26:ac5656aa35c7 532 // Proportional part:
MarijkeZondag 26:ac5656aa35c7 533 double u_k2 = Kp2 * err2;
MarijkeZondag 26:ac5656aa35c7 534
MarijkeZondag 26:ac5656aa35c7 535 // Integral part
MarijkeZondag 26:ac5656aa35c7 536 err_integral2 = err_integral2 + err2 * T;
MarijkeZondag 26:ac5656aa35c7 537 double u_i2 = Ki2 * err_integral2;
MarijkeZondag 26:ac5656aa35c7 538
MarijkeZondag 26:ac5656aa35c7 539 // Derivative part
MarijkeZondag 26:ac5656aa35c7 540 double err_derivative2 = (err2 - err_prev2)/T;
MarijkeZondag 26:ac5656aa35c7 541 double filtered_err_derivative2 = LowPassFilter2.step(err_derivative2);
MarijkeZondag 26:ac5656aa35c7 542 double u_d2 = Kd2 * filtered_err_derivative2;
MarijkeZondag 26:ac5656aa35c7 543 err_prev2 = err2;
MarijkeZondag 26:ac5656aa35c7 544
MarijkeZondag 26:ac5656aa35c7 545 // Sum all parts and return it
MarijkeZondag 26:ac5656aa35c7 546 return u_k2 + u_i2 + u_d2;
MarijkeZondag 26:ac5656aa35c7 547 }
MarijkeZondag 26:ac5656aa35c7 548
MarijkeZondag 26:ac5656aa35c7 549 void start_your_engines2(double u2)
MarijkeZondag 26:ac5656aa35c7 550 {
MarijkeZondag 27:fa493551be99 551 if(encoder2<12600 && encoder2>-1) //limits translation in counts
MarijkeZondag 27:fa493551be99 552 {
MarijkeZondag 27:fa493551be99 553 pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
MarijkeZondag 27:fa493551be99 554 directionpin2.write(u2 < 0.0f);
MarijkeZondag 27:fa493551be99 555 }
MarijkeZondag 26:ac5656aa35c7 556 else
MarijkeZondag 27:fa493551be99 557 {
MarijkeZondag 26:ac5656aa35c7 558 pwmpin2 = 0;
MarijkeZondag 27:fa493551be99 559 }
MarijkeZondag 27:fa493551be99 560
MarijkeZondag 26:ac5656aa35c7 561 }
MarijkeZondag 26:ac5656aa35c7 562
MarijkeZondag 26:ac5656aa35c7 563 void engine_control2() //Engine 2 is translational engine, connected with right side wires
MarijkeZondag 26:ac5656aa35c7 564 {
MarijkeZondag 27:fa493551be99 565 while(start_control == 1)
MarijkeZondag 27:fa493551be99 566 {
MarijkeZondag 27:fa493551be99 567 encoder_radians2 = encoder2*(2*PI)/8400;
MarijkeZondag 27:fa493551be99 568 double err2 = q2ref - encoder_radians2;
MarijkeZondag 27:fa493551be99 569 double u2 = PID_controller2(err2); //PID controller function call
MarijkeZondag 27:fa493551be99 570 start_your_engines2(u2); //Call start_your_engines function
MarijkeZondag 27:fa493551be99 571
MarijkeZondag 27:fa493551be99 572 break;
MarijkeZondag 27:fa493551be99 573 }
MarijkeZondag 26:ac5656aa35c7 574 }
MarijkeZondag 26:ac5656aa35c7 575
MarijkeZondag 26:ac5656aa35c7 576 //------------------ Start main function --------------------------//
MarijkeZondag 26:ac5656aa35c7 577
MarijkeZondag 26:ac5656aa35c7 578
MarijkeZondag 23:97a976a8f0fc 579 int main()
MarijkeZondag 23:97a976a8f0fc 580 {
MarijkeZondag 23:97a976a8f0fc 581 pc.baud(115200);
MarijkeZondag 26:ac5656aa35c7 582 pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
MarijkeZondag 26:ac5656aa35c7 583 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 23:97a976a8f0fc 584
MarijkeZondag 23:97a976a8f0fc 585 emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( &notch1 ); //attach biquad elements to chain
MarijkeZondag 23:97a976a8f0fc 586 emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
MarijkeZondag 23:97a976a8f0fc 587 emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );
MarijkeZondag 10:39ec51206c8b 588
MarijkeZondag 23:97a976a8f0fc 589 while(true)
MarijkeZondag 23:97a976a8f0fc 590 {
MarijkeZondag 27:fa493551be99 591 ticker.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
MarijkeZondag 26:ac5656aa35c7 592 ticker.attach(&v_des_calculate_qref,T); //v_des determined every T
Marle 25:bbef09ff226b 593
MarijkeZondag 26:ac5656aa35c7 594 // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
MarijkeZondag 23:97a976a8f0fc 595
MarijkeZondag 26:ac5656aa35c7 596 button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
MarijkeZondag 26:ac5656aa35c7 597 wait(0.2f); //Wait to avoid bouncing of button
MarijkeZondag 26:ac5656aa35c7 598 button2.rise(calibrate); //Calibrate threshold for 3 muscles
MarijkeZondag 26:ac5656aa35c7 599 wait(0.2f); //Wait to avoid bouncing of button
MarijkeZondag 23:97a976a8f0fc 600
MarijkeZondag 26:ac5656aa35c7 601 pc.printf("x is %i\n\r",x);
MarijkeZondag 26:ac5656aa35c7 602 pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2);
MarijkeZondag 26:ac5656aa35c7 603 pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
MarijkeZondag 26:ac5656aa35c7 604 //wait(2.0f);
MarijkeZondag 23:97a976a8f0fc 605 }
vsluiter 0:c8f15874531b 606 }