Remco Dasselaar
/
TotalControlEmg2
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.
Dependencies: HIDScope MODSERIAL QEI TextLCD mbed
Fork of
TotalControlEmg2
by 
Remco Dasselaar
Revision 30:8ae855348d22, committed 2015-10-19
- Comitter:
- RemcoDas
- Date:
- Mon Oct 19 10:09:42 2015 +0000
- Parent:
- 29:9610df479f89
- Child:
- 31:074b9d03d816
- Commit message:
- Final werkend met EMG
Changed in this revision
--- a/Filterdesigns.cpp Sat Oct 17 14:13:57 2015 +0000
+++ b/Filterdesigns.cpp Mon Oct 19 10:09:42 2015 +0000
@@ -5,13 +5,15 @@
//Notch 50Hz
// Filter1a: 50Hz Notch
-double v1_50a = 0, v2_50a = 0;
+double v1_50a_L = 0, v2_50a_L = 0;
+double v1_50a_R = 0, v2_50a_R = 0;
const double a1_50a = -1.52337295428, a2_50a = 0.93195385841;
const double b0_50a = 1.00000000000, b1_50a = -1.61854514929, b2_50a = 1.0;
const double gain_50a = 1.000000;
// Filter1b: 50Hz Notch
-double v1_50b = 0, v2_50b = 0;
+double v1_50b_L = 0, v2_50b_L = 0;
+double v1_50b_R = 0, v2_50b_R = 0;
const double a1_50b = -1.60486796113, a2_50b = 0.93780356783;
const double b0_50b = 1.00000000000, b1_50b = -1.61854514929, b2_50b = 1.0;
const double gain_50b = 0.934872;
@@ -19,7 +21,8 @@
//HighPass 20Hz
// Filter2a: 20Hz HighPass
-double v1_HP = 0, v2_HP = 0;
+double v1_HP_L = 0, v2_HP_L = 0;
+double v1_HP_R = 0, v2_HP_R = 0;
const double a1_HP = -0.76475499450, a2_HP = 0.27692273367;
const double b0_HP = 1.00000000000, b1_HP = -2.0, b2_HP = 1.00000000000;
const double gain_HP = 0.510419;
@@ -27,16 +30,16 @@
//LowPass 3Hz
// Filter3a: 3Hz LowPass
-double v1_LP = 0, v2_LP = 0;
+double v1_LP_L = 0, v2_LP_L = 0;
+double v1_LP_R = 0, v2_LP_R = 0;
const double a1_LP = -1.93503824887, a2_LP = 0.93708289663;
const double b0_LP = 1.00000000000, b1_LP = 2.0, b2_LP = 1.00000000000;
const double gain_LP = 0.000511;
-// constante variabelen:
+
-double y=0;
-
-double Filterdesigns(double u){
+double FilterdesignsLeft(double u){ // input u, oude y waarde
+ double y = 0;
//u = input waarde
//y = output waarde
@@ -44,19 +47,43 @@
// Filter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2, const double gain)
// 50Hz Notch filter
- double y50a = Filter(u, v1_50a, v2_50a, a1_50a, a2_50a, b0_50a, b1_50a, b2_50a, gain_50a);
- double y50b = Filter(y50a, v1_50b, v2_50b, a1_50b, a2_50b, b0_50b, b1_50b, b2_50b, gain_50b);
+ double y50a = Filter(u, v1_50a_L, v2_50a_L, a1_50a, a2_50a, b0_50a, b1_50a, b2_50a, gain_50a);
+ double y50b = Filter(y50a, v1_50b_L, v2_50b_L, a1_50b, a2_50b, b0_50b, b1_50b, b2_50b, gain_50b);
+
+ // High Pass filter. Tot 20Hz wordt weggefliterd
+ double yHP = Filter(y50b, v1_HP_L, v2_HP_L, a1_HP, a2_HP, b0_HP, b1_HP, b2_HP, gain_HP);
+
+ // Absolute waarde wordt genomen
+ double y1 = fabs(yHP);
+
+ // Low Pass filter. Alles vanaf 5Hz wordt weggefilterd
+ double yLP = Filter(y1, v1_LP_L, v2_LP_L, a1_LP, a2_LP, b0_LP, b1_LP, b2_LP, gain_LP);
- // High Pass filter. Tot 20Hz wordt weggefliterd
- double yHP = Filter(u, v1_HP, v2_HP, a1_HP, a2_HP, b0_HP, b1_HP, b2_HP, gain_HP);
+ y = 10 * yLP;
+ return y;
+ }
+
+double FilterdesignsRight(double u){ // input u, oude y waarde
+ //u = input waarde
+ //y = output waarde
+
+ // Op deze manier worden de waardes ingelezen in Filter. Zorg dus voor dezelfde volgorde, zodat de waardes goed uitgelezen worden!:
+ // Filter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2, const double gain)
+
+ // 50Hz Notch filter
+ double y50a = Filter(u, v1_50a_R, v2_50a_R, a1_50a, a2_50a, b0_50a, b1_50a, b2_50a, gain_50a);
+ double y50b = Filter(y50a, v1_50b_R, v2_50b_R, a1_50b, a2_50b, b0_50b, b1_50b, b2_50b, gain_50b);
+
+ // High Pass filter. Tot 20Hz wordt weggefliterd !!! WAT IS HIER INPUT U? !!!
+ double yHP = Filter(u, v1_HP_R, v2_HP_R, a1_HP, a2_HP, b0_HP, b1_HP, b2_HP, gain_HP);
// Absolute waarde wordt genomen.
double y1 = fabs(yHP);
// Low Pass filter. Alles vanaf 5Hz wordt weggefilterd
- double yLP = Filter(y1, v1_LP, v2_LP, a1_LP, a2_LP, b0_LP, b1_LP, b2_LP, gain_LP);
+ double yLP = Filter(y1, v1_LP_R, v2_LP_R, a1_LP, a2_LP, b0_LP, b1_LP, b2_LP, gain_LP);
- y = 10 * yLP;
-
+ double y = 10 * yLP;
return y;
-}
\ No newline at end of file
+}
+
--- a/Filterdesigns.h Sat Oct 17 14:13:57 2015 +0000 +++ b/Filterdesigns.h Mon Oct 19 10:09:42 2015 +0000 @@ -1,3 +1,5 @@ #include "mbed.h" -double Filterdesigns(double u); \ No newline at end of file +double FilterdesignsLeft(double u); + +double FilterdesignsRight(double u); \ No newline at end of file
--- a/Kalibratie.cpp Sat Oct 17 14:13:57 2015 +0000
+++ b/Kalibratie.cpp Mon Oct 19 10:09:42 2015 +0000
@@ -14,14 +14,19 @@
LedRed = 1;
}
-double KalibratieMax(AnalogIn& emg){ //Kalibratie van de maximum waarde
+double KalibratieMax(AnalogIn& emg, bool side){ //Kalibratie van de maximum waarde
LedGreen.write(0); //Led aan
double ymax = 0;
for(int i = 1; i <= samples; i++) { //Als timer onder de 5 seconden is dit uitvoeren
double u = emg.read();
- double y = Filterdesigns(u);
-
+ double y = 0;
+ if(side){ // links
+ y = FilterdesignsLeft(u);
+ }
+ else {
+ y = FilterdesignsRight(u); // rechts
+ }
if (y > ymax && i >= samples / 10) { //Als de gemeten waarde groter is dan de opgeslagen waarde wordt dit de nieuwe opgeslagen waarde
ymax = y;
}
@@ -31,13 +36,19 @@
return ymax;
}
-double KalibratieMin(AnalogIn& emg){ //Kalibratie van de minimum waarde
+double KalibratieMin(AnalogIn& emg, bool side){ //Kalibratie van de minimum waarde
LedRed.write(0);
double ymin = 10;
for(int i = 1; i <= samples; i++) {
double u = emg.read();
- double y = Filterdesigns(u);
+ double y = 0;
+ if(side){ // links
+ y = FilterdesignsLeft(u);
+ }
+ else {
+ y = FilterdesignsRight(u); // rechts
+ }
if (y < ymin && i >= samples / 10) {
ymin = y;
--- a/Kalibratie.h Sat Oct 17 14:13:57 2015 +0000 +++ b/Kalibratie.h Mon Oct 19 10:09:42 2015 +0000 @@ -1,7 +1,7 @@ #include "mbed.h" -double KalibratieMax(AnalogIn& emg); +double KalibratieMax(AnalogIn& emg, bool side); -double KalibratieMin(AnalogIn& emg); +double KalibratieMin(AnalogIn& emg, bool side); void Init(); \ No newline at end of file
--- a/main.cpp Sat Oct 17 14:13:57 2015 +0000
+++ b/main.cpp Mon Oct 19 10:09:42 2015 +0000
@@ -15,7 +15,7 @@
AnalogIn emgL(A0), emgR(A1); // Analog input van emg kabels links en rechts
AnalogIn potL(A2), potR(A3); // Intensiteit 'EMG' signaal door potmeter
AnalogIn KS(A4); // Killswitch
-HIDScope scope(6); // 3 scopes
+HIDScope scope(6);
Ticker EMGticker, tickerRegelaar, tickerRem; // regelaar button en rem ticker
// QEI Encoder(poort 1, poort 2, ,counts/rev
@@ -41,6 +41,7 @@
double t = 1/ Fs; // tijd EMGticker
double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0;
double thresholdlowR= 0, thresholdmidR = 0, thresholdhighR= 0;
+double yL = 0, yR = 0; // y waarden EMG links en rechts
volatile bool L = false, R = false, RH = false; // Booleans voor knop link en knop rechts
volatile bool btn = false; // boolean om programma te runnen, drukknop ingedrukt
@@ -60,15 +61,15 @@
void EMGkalibratieL(){ // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
LedB = 1;
Init();
- double ymin = KalibratieMin(emgL);
+ double ymin = KalibratieMin(emgL, true);
wait(1);
- double ymax = KalibratieMax(emgL);
+ double ymax = KalibratieMax(emgL, true);
if((ymax-ymin) < 0.05){ // voor als er geen kabels in de EMG zitten
ymin = 10;
ymax = 10;
}
- thresholdlowL = 8 * ymin; // standaardwaarde
+ thresholdlowL = 4 * ymin; // standaardwaarde
thresholdmidL = 0.5 * ymax; // afhankelijk van max output gebruiker
thresholdhighL = 0.8 * ymax;
@@ -77,28 +78,29 @@
void EMGkalibratieR(){ // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
LedB = 1;
Init();
- double ymin = KalibratieMin(emgR);
+ double ymin = KalibratieMin(emgR, false);
wait(1);
- double ymax = KalibratieMax(emgR);
+ double ymax = KalibratieMax(emgR, false);
if((ymax-ymin) < 0.05){ // voor als er geen kabels in de EMG zitten
ymin = 10;
ymax = 10;
}
- thresholdlowR = 8 * ymin; // standaardwaarde
+ thresholdlowR = 4 * ymin; // standaardwaarde
thresholdmidR = 0.5 * ymax; // afhankelijk van max output gebruiker
thresholdhighR = 0.8 * ymax;
pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); //bugfix
}
int EMGfilter(AnalogIn& emg, bool side){
- double u = emg.read(); // uitlezen emg signaal
- double y = Filterdesigns(u); // signaal filteren
+ double u = emg.read(); // uitlezen emg signaal (linker of rechter EMG)
int mode = 1;
- if(side){ // linker EMG
+ if(side){
+ double y = FilterdesignsLeft(u); // signaal filteren // linker EMG
mode = Mode(y, thresholdlowL, thresholdmidL, thresholdhighL); //bepaald welk signaal de motor krijgt (1, 2 ,3 )
}
- else { // rechter EMG
+ else {
+ double y = FilterdesignsRight(u); // signaal filteren // rechter EMG
mode = Mode(y, thresholdlowR, thresholdmidR, thresholdhighR); //bepaald welk signaal de motor krijgt (1, 2 ,3 )
}
return mode;
@@ -149,10 +151,11 @@
}
void motorAim(int dir){ // Aim motor laten draaien met gegeven direction
dirM2.write(dir); // richting
- pwmM2.write(0.5); // width aanpassen
+ pwmM2.write(0.25); // width aanpassen
}
bool controlAim(){ // Function om aim motor aan te sturen
bool both = false; // boolean of beide knoppen ingedrukt zijn
+
int modeL = defMode(emgL, potL, true);
int modeR = defMode(emgR, potR, false);
@@ -167,7 +170,7 @@
L = true;
}
- if((modeL>2) && (modeR >2)) { // mode L en mode R beide > 2
+ if((modeL>2) && (modeR >2 && R && L)) { // mode L en mode R beide > 2
both = true; // Return true
pwmM2.write(0); // Aim motor stilzetten
aimState = OFF;
@@ -245,12 +248,14 @@
pc.printf("Kalibrate pendulum motor with setknop\r\n");
pwmM1.period(1/100000); // aanstuurperiode motor 1
servo.period(0.02); // 20ms period aanstuurperiode
- pwmM1.write(0.5); // Motor aanzetten, laag vermogen
+ pwmM1.write(0.25); // Motor aanzetten, laag vermogen
btn = false;
while(state==KALIBRATE_PEND){
if(regelaarFlag){
pc.printf(""); // lege regel printen, anders doet setknop het niet
- regelaarFlag = false;
+ regelaarFlag = false;
+
+
if (btn){ // Als setknop ingedrukt is reset
pwmM1.write(0); // Motor 1 stilzetten
enc1.reset(); // encoder 1 resetten
@@ -298,9 +303,13 @@
if(L && R){
if((modeL > 1) && modeR > 1) { // beide aangespannenR = false;
state = FIRE;
+ R = false;
+ L = false;
}
else if(modeL > 2 || modeR > 2) {// links of rechts vol ingedrukt = schieten
state = FIRE;
+ R = false;
+ L = false;
}
else if((modeL == 2) && L) { // links ingedrukt = rem verlagen
if(remPerc>1){
@@ -327,10 +336,10 @@
case FIRE: {
pc.printf("Shooting\r\n");
servo.pulsewidth(servoL); // schrijfrem release
- pwmM1.write(0.5); // motor aanzetten
+ pwmM1.write(0.25); // motor aanzetten
bool servoPos = true;
while(state == FIRE){ // Zolang in FIRE state
- if(remFlag && (abs(enc1.getPulses())-100)%4200 < (2100/**abs(remPerc)/10*/)){ // Rem goFlag en half rondje = remmen
+ if(remFlag && (abs(enc1.getPulses())-100)%4200 < (2100*abs(remPerc)/10)){ // Rem goFlag en half rondje = remmen
remFlag = false;
if(servoPos){ //
servoPos = false;