Iris van Leeuwen
2 losse EMG signalen van de biceps en deltoid
Dependencies: HIDScope MODSERIAL mbed-dsp mbed Encoder
Fork of
Lampje_EMG_Gr6
by 
Jesse Kaiser
Revision 35:72d6cd2b2162, committed 2014-11-04
- Comitter:
- irisl
- Date:
- Tue Nov 04 09:46:26 2014 +0000
- Parent:
- 34:7e0bf3e9f135
- Child:
- 36:2166eaca545e
- Commit message:
- kleine aanpassingen, heeft problemen met de filtering;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Nov 04 08:39:49 2014 +0000
+++ b/main.cpp Tue Nov 04 09:46:26 2014 +0000
@@ -6,10 +6,10 @@
#include "PwmOut.h"
#define TSAMP 0.005
-#define K_P1 (3.5)
+#define K_P1 (3.5)
#define K_I1 (0.01)
-#define K_P2 (3.5)
-#define K_I2 (0.01)
+#define K_P2 (3.5)
+#define K_I2 (0.04)
#define I_LIMIT 1.
#define l_arm 0.5
@@ -75,7 +75,8 @@
arm_biquad_casd_df1_inst_f32 highnotch_biceps;
arm_biquad_casd_df1_inst_f32 highnotch_deltoid;
//highpass filter settings: Fc = 10 Hz, Fs = 500 Hz, Gain = -3 dB, notch Fc = 50, Fs =500Hz, Gain = -3 dB
-float highnotch_const[] = {0.9149684297741606, -1.8299368595483212, 0.9149684297741606, 1.8226935021735358, -0.8371802169231065 ,0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
+float highnotch_const[] = {0.9149684297741606, -1.8299368595483212, 0.9149684297741606, 1.8226935021735358, -0.8371802169231065 , 0.2538762935939654, -0.4107804719728833, 0.2538762935939654, 0.4107804719728833, 0.4922474128120692};
+//0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
//state values
float highnotch_biceps_states[8];
float highnotch_deltoid_states[8];
@@ -300,7 +301,7 @@
setpoint1 = (0*2.3*2.0*PI/360);
}
//if(setpoint1 < -(0*2.3*2.0*PI/360)) {
- // setpoint1 = -(0*2.3*2.0*PI/360);
+ // setpoint1 = -(0*2.3*2.0*PI/360);
//}
prev_setpoint1 = setpoint1;
}
@@ -325,15 +326,15 @@
speed2_rad = -6.0; //positief is CCW, negatief CW (boven aanzicht)
//setpoint2 = -(120*2.0*PI/360);
setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
- if(setpoint2 > (140*2.0*PI/360)) { //setpoint in graden
+ if(setpoint2 > (120*2.0*PI/360)) { //setpoint in graden
setpoint2 = (120.0*2.0*PI/360);
}
- if(setpoint2 < -(140.0*2.0*PI/360)) {
- setpoint2 = -(140.0*2.0*PI/360);
+ if(setpoint2 < -(120.0*2.0*PI/360)) {
+ setpoint2 = -(120.0*2.0*PI/360);
}
-
+
prev_setpoint2 = setpoint2;
- if(setpoint2 <= -((140.0*2.0*PI/360)-0.1)) {
+ if(setpoint2 <= -((120.0*2.0*PI/360)-0.1)) {
staat2 = 1;
}
}
@@ -343,14 +344,14 @@
{
speed2_rad = -4.0;
setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
- if(setpoint2 > (140.0*2.0*PI/360)) {
- setpoint2 = (155.0*2.0*PI/360);
+ if(setpoint2 > (120.0*2.0*PI/360)) {
+ setpoint2 = (120.0*2.0*PI/360);
}
- if(setpoint2 < -(140.0*2.0*PI/360)) {
- setpoint2 = -(140.0*2.0*PI/360);
+ if(setpoint2 < -(120.0*2.0*PI/360)) {
+ setpoint2 = -(120.0*2.0*PI/360);
}
prev_setpoint2 = setpoint2;
- if(setpoint2 <= -((140.0*2.0*PI/360)-0.1)) {
+ if(setpoint2 <= -((120.0*2.0*PI/360)-0.1)) {
staat2 = 1;
}
}
@@ -360,14 +361,14 @@
{
speed2_rad = -2.0;
setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
- if(setpoint2 > (155*2.0*PI/360)) {
- setpoint2 = (155*2.0*PI/360);
+ if(setpoint2 > (120*2.0*PI/360)) {
+ setpoint2 = (120*2.0*PI/360);
}
- if(setpoint2 < -(140.0*2.0*PI/360)) {
- setpoint2 = -(140.0*2.0*PI/360);
+ if(setpoint2 < -(120.0*2.0*PI/360)) {
+ setpoint2 = -(120.0*2.0*PI/360);
}
prev_setpoint2 = setpoint2;
- if(setpoint2 <= ((140.0*2.0*PI/360)-0.1)) {
+ if(setpoint2 <= -((120.0*2.0*PI/360)-0.1)) {
staat2 = 1;
}
}
@@ -435,74 +436,77 @@
wait_iterator1 = 0;
} else if(staat1 ==1) {
wait_iterator1++;
- if(wait_iterator1 > 1200) {
- staat1 = 2;
+ }
+ if(wait_iterator1 > 1200) {
+ staat1 = 2;
- batje_begin_rechts();
- }
+ batje_begin_rechts();
}
}
+
if (batje_hoek == 2) {
if(staat1 == 0) {
batje_links();
wait_iterator1 = 0;
} else if(staat1 ==1) {
wait_iterator1++;
- if(wait_iterator1 > 1200) {
- staat1 = 2;
+ }
+ if(wait_iterator1 > 1200) {
+ staat1 = 2;
- batje_begin_links ();
- }
+ batje_begin_links ();
}
}
+
if(arm_hoogte == 1) {
if(staat2 == 0) {
arm_laag();
wait_iterator2 = 0;
} else if(staat2 == 1) {
wait_iterator2++;
- if(wait_iterator2 > 400) {
- staat2 = 2;
+ }
+ if(wait_iterator2 > 1200) {
+ staat2 = 2;
- arm_begin();
- }
+ arm_begin();
}
}
+
if(arm_hoogte == 2) {
if(staat2 == 0) {
arm_mid();
wait_iterator2 = 0;
} else if(staat2 == 1) {
- wait_iterator2++;
- if(wait_iterator2 > 400) {
- staat2 = 2;
+ } wait_iterator2++;
+ if(wait_iterator2 > 400) {
+ staat2 = 2;
- arm_begin();
- }
+ arm_begin();
}
}
+
if(arm_hoogte == 3) {
if(staat2 == 0) {
arm_hoog();
wait_iterator2 = 0;
} else if(staat2 == 1) {
wait_iterator2++;
- if(wait_iterator2 > 400) {
- staat2 = 2;
-
- arm_begin();
- }
+ }
+ if(wait_iterator2 > 400) {
+ staat2 = 2;
+ arm_begin();
}
}
+}
-}
// Hoofdprogramma, hierin staat de aansturing vd LED
int main()
{
-
+ float emg_arm1 = 0.45;
+ float emg_arm2 = 0.45;
pwm_motor1.period_us(100);
motor1.setPosition(0);
pwm_motor2.period_us(100);
@@ -528,27 +532,27 @@
pc.printf("Span de biceps aan om het instellen te starten.\n");
do {
ShineRed();
- } while(filtered_average_bi < 0.05 && filtered_average_del <0.05); // In rust, geen meting
- if (filtered_average_bi > 0.05) { // Beginnen met meting wanneer biceps wordt aangespannen
+ } while(filtered_average_bi < emg_arm1 && filtered_average_del < emg_arm2); // In rust, geen meting
+ if (filtered_average_bi > 0.04) { // Beginnen met meting wanneer biceps wordt aangespannen
BlinkRed(10); // 2 seconden rood knipperen, geen signaal verwerking
BlinkGreen(); // groen knipperen, meten van spieraanspanning
while (1) { // eerste loop, keuze voor de positie van het batje
pc.printf("In de loop.\n");
- if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { //bi en del aangespannen --> batje in het midden
+ if (filtered_average_bi > emg_arm1 && filtered_average_del > emg_arm2) { //bi en del aangespannen --> batje in het midden
stopblinkgreen();
pc.printf("ShineGreen.\n");
ShineGreen();
wait (4);
break;
}
- if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> batje naar links
+ if (filtered_average_bi < emg_arm1 && filtered_average_del > emg_arm2) { // del aanspannen --> batje naar links
stopblinkgreen();
pc.printf("ShineBlue.\n");
ShineBlue();
batje_hoek = 2;
wait(4);
break;
- } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> batje naar rechts
+ } else if (filtered_average_bi > emg_arm1 && filtered_average_del < emg_arm2) { // bi aanspannen --> batje naar rechts
stopblinkgreen();
pc.printf("ShineRed.\n");
ShineRed();
@@ -560,7 +564,7 @@
BlinkGreen();
while (1) { // loop voor het instellen van de kracht
pc.printf("In de loop.\n");
- if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { // bi en del aanspannen --> hoog slaan
+ if (filtered_average_bi > emg_arm1 && filtered_average_del > emg_arm2) { // bi en del aanspannen --> hoog slaan
stopblinkgreen();
pc.printf("ShineGreen.\n");
ShineGreen();
@@ -568,14 +572,14 @@
wait (4);
break;
}
- if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> laag slaan
+ if (filtered_average_bi < emg_arm1 && filtered_average_del > emg_arm2) { // del aanspannen --> laag slaan
stopblinkgreen();
pc.printf("ShineBlue.\n");
ShineBlue();
arm_hoogte = 1;
wait(4);
break;
- } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> midden slaan
+ } else if (filtered_average_bi > emg_arm1 && filtered_average_del < emg_arm2) { // bi aanspannen --> midden slaan
stopblinkgreen();
pc.printf("ShineRed.\n");
ShineRed();
@@ -586,7 +590,6 @@
}
}
-
}
}
}
\ No newline at end of file