Janine Klein Rot
/
PIPO
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" //Mbed bibliotheek inladen, standaard functies 00002 #include "MODSERIAL.h" //MODSERIAL bibliotheek inladen, communicatie met PC 00003 #include "encoder.h" //Encoder bibliotheek inladen, communicatie met encoder 00004 00005 #define SAMPLETIME_EMG 0.005 //Constanten definiëren en een waarde toekennen 00006 #define A1 2 00007 #define A2 -157.9 00008 #define B0 1 00009 #define B1 -1.964 00010 #define B2 0.9651 00011 #define C1 1.158 00012 #define C2 -0.4112 00013 #define D0 1 00014 #define D1 -1.709 00015 #define D2 0.9625 00016 #define E1 1.823 00017 #define E2 -0.8372 00018 #define F1 0.007438 00019 #define F2 0.00701 00020 00021 //Pinverdeling en naamgeving variabelen 00022 Encoder motor_arm1( //Encoder motor arm 1 00023 Encoder motor_arm2( //Encoder motor arm 2 00024 00025 PwmOut pwm_motor_arm1( //PWM naar motor arm 1 00026 PwmOut pwm_motor_arm2( //PWM naar motor arm 2 00027 00028 DigitalOut dir_motor_arm1( //Richting motor arm 1 00029 DigitalOut dir_motor_arm2( //Richting motor arm 2 00030 00031 AnalogIn emg_bi( //EMG signalen biceps 00032 AnalogIn emg_tri( //EMG signalen triceps 00033 00034 Ticker ticker_sample_EMG( //Uitlezen EMG iedere zoveel seconden 00035 00036 00037 //Gedefinieerde datatypes en variabelen 00038 bool rust = false 00039 bool kalibratie_positie = false 00040 bool kalibratie_EMG = false 00041 char lcd_r1 = new char[16]; //LCD regel 1, 16 karakters 00042 char lcd_r2 = new char[16]; //LCD regel 2, 16 karakters 00043 void sample_bi_EMG //Acties voor inladen EMG 00044 void sample_tri_EMG //Acties voor inladen EMG 00045 float xbk //x(n) biceps 00046 float xbk1 //x(n-1) biceps 00047 float xbk2 //x(n-2) biceps 00048 float ybk //y(n) biceps 00049 float ybk1 //y(n-1) biceps 00050 float ybk2 //y(n-2) biceps 00051 00052 float xtk //x(n) triceps 00053 float xtk1 //x(n-1) triceps 00054 float xtk2 //x(n-2) triceps 00055 float ytk //y(n) triceps 00056 float ytk1 //y(n-1) triceps 00057 float ytk2 //y(n-2) triceps 00058 00059 00060 //Beginwaarden voor variabelen 00061 00062 xbk1 = 0 //xbk(n-1) = 0 00063 xbk2 = 0 //xbk(n-2) = 0 00064 ybk1 = 0 //ybk(n-1) = 0 00065 ybk2 = 0 //ybk(n-2) = 0 00066 00067 xtk1 = 0 //xtk(n-1) = 0 00068 xtk2 = 0 //xtk(n-2) = 0 00069 ytk1 = 0 //ytk(n-1) = 0 00070 ytk2 = 0 //ytk(n-2) = 0 00071 00072 00073 //PC baud rate 00074 pc.baud(38400) 00075 00076 int main{ 00077 00078 if (rust = false && kalibratie_positie = false && kalibratie_EMG = false){ 00079 lcd_r1 = " BMT M9 GR. 4 "; //Tekst in LCD scherm 00080 lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst in LCD scherm 00081 wait(2); //2 seconden wachten 00082 rust = true 00083 } 00084 00085 if (rust = true && kalibratie_positie = false && kalibratie_EMG = false){ 00086 wait(1); //1 seconde wachten 00087 00088 pwm_motor_arm1 = 0.02036 //Dutycycle motor arm 1/100 00089 dir_motor_arm1 = // 1 of 0 //Draairichting motor arm 1 00090 00091 while(motor_arm1 < 363){ //Zolang aantal encoderpulsen < 363 --> PWM motor arm 1 blijft gelijk 00092 pwm_motor_arm1 = 0.02036 00093 } 00094 00095 pwm_motor_arm1 = 0 //Na while loop PWM motor arm 1 op nul 00096 motor_arm1 = 0 //Encoderpulsen op nul zetten 00097 00098 pwm_motor_arm2 = 0.02398 //Dutycycle motor arm 2/100 00099 dir_motor_arm2 = // 1 of 0 //Draairichting motor arm 2 00100 00101 while(motor_arm_2 < 787){ //Zolang aantal encoderpulsen < 787 --> PWM motor arm 3 blijft gelijk 00102 pwm_motor_arm2 = 0.02398 00103 } 00104 00105 pwm_motor_arm2 = 0 //Na while loop PWM motor arm 2 op nul 00106 motor_arm2 = 0 //Encoderpulsen op nul zetten 00107 00108 lcd_r1 = " Beginpositie " //Tekst in LCD scherm 00109 lcd_r2 = " ingesteld " //Tekst in LCD scherm 00110 wait(2); //2 seconden wachten 00111 kalibratie_positie = true 00112 } 00113 00114 if (rust = true && kalibratie_positie = true && kalibratie_EMG = false){ 00115 wait(1); //1 seconde wachten 00116 00117 //EMG inladen 00118 void sample_EMG{ 00119 xbk = emg_bi.read(); //Uitlezen EMG signaal biceps 00120 xtk = emg_tri.read(); //Uitlezen EMG signaal triceps 00121 filter(); 00122 } 00123 00124 00125 void filter() 00126 { 00127 /* do filtering */ 00128 00129 } 00130 ticker_sample_EMG.attach(sample_EMG,SAMPLETIME_EMG) //Ticker gaat de void sample_EMG uitvoeren en dus de EMG uitlezen iedere 0.005 seconden 00131 00132 while t<600{ 00133 xbkt(t)=xbk 00134 ybkt(t) = ybk 00135 xtkt(t) = xtk 00136 ytkt(t) = ytk 00137 t = t+1 00138 } 00139 00140 00141 //Detrend 00142 n = 600; 00143 00144 //hier wordt de som van de x en y waardes van het gemeten signaal berekend 00145 00146 int i, n //n = het aantaal samples dat we hebben, is afhankelijk van het tijdinterval dat we kiezen 00147 for(i=0; i<n; i++) { 00148 sum_bk = sum_bk + ybk(i) //functie met line 88 voor een soomatie van alle waardes 00149 //sum = sum_bk 00150 sum_tk += ytk(i) //betekenis van de += dat de waardes altijd bij elkaar op worden geteld 00151 00152 // ook mean van de x en dan met de juiste/betere manier 00153 } 00154 00155 //hier wordt de mean van de x en y van het gemeten signaal gemeten 00156 00157 mean_ybk = sum_bk./n 00158 mean_xbk = sum_xbk./n 00159 00160 00161 //Hoogdoorlaatfilter 2Hz 00162 while(true){ 00163 ybk = A1*ybk1+A2*ybk2+B0*xbk+B1*xbk1+B2*xbk2 //Overdracht 2e orde hoogdoorlaatfilter 00164 00165 xbk2 = xbk1 //xbk(n-2) wordt xbk(n-1) in de volgende loop 00166 xbk1 = xbk //xbk(n-1) wordt xbk(n) in de volgende loop 00167 ybk2 = ybk1 //ybk(n-2) wordt ybk(n-1) in de volgende loop 00168 ybk1 = ybk //ybk(n-1) wordt ybk(n) in de volgende loop 00169 00170 ytk = A1*ytk1+A2*ytk2+B0*xtk+B1*xtk1+B2*xtk2 //Overdracht 2e orde hoogdoorlaatfilter 00171 00172 xtk2 = xtk1 //xtk(n-2) wordt xtk(n-1) in de volgende loop 00173 xtk1 = xtk //xtk(n-1) wordt xtk(n) in de volgende loop 00174 ytk2 = ytk1 //ytk(n-2) wordt ytk(n-1) in de volgende loop 00175 ytk1 = ytk //ytk(n-1) wordt ytk(n) in de volgende loop 00176 00177 wait(SAMPLETIME_EMG); //0.005 seconden wachten 00178 } 00179 00180 //Bandstop 40 - 60Hz 00181 while(true){ 00182 blabla = 00183 00184 //Rectifier 00185 00186 00187 //Laagdoorlaatfilter 10Hz 00188 00189 00190 //Moving average 4e orde 00191 00192 00193 00194 00195
Generated on Wed Jul 20 2022 02:11:11 by
1.7.2