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@19:466ada92bf65, 2018-10-22 (annotated)

Committer:
MarijkeZondag
Date:
Mon Oct 22 12:57:28 2018 +0000
Revision:
19:466ada92bf65
Parent:
18:898f54c6aa3d
Child:
20:bd9495b31f50
hidscope werkend hopelijk;

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 10:39ec51206c8b 7 AnalogIn emg0_in (A0);
MarijkeZondag 10:39ec51206c8b 8 AnalogIn emg1_in (A1);
MarijkeZondag 10:39ec51206c8b 9 AnalogIn emg2_in (A2);
MarijkeZondag 10:39ec51206c8b 10
MarijkeZondag 16:5f7196ddc77b 11 InterruptIn button1 (D10); //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten.
MarijkeZondag 16:5f7196ddc77b 12 InterruptIn button2 (D11);
MarijkeZondag 9:c722418997b5 13 InterruptIn encoderA (D9);
MarijkeZondag 9:c722418997b5 14 InterruptIn encoderB (D8);
MarijkeZondag 6:f4bbb73f3989 15
MarijkeZondag 9:c722418997b5 16 DigitalOut directionpin1 (D4);
MarijkeZondag 9:c722418997b5 17 DigitalOut directionpin2 (D7);
MarijkeZondag 17:741798018c0d 18 DigitalOut ledr (LED_RED);
MarijkeZondag 17:741798018c0d 19 DigitalOut ledb (LED_BLUE);
MarijkeZondag 17:741798018c0d 20 DigitalOut ledg (LED_GREEN);
MarijkeZondag 13:a3d4b4daf5b4 21
MarijkeZondag 9:c722418997b5 22 PwmOut pwmpin1 (D5);
MarijkeZondag 9:c722418997b5 23 PwmOut pwmpin2 (D6);
MarijkeZondag 9:c722418997b5 24
MarijkeZondag 6:f4bbb73f3989 25
vsluiter 0:c8f15874531b 26 MODSERIAL pc(USBTX, USBRX);
vsluiter 0:c8f15874531b 27
MarijkeZondag 19:466ada92bf65 28 //HIDscope
MarijkeZondag 19:466ada92bf65 29 Ticker sample_timer;
MarijkeZondag 19:466ada92bf65 30 HIDScope scope( 2 );
MarijkeZondag 19:466ada92bf65 31 DigitalOut led(LED1);
MarijkeZondag 9:c722418997b5 32
MarijkeZondag 9:c722418997b5 33 //Global variables
MarijkeZondag 13:a3d4b4daf5b4 34 int encoder = 0; //Starting point encoder
MarijkeZondag 12:eaed305a76c3 35 const float T = 0.001f; //Ticker period
MarijkeZondag 10:39ec51206c8b 36
MarijkeZondag 13:a3d4b4daf5b4 37 //EMG filter
MarijkeZondag 13:a3d4b4daf5b4 38 double emgfilter0, emgfilter1, emgfilter2; //Filtered EMG data 0, 1 and 2
MarijkeZondag 14:fa09dae67390 39 const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated
MarijkeZondag 13:a3d4b4daf5b4 40 double sum, sum1, sum2, sum3; //variables used to sum elements in array
MarijkeZondag 14:fa09dae67390 41 double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg
MarijkeZondag 13:a3d4b4daf5b4 42 double movAg0,movAg1,movAg2; //outcome of MovAg
MarijkeZondag 13:a3d4b4daf5b4 43
MarijkeZondag 13:a3d4b4daf5b4 44 //calibration
MarijkeZondag 18:898f54c6aa3d 45 static int x = -1;
MarijkeZondag 13:a3d4b4daf5b4 46 int emg_cal = 0;
MarijkeZondag 13:a3d4b4daf5b4 47 const int sizeCal = 2000;
MarijkeZondag 14:fa09dae67390 48 double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal];
MarijkeZondag 14:fa09dae67390 49 double Mean0,Mean1,Mean2;
MarijkeZondag 13:a3d4b4daf5b4 50 double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1;
MarijkeZondag 13:a3d4b4daf5b4 51
MarijkeZondag 13:a3d4b4daf5b4 52
MarijkeZondag 10:39ec51206c8b 53 //Biquad
MarijkeZondag 10:39ec51206c8b 54 BiQuadChain emg0band;
MarijkeZondag 10:39ec51206c8b 55 BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 56 BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 57 BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 10:39ec51206c8b 58
MarijkeZondag 10:39ec51206c8b 59 BiQuadChain emg1band;
MarijkeZondag 10:39ec51206c8b 60 BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 61 BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 62 BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 10:39ec51206c8b 63
MarijkeZondag 10:39ec51206c8b 64 BiQuadChain emg2band;
MarijkeZondag 10:39ec51206c8b 65 BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 66 BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 67 BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 10:39ec51206c8b 68
MarijkeZondag 10:39ec51206c8b 69 BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 14:fa09dae67390 70 BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 14:fa09dae67390 71 BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 72
MarijkeZondag 10:39ec51206c8b 73
MarijkeZondag 10:39ec51206c8b 74 //Tickers
MarijkeZondag 12:eaed305a76c3 75 Ticker filter_tick;
MarijkeZondag 12:eaed305a76c3 76 Ticker MovAg_tick;
MarijkeZondag 9:c722418997b5 77
MarijkeZondag 9:c722418997b5 78 //Functions
MarijkeZondag 19:466ada92bf65 79 void sample()
MarijkeZondag 19:466ada92bf65 80 {
MarijkeZondag 19:466ada92bf65 81 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
MarijkeZondag 19:466ada92bf65 82 scope.set(0, emg0_in.read() );
MarijkeZondag 19:466ada92bf65 83 scope.set(1, emg1_in.read() );
MarijkeZondag 19:466ada92bf65 84 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
MarijkeZondag 19:466ada92bf65 85 * Ensure that enough channels are available (HIDScope scope( 2 ))
MarijkeZondag 19:466ada92bf65 86 * Finally, send all channels to the PC at once */
MarijkeZondag 19:466ada92bf65 87 scope.send();
MarijkeZondag 19:466ada92bf65 88 /* To indicate that the function is working, the LED is toggled */
MarijkeZondag 19:466ada92bf65 89 led = !led;
MarijkeZondag 19:466ada92bf65 90 }
MarijkeZondag 19:466ada92bf65 91
MarijkeZondag 10:39ec51206c8b 92 void EMGFilter0()
MarijkeZondag 10:39ec51206c8b 93 {
MarijkeZondag 11:b95b0e9e1b89 94 double emg0 = emg0_in.read();
MarijkeZondag 11:b95b0e9e1b89 95 double bandpass0 = emg0band.step(emg0);
MarijkeZondag 11:b95b0e9e1b89 96 double absolute0 = fabs(bandpass0);
MarijkeZondag 11:b95b0e9e1b89 97 double notch0 = notch1.step(absolute0);
MarijkeZondag 10:39ec51206c8b 98 }
MarijkeZondag 10:39ec51206c8b 99
MarijkeZondag 10:39ec51206c8b 100 void EMGFilter1()
MarijkeZondag 10:39ec51206c8b 101 {
MarijkeZondag 11:b95b0e9e1b89 102 double emg1 = emg1_in.read();
MarijkeZondag 11:b95b0e9e1b89 103 double bandpass1 = emg1band.step(emg1);
MarijkeZondag 11:b95b0e9e1b89 104 double absolute1 = fabs(bandpass1);
MarijkeZondag 14:fa09dae67390 105 double notch1 = notch2.step(absolute1);
MarijkeZondag 10:39ec51206c8b 106 }
MarijkeZondag 10:39ec51206c8b 107
MarijkeZondag 10:39ec51206c8b 108 void EMGFilter2()
MarijkeZondag 10:39ec51206c8b 109 {
MarijkeZondag 11:b95b0e9e1b89 110 double emg2 = emg2_in.read();
MarijkeZondag 11:b95b0e9e1b89 111 double bandpass2 = emg2band.step(emg2);
MarijkeZondag 11:b95b0e9e1b89 112 double absolute2 = fabs(bandpass2);
MarijkeZondag 14:fa09dae67390 113 double notch2 = notch3.step(absolute2);
MarijkeZondag 10:39ec51206c8b 114 }
MarijkeZondag 10:39ec51206c8b 115
MarijkeZondag 13:a3d4b4daf5b4 116 void MovAg() //Calculate moving average (MovAg)
MarijkeZondag 13:a3d4b4daf5b4 117 {
MarijkeZondag 13:a3d4b4daf5b4 118 for (int i = windowsize-1; i>=0; i--) //Make array of the last datapoints of the filtered signal
MarijkeZondag 13:a3d4b4daf5b4 119 {
MarijkeZondag 13:a3d4b4daf5b4 120 StoreArray0[i] = StoreArray0[i-1];
MarijkeZondag 13:a3d4b4daf5b4 121 StoreArray1[i] = StoreArray1[i-1];
MarijkeZondag 13:a3d4b4daf5b4 122 StoreArray2[i] = StoreArray2[i-1];
MarijkeZondag 13:a3d4b4daf5b4 123 }
MarijkeZondag 13:a3d4b4daf5b4 124
MarijkeZondag 13:a3d4b4daf5b4 125 StoreArray0[0] = emgfilter0; //Stores the latest datapoint in the first element of the array
MarijkeZondag 13:a3d4b4daf5b4 126 StoreArray1[0] = emgfilter1;
MarijkeZondag 13:a3d4b4daf5b4 127 StoreArray2[0] = emgfilter2;
MarijkeZondag 13:a3d4b4daf5b4 128
MarijkeZondag 13:a3d4b4daf5b4 129 sum1 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 130 sum2 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 131 sum3 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 132
MarijkeZondag 13:a3d4b4daf5b4 133 for(int a = 0; a<= windowsize-1; a++) //Sum the elements in the array
MarijkeZondag 13:a3d4b4daf5b4 134 {
MarijkeZondag 13:a3d4b4daf5b4 135 sum1 += StoreArray0[a];
MarijkeZondag 13:a3d4b4daf5b4 136 sum2 += StoreArray1[a];
MarijkeZondag 13:a3d4b4daf5b4 137 sum3 += StoreArray2[a];
MarijkeZondag 13:a3d4b4daf5b4 138 }
MarijkeZondag 13:a3d4b4daf5b4 139
MarijkeZondag 13:a3d4b4daf5b4 140 movAg0 = sum1/windowsize; //calculates an average in the array
MarijkeZondag 13:a3d4b4daf5b4 141 movAg1 = sum2/windowsize;
MarijkeZondag 14:fa09dae67390 142 movAg2 = sum3/windowsize;
MarijkeZondag 13:a3d4b4daf5b4 143 }
MarijkeZondag 13:a3d4b4daf5b4 144
MarijkeZondag 13:a3d4b4daf5b4 145 void emg_filtered() //Call all filter functions
MarijkeZondag 12:eaed305a76c3 146 {
MarijkeZondag 12:eaed305a76c3 147 EMGFilter0();
MarijkeZondag 12:eaed305a76c3 148 EMGFilter1();
MarijkeZondag 12:eaed305a76c3 149 EMGFilter2();
MarijkeZondag 13:a3d4b4daf5b4 150 MovAg();
MarijkeZondag 12:eaed305a76c3 151 }
MarijkeZondag 12:eaed305a76c3 152
MarijkeZondag 13:a3d4b4daf5b4 153 void switch_to_calibrate()
MarijkeZondag 13:a3d4b4daf5b4 154 {
MarijkeZondag 13:a3d4b4daf5b4 155 x++;
MarijkeZondag 13:a3d4b4daf5b4 156
MarijkeZondag 13:a3d4b4daf5b4 157 if(x==0) //If x = 0, led is red
MarijkeZondag 13:a3d4b4daf5b4 158 {
MarijkeZondag 17:741798018c0d 159 ledr = 0;
MarijkeZondag 17:741798018c0d 160 ledb = 1;
MarijkeZondag 17:741798018c0d 161 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 162 }
MarijkeZondag 13:a3d4b4daf5b4 163 else if (x==1) //If x = 1, led is blue
MarijkeZondag 13:a3d4b4daf5b4 164 {
MarijkeZondag 17:741798018c0d 165 ledr = 1;
MarijkeZondag 17:741798018c0d 166 ledb = 0;
MarijkeZondag 17:741798018c0d 167 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 168 }
MarijkeZondag 13:a3d4b4daf5b4 169 else if (x == 2) //If x = 2, led is green
MarijkeZondag 13:a3d4b4daf5b4 170 {
MarijkeZondag 17:741798018c0d 171 ledr = 1;
MarijkeZondag 17:741798018c0d 172 ledb = 1;
MarijkeZondag 17:741798018c0d 173 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 174 }
MarijkeZondag 18:898f54c6aa3d 175 else //If x = 3, led is white
MarijkeZondag 13:a3d4b4daf5b4 176 {
MarijkeZondag 17:741798018c0d 177 ledr = 0;
MarijkeZondag 17:741798018c0d 178 ledb = 0;
MarijkeZondag 17:741798018c0d 179 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 180 }
MarijkeZondag 13:a3d4b4daf5b4 181
MarijkeZondag 13:a3d4b4daf5b4 182 if(x>=4) //Reset back to x = 0
MarijkeZondag 13:a3d4b4daf5b4 183 {
MarijkeZondag 16:5f7196ddc77b 184 x = -1;
MarijkeZondag 13:a3d4b4daf5b4 185 }
MarijkeZondag 13:a3d4b4daf5b4 186 }
MarijkeZondag 13:a3d4b4daf5b4 187
MarijkeZondag 13:a3d4b4daf5b4 188
MarijkeZondag 13:a3d4b4daf5b4 189 void calibrate(void)
MarijkeZondag 12:eaed305a76c3 190 {
MarijkeZondag 13:a3d4b4daf5b4 191 switch(x)
MarijkeZondag 13:a3d4b4daf5b4 192 {
MarijkeZondag 13:a3d4b4daf5b4 193 case 0:
MarijkeZondag 13:a3d4b4daf5b4 194 {
MarijkeZondag 13:a3d4b4daf5b4 195 sum = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 196 for(int j = 0; j<=sizeCal-1; j++)
MarijkeZondag 13:a3d4b4daf5b4 197 {
MarijkeZondag 13:a3d4b4daf5b4 198 StoreCal0[j] = emgfilter0;
MarijkeZondag 13:a3d4b4daf5b4 199 sum+=StoreCal0[j];
MarijkeZondag 18:898f54c6aa3d 200 //wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 201 }
MarijkeZondag 13:a3d4b4daf5b4 202 Mean0 = sum/sizeCal;
MarijkeZondag 13:a3d4b4daf5b4 203 Threshold0 = Mean0/2;
MarijkeZondag 13:a3d4b4daf5b4 204 break;
MarijkeZondag 13:a3d4b4daf5b4 205 }
MarijkeZondag 13:a3d4b4daf5b4 206 case 1:
MarijkeZondag 13:a3d4b4daf5b4 207 {
MarijkeZondag 13:a3d4b4daf5b4 208 sum = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 209 for(int j = 0; j<=sizeCal-1; j++)
MarijkeZondag 13:a3d4b4daf5b4 210 {
MarijkeZondag 13:a3d4b4daf5b4 211 StoreCal1[j] = emgfilter1;
MarijkeZondag 13:a3d4b4daf5b4 212 sum+=StoreCal1[j];
MarijkeZondag 18:898f54c6aa3d 213 //wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 214 }
MarijkeZondag 13:a3d4b4daf5b4 215 Mean1 = sum/sizeCal;
MarijkeZondag 13:a3d4b4daf5b4 216 Threshold1 = Mean1/2;
MarijkeZondag 13:a3d4b4daf5b4 217 break;
MarijkeZondag 13:a3d4b4daf5b4 218 }
MarijkeZondag 13:a3d4b4daf5b4 219 case 2:
MarijkeZondag 13:a3d4b4daf5b4 220 {
MarijkeZondag 13:a3d4b4daf5b4 221 sum = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 222 for(int j = 0; j<=sizeCal-1; j++)
MarijkeZondag 13:a3d4b4daf5b4 223 {
MarijkeZondag 13:a3d4b4daf5b4 224 StoreCal1[j] = emgfilter2;
MarijkeZondag 13:a3d4b4daf5b4 225 sum+=StoreCal2[j];
MarijkeZondag 18:898f54c6aa3d 226 //wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 227 }
MarijkeZondag 13:a3d4b4daf5b4 228 Mean2 = sum/sizeCal;
MarijkeZondag 13:a3d4b4daf5b4 229 Threshold2 = Mean2/2;
MarijkeZondag 13:a3d4b4daf5b4 230 break;
MarijkeZondag 13:a3d4b4daf5b4 231 }
MarijkeZondag 13:a3d4b4daf5b4 232 case 3: //EMG is calibrated, robot can be set to Home position.
MarijkeZondag 13:a3d4b4daf5b4 233 {
MarijkeZondag 13:a3d4b4daf5b4 234 emg_cal = 1;
MarijkeZondag 18:898f54c6aa3d 235 //wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 236 break;
MarijkeZondag 13:a3d4b4daf5b4 237 }
MarijkeZondag 13:a3d4b4daf5b4 238 default: //Ensures nothing happens if x is not 0,1 or 2.
MarijkeZondag 13:a3d4b4daf5b4 239 {
MarijkeZondag 13:a3d4b4daf5b4 240 break;
MarijkeZondag 13:a3d4b4daf5b4 241 }
MarijkeZondag 13:a3d4b4daf5b4 242 }
MarijkeZondag 12:eaed305a76c3 243 }
MarijkeZondag 13:a3d4b4daf5b4 244
MarijkeZondag 8:895d941a5910 245 void encoderA_rise()
MarijkeZondag 8:895d941a5910 246 {
MarijkeZondag 8:895d941a5910 247 if(encoderB==false)
MarijkeZondag 8:895d941a5910 248 {
MarijkeZondag 8:895d941a5910 249 encoder++;
MarijkeZondag 8:895d941a5910 250 }
MarijkeZondag 8:895d941a5910 251 else
MarijkeZondag 8:895d941a5910 252 {
MarijkeZondag 8:895d941a5910 253 encoder--;
MarijkeZondag 8:895d941a5910 254 }
MarijkeZondag 8:895d941a5910 255 }
MarijkeZondag 8:895d941a5910 256
MarijkeZondag 8:895d941a5910 257 void encoderA_fall()
MarijkeZondag 8:895d941a5910 258 {
MarijkeZondag 8:895d941a5910 259 if(encoderB==true)
MarijkeZondag 8:895d941a5910 260 {
MarijkeZondag 8:895d941a5910 261 encoder++;
MarijkeZondag 8:895d941a5910 262 }
MarijkeZondag 8:895d941a5910 263 else
MarijkeZondag 8:895d941a5910 264 {
MarijkeZondag 8:895d941a5910 265 encoder--;
MarijkeZondag 8:895d941a5910 266 }
MarijkeZondag 8:895d941a5910 267 }
MarijkeZondag 8:895d941a5910 268
MarijkeZondag 8:895d941a5910 269 void encoderB_rise()
MarijkeZondag 8:895d941a5910 270 {
MarijkeZondag 8:895d941a5910 271 if(encoderA==true)
MarijkeZondag 8:895d941a5910 272 {
MarijkeZondag 8:895d941a5910 273 encoder++;
MarijkeZondag 8:895d941a5910 274 }
MarijkeZondag 8:895d941a5910 275 else
MarijkeZondag 8:895d941a5910 276 {
MarijkeZondag 8:895d941a5910 277 encoder--;
MarijkeZondag 8:895d941a5910 278 }
MarijkeZondag 8:895d941a5910 279 }
MarijkeZondag 8:895d941a5910 280
MarijkeZondag 8:895d941a5910 281 void encoderB_fall()
MarijkeZondag 8:895d941a5910 282 {
MarijkeZondag 8:895d941a5910 283 if(encoderA==false)
MarijkeZondag 8:895d941a5910 284 {
MarijkeZondag 8:895d941a5910 285 encoder++;
MarijkeZondag 8:895d941a5910 286 }
MarijkeZondag 8:895d941a5910 287 else
MarijkeZondag 8:895d941a5910 288 {
MarijkeZondag 8:895d941a5910 289 encoder--;
MarijkeZondag 8:895d941a5910 290 }
MarijkeZondag 8:895d941a5910 291 }
MarijkeZondag 8:895d941a5910 292
MarijkeZondag 8:895d941a5910 293
MarijkeZondag 9:c722418997b5 294 // Main function start.
MarijkeZondag 8:895d941a5910 295
vsluiter 0:c8f15874531b 296 int main()
MarijkeZondag 3:a3ad58758696 297 {
MarijkeZondag 13:a3d4b4daf5b4 298 //pc.baud(115200);
MarijkeZondag 13:a3d4b4daf5b4 299 //pc.printf("hello\n\r");
MarijkeZondag 19:466ada92bf65 300 sample_timer.attach(&sample, 0.002);
MarijkeZondag 10:39ec51206c8b 301
MarijkeZondag 17:741798018c0d 302 ledr = 0; //Begin led = red, first state of calibration
MarijkeZondag 17:741798018c0d 303 ledb = 1;
MarijkeZondag 17:741798018c0d 304 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 305
MarijkeZondag 13:a3d4b4daf5b4 306 filter_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
MarijkeZondag 13:a3d4b4daf5b4 307 button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
MarijkeZondag 15:be67b090b64a 308 wait(0.2f);
MarijkeZondag 13:a3d4b4daf5b4 309 button2.rise(calibrate); //calibrate threshold for 3 muscles
MarijkeZondag 15:be67b090b64a 310 wait(0.2f);
MarijkeZondag 13:a3d4b4daf5b4 311
MarijkeZondag 13:a3d4b4daf5b4 312 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 6:f4bbb73f3989 313
MarijkeZondag 8:895d941a5910 314 encoderA.rise(&encoderA_rise);
MarijkeZondag 8:895d941a5910 315 encoderA.fall(&encoderA_fall);
MarijkeZondag 8:895d941a5910 316 encoderB.rise(&encoderB_rise);
MarijkeZondag 8:895d941a5910 317 encoderB.fall(&encoderB_fall);
vsluiter 0:c8f15874531b 318
MarijkeZondag 16:5f7196ddc77b 319 if(emg_cal==1)
MarijkeZondag 16:5f7196ddc77b 320 {
MarijkeZondag 16:5f7196ddc77b 321 while (true)
MarijkeZondag 16:5f7196ddc77b 322 {
MarijkeZondag 16:5f7196ddc77b 323 //Motor aansturen en encoder uitlezen
MarijkeZondag 16:5f7196ddc77b 324 //float u1 = potmetervalue1;
MarijkeZondag 16:5f7196ddc77b 325 //float u2 = potmetervalue2;
MarijkeZondag 16:5f7196ddc77b 326
MarijkeZondag 16:5f7196ddc77b 327 //float m1 = ((u1*2.0f)-1.0f);
MarijkeZondag 16:5f7196ddc77b 328 //float m2 = ((u2*2.0f)-1.0f);
MarijkeZondag 16:5f7196ddc77b 329
MarijkeZondag 16:5f7196ddc77b 330 //pwmpin1 = fabs(m1*0.6f)+0.4f; //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling.
MarijkeZondag 16:5f7196ddc77b 331
MarijkeZondag 16:5f7196ddc77b 332 if(emgfilter0>Threshold0)
MarijkeZondag 16:5f7196ddc77b 333 {
MarijkeZondag 16:5f7196ddc77b 334 pwmpin1 = 1;
MarijkeZondag 16:5f7196ddc77b 335 directionpin1.write(1);
MarijkeZondag 16:5f7196ddc77b 336 }
MarijkeZondag 16:5f7196ddc77b 337 else
MarijkeZondag 16:5f7196ddc77b 338 {
MarijkeZondag 16:5f7196ddc77b 339 pwmpin1 = 0;
MarijkeZondag 16:5f7196ddc77b 340 }
MarijkeZondag 9:c722418997b5 341
MarijkeZondag 16:5f7196ddc77b 342 if(emgfilter1>Threshold1)
MarijkeZondag 16:5f7196ddc77b 343 {
MarijkeZondag 16:5f7196ddc77b 344 pwmpin2 = 1;
MarijkeZondag 16:5f7196ddc77b 345 directionpin2.write(1);
MarijkeZondag 16:5f7196ddc77b 346 }
MarijkeZondag 16:5f7196ddc77b 347 else
MarijkeZondag 16:5f7196ddc77b 348 {
MarijkeZondag 16:5f7196ddc77b 349 pwmpin2 = 0;
MarijkeZondag 16:5f7196ddc77b 350 }
MarijkeZondag 16:5f7196ddc77b 351 if(emgfilter2>Threshold2)
MarijkeZondag 16:5f7196ddc77b 352 {
MarijkeZondag 16:5f7196ddc77b 353 pwmpin1 = 1;
MarijkeZondag 16:5f7196ddc77b 354 pwmpin2 = 2;
MarijkeZondag 16:5f7196ddc77b 355 directionpin1.write(1);
MarijkeZondag 16:5f7196ddc77b 356 directionpin2.write(1);
MarijkeZondag 16:5f7196ddc77b 357 }
MarijkeZondag 16:5f7196ddc77b 358 else
MarijkeZondag 16:5f7196ddc77b 359 {
MarijkeZondag 16:5f7196ddc77b 360 pwmpin1 = 0;
MarijkeZondag 16:5f7196ddc77b 361 pwmpin2 = 0;
MarijkeZondag 16:5f7196ddc77b 362 }
MarijkeZondag 16:5f7196ddc77b 363
MarijkeZondag 16:5f7196ddc77b 364 //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom.
MarijkeZondag 16:5f7196ddc77b 365 //wait(0.01f); //zodat de code niet oneindig doorgaat.
MarijkeZondag 16:5f7196ddc77b 366 //pwmpin2 = fabs(m2*0.6f)+0.4f;
MarijkeZondag 16:5f7196ddc77b 367 //directionpin2.write(m2>0);
MarijkeZondag 16:5f7196ddc77b 368
MarijkeZondag 16:5f7196ddc77b 369 //float encoderDegrees = float(encoder)*(360.0/8400.0);
MarijkeZondag 16:5f7196ddc77b 370
MarijkeZondag 16:5f7196ddc77b 371 //pc.printf("Encoder count: %f \n\r",encoderDegrees);
MarijkeZondag 10:39ec51206c8b 372
vsluiter 0:c8f15874531b 373 }
vsluiter 0:c8f15874531b 374 }
MarijkeZondag 19:466ada92bf65 375 while(1) {}
MarijkeZondag 16:5f7196ddc77b 376 }