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 27:f62e450bb411, committed 2015-10-16
- Comitter:
- RemcoDas
- Date:
- Fri Oct 16 12:37:26 2015 +0000
- Parent:
- 26:d9855716ced7
- Child:
- 28:e6d2fe0e593e
- Commit message:
- Dubbele kalibratie toegevoegd
Changed in this revision
--- a/Kalibratie.cpp Fri Oct 16 09:12:51 2015 +0000
+++ b/Kalibratie.cpp Fri Oct 16 12:37:26 2015 +0000
@@ -26,10 +26,10 @@
ymax = y;
}
wait(0.05);
- }
+ }
LedGreen.write(1); //Led aan
return ymax;
-}
+ }
double KalibratieMin(AnalogIn& emg){ //Kalibratie van de minimum waarde
LedRed.write(0);
--- a/Mode.cpp Fri Oct 16 09:12:51 2015 +0000
+++ b/Mode.cpp Fri Oct 16 12:37:26 2015 +0000
@@ -2,9 +2,7 @@
double mode;
-int Mode(double y, double ymax, double thresholdlow, double thresholdmid, double thresholdhigh)
-{
-
+int Mode(double y, double thresholdlow, double thresholdmid, double thresholdhigh){
if ( y <= thresholdlow){
mode = 1;
}
@@ -14,6 +12,5 @@
if (y > thresholdmid && mode == 1){
mode = 2;
}
-
return mode;
}
--- a/Mode.h Fri Oct 16 09:12:51 2015 +0000 +++ b/Mode.h Fri Oct 16 12:37:26 2015 +0000 @@ -1,3 +1,3 @@ #include "mbed.h" -int Mode(double y, double ymax, double thresholdlow, double thresholdmid, double thresholdhigh); \ No newline at end of file +int Mode(double y, double thresholdlow, double thresholdmid, double thresholdhigh); \ No newline at end of file
--- a/main.cpp Fri Oct 16 09:12:51 2015 +0000
+++ b/main.cpp Fri Oct 16 12:37:26 2015 +0000
@@ -13,8 +13,8 @@
MODSERIAL pc(USBTX, USBRX); // Modserial voor Putty
TextLCD lcd(PTC5, PTC7, PTC0, PTC9, PTC8, PTC1); // LCD scherm op binnenste rij van K64F, rs, e, d4-d7
PwmOut servo(D3); // PwmOut servo
-AnalogIn emgL(A0), emgR(A1); // Analog input van emg kabels links en rechts
-AnalogIn potL(A2), potR(A3); // Intensiteit 'EMG' signaal door potmeter
+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
Ticker EMGticker, tickerRegelaar, tickerRem; // regelaar button en rem ticker
@@ -27,8 +27,8 @@
// Motor2 met PWM aansturen en richting aangeven
PwmOut pwmM2(D5);
DigitalOut dirM2(D4);
-enum spelfase {KALIBRATE_EMG1, KALIBRATE_AIM, KALIBRATE_PEND, AIM, REM, FIRE}; // Spelfases, ACKNOWLEDGEMENT: BMT groep 4 2014
-uint8_t state = KALIBRATE_EMG1; // eerste spelfase
+enum spelfase {KALIBRATE_EMG, KALIBRATE_AIM, KALIBRATE_PEND, AIM, REM, FIRE}; // Spelfases, ACKNOWLEDGEMENT: BMT groep 4 2014
+uint8_t state = KALIBRATE_EMG; // eerste spelfase
enum aimFase {OFF, CW, CCW}; // Aim motor mogelijkheden
uint8_t aimState = OFF; // mogelijkheid begin
//-------------------------------Variables---------------------------------------------------------------------
@@ -38,12 +38,12 @@
const double servoL = 0.001, servoR = 0.0011; // uitwijking servo, bereik tussen L en R (= pulsewidth pwm servo)
const double tRegelaar = 0.005, tRem = 0.1; // tijd ticker voor regelaar en knoppen/EMG
-const double Fs = 200; //Sample frequentie Hz
+const double Fs = 50; //Sample frequentie Hz
double t = 1/ Fs; // tijd EMGticker
-double ymin = 0, ymax = 0;
-double thresholdlow = 0, thresholdmid = 0, thresholdhigh= 0;
+double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0;
+double thresholdlowR= 0, thresholdmidR = 0, thresholdhighR= 0;
-volatile bool L = false, R = false; // Booleans voor knop link en knop 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
volatile bool regelaarFlag = false, btnFlag = false, remFlag = false; // Go flags
volatile bool emgFlag = false;
@@ -58,29 +58,51 @@
lcd.printf(text); // print text op lcd
pc.printf(text); // print text op terminal
}
-void EMGkalibratie(){ // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
+void EMGkalibratieL(){ // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
+ LedB = 1;
+ Init();
+ double ymin = KalibratieMin(emgL);
+ wait(1);
+ double ymax = KalibratieMax(emgL);
+
+ if((ymax-ymin) < 0.05){ // voor als er geen kabels in de EMG zitten
+ ymin = 10;
+ ymax = 10;
+ }
+ thresholdlowL = 8 * ymin; // standaardwaarde
+ thresholdmidL = 0.5 * ymax; // afhankelijk van max output gebruiker
+ thresholdhighL = 0.8 * ymax;
+
+ pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin); //bugfix
+ }
+void EMGkalibratieR(){ // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
LedB = 1;
Init();
- ymin = KalibratieMin(emgL);
+ double ymin = KalibratieMin(emgR);
wait(1);
- ymax = KalibratieMax(emgL);
-
+ double ymax = KalibratieMax(emgR);
+
+ if((ymax-ymin) < 0.05){ // voor als er geen kabels in de EMG zitten
+ ymin = 10;
+ ymax = 10;
+ }
// bepalen van thresholds voor aan/uit
- thresholdlow = 8 * ymin; // standaardwaarde
- thresholdmid = 0.5 * ymax; // afhankelijk van max output gebruiker
- thresholdhigh = 0.8 * ymax;
+ thresholdlowR = 8 * ymin; // standaardwaarde
+ thresholdmidR = 0.5 * ymax; // afhankelijk van max output gebruiker
+ thresholdhighR = 0.8 * ymax;
- pc.printf("ymax = %f en ymin = %f \n",ymax, ymin); //bugfix
+ pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); //bugfix
}
-int EMGfilter(AnalogIn& emg, int w){
+int EMGfilter(AnalogIn& emg, bool side){
double u = emg.read(); // uitlezen emg signaal
- double y = Filterdesigns(u); // signaal filteren
- int mode = Mode(y, ymax, thresholdlow, thresholdmid, thresholdhigh); //bepaald welk signaal de motor krijgt (1, 2 ,3 )
-
- scope.set(w,u); //ongefilterde waarde naar scope 1
- scope.set(w+1,y); //gefilterde waarde naar scope 2
- scope.set(w+2,mode);
- scope.send(); //stuur de waardes naar HIDscope
+ double y = Filterdesigns(u); // signaal filteren
+ int mode = 1;
+ if(side){ // linker EMG
+ mode = Mode(y, thresholdlowL, thresholdmidL, thresholdhighL); //bepaald welk signaal de motor krijgt (1, 2 ,3 )
+ }
+ else { // rechter EMG
+ mode = Mode(y, thresholdlowR, thresholdmidR, thresholdhighR); //bepaald welk signaal de motor krijgt (1, 2 ,3 )
+ }
return mode;
}
int PotReader(AnalogIn& pot){ // potmeter uitlezen en mode bepalen (thresholds)
@@ -94,8 +116,8 @@
}
return mode;
}
-int defMode(AnalogIn& emg, int w, AnalogIn& pot){ // bepaal mode uit emg en pot
- int emgMode = EMGfilter(emg, w);
+int defMode(AnalogIn& emg, AnalogIn& pot, bool side){ // bepaal mode uit emg en pot
+ int emgMode = EMGfilter(emg, side);
int potMode = PotReader(pot);
int mode = 1;
if(!(emgMode==1) != !(potMode==1)){ // emgMode = 1 XOR potMode = 1
@@ -105,7 +127,7 @@
else{
mode = potMode;
}
- }
+ }
return mode;
}
void setEmgFlag(){
@@ -121,11 +143,11 @@
remFlag = true;
}
void checkAim(){ // controleer of killer switch geraakt is en of max aantal counts niet bereikt is
- double volt = KS.read();
- if(volt< 0.5 /*|| abs(enc2.getPulses()) > maxCounts*/){
- pwmM2.write(0); // Aim motor stilzetten
+ /*double volt = KS.read();
+ if(volt> 0.5 /*|| abs(enc2.getPulses()) > maxCounts*///){
+ /*pwmM2.write(0); // Aim motor stilzetten
pc.printf("BOEM! CRASH! KASTUK! \r\n");
- }
+ }*/
}
void motorAim(int dir){ // Aim motor laten draaien met gegeven direction
dirM2.write(dir); // richting
@@ -133,43 +155,48 @@
}
bool controlAim(){ // Function om aim motor aan te sturen
bool both = false; // boolean of beide knoppen ingedrukt zijn
- int modeL = defMode(emgL, 0, potL);
- int modeR = defMode(emgR, 3, potR);
-
+ int modeL = defMode(emgL, potL, true);
+ int modeR = defMode(emgR, potR, false);
+
+ scope.set(0, modeL);
+ scope.set(1, modeR);
+ scope.send(); //stuur de waardes naar HIDscope
+
+ if(modeR == 1 && !R){ // R is niet aan
+ R = true;
+ }
+ if(modeL == 1 && !L){ // L is niet aan
+ L = true;
+ }
+
if(modeL>1 && modeR >1) { // mode L en mode R beide > 1
R = false;
L = false;
pwmM2.write(0); // Aim motor stilzetten
aimState = OFF;
}
- else if(modeL == 3) {
+ else if((modeL == 3)) {
both = true; // Return true
pwmM2.write(0); // Aim motor stilzetten
aimState = OFF;
L = false;
}
else if(modeR == 3 && aimState!=OFF) {
- R = false;
+ R = false;
pwmM2.write(0); // Aim motor stilzetten
aimState = OFF;
PRINT("Motor freeze\r\n");
}
- else if(modeL == 2 && aimState != CCW && L) { // modeL ==2 AND richting is niet CCW
+ else if((modeL == 2) && (aimState != CCW) && L) { // modeL ==2 AND richting is niet CCW
aimState = CCW; // draai CCW
pc.printf("Turn negative (CCW)\r\n");
motorAim(0);
}
- else if((modeR == 2) && aimState != CW && R) { // modeR == 2 AND richting is niet CW
+ else if((modeR == 2) && (aimState != CW) && R) { // modeR == 2 AND richting is niet CW
aimState = CW; // draai CW
PRINT("Turn positive (CW)\r\n");
motorAim(1);
- }
- if(modeR == 1 && !R){ // R is niet aan
- R = true;
- }
- if(modeL == 1 && !L){ // L is niet aan
- L = true;
- }
+ }
return both;
}
int main(){
@@ -184,12 +211,13 @@
PRINT("--- NEW GAME ---\r\n");
while(1){ // Programma door laten lopen
switch(state){
- case KALIBRATE_EMG1: {
- pc.printf("Kalibrate EMG signal in 5 seconds\r\n");
- lcd.cls();
- lcd.printf("Kalibrate EMG\n in 5 sec.");
+ case KALIBRATE_EMG: {
+ PRINT("Kalibrate EMG left in 5 seconds\r\n");
+
+ EMGkalibratieL();
- EMGkalibratie();
+ PRINT("Kalibrate EMG right in 5 seconds\r\n");
+ EMGkalibratieR();
state = KALIBRATE_AIM;
break;
@@ -261,9 +289,9 @@
while(state == REM){
if(emgFlag){ // Go flag EMG sampelen
emgFlag = false;
- int modeL = defMode(emgL, 0, potL);
- int modeR = defMode(emgR, 3, potR);
-
+ int modeL = defMode(emgL, potL, true);
+ int modeR = defMode(emgR, potR, false);
+
if(modeR < 2) { // R is niet aan
R = true;
}
@@ -271,20 +299,20 @@
L = true;
}
if(L && R){
- if(modeL > 1 && modeR > 1) { // beide aangespannenR = false;
+ if((modeL > 1) && modeR > 1) { // beide aangespannenR = false;
state = FIRE;
}
else if(modeL > 2 || modeR > 2) {// links of rechts vol ingedrukt = schieten
state = FIRE;
}
- else if(modeL == 2 && L) { // links ingedrukt = rem verlagen
+ else if((modeL == 2) && L) { // links ingedrukt = rem verlagen
if(remPerc>1){
remPerc--; // Rem percentage verlagen met 1
}
L = false;
}
- else if(modeR == 2 && R) { // Rechts half ingedrukt = rem verhogen
- if(remPerc<9){
+ else if((modeR == 2) && R) { // Rechts half ingedrukt = rem verhogen
+ if(remPerc<10){
remPerc++; // rem percentage verhogen met 1
}
R = false;
@@ -305,7 +333,7 @@
pwmM1.write(0.5); // 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;