Iris van Leeuwen / Mbed 2 deprecated Lampje_EMG_Gr6

2 losse EMG signalen van de biceps en deltoid

Dependencies:   HIDScope MODSERIAL mbed-dsp mbed Encoder

Fork of Lampje_EMG_Gr6 by Jesse Kaiser

Files at this revision

API Documentation at this revision

Revision 36:2166eaca545e, committed 2014-11-05

Comitter:
irisl
Date:
Wed Nov 05 16:10:07 2014 +0000
Parent:
35:72d6cd2b2162
Child:
37:f72a3902482f
Commit message:
Laatste versie groep 6, robotarm aansturing mbv EMG

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Tue Nov 04 09:46:26 2014 +0000
+++ b/main.cpp	Wed Nov 05 16:10:07 2014 +0000
@@ -6,10 +6,10 @@
 #include "PwmOut.h"
 
 #define TSAMP 0.005
-#define K_P1 (3.5)
-#define K_I1 (0.01)
-#define K_P2 (3.5)
-#define K_I2 (0.04)
+#define K_P1 (0.8)
+#define K_I1 (0.001)
+#define K_P2 (4.0)
+#define K_I2 (0.01)
 #define I_LIMIT 1.
 #define l_arm 0.5
 
@@ -31,7 +31,7 @@
 //EMG
 AnalogIn    emg0(PTB0); //Analog input
 AnalogIn    emg1(PTC2); //Analog input
-HIDScope scope(3);
+HIDScope scope(4);
 
 //motor1 25D
 Encoder motor1(PTD3,PTD5); //wit, geel
@@ -75,8 +75,7 @@
 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.2538762935939654, -0.4107804719728833, 0.2538762935939654, 0.4107804719728833, 0.4922474128120692};
-//0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
+float highnotch_const[] = {0.9149684297741606, -1.8299368595483212, 0.9149684297741606, 1.8226935021735358, -0.8371802169231065 ,0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
 //state values
 float highnotch_biceps_states[8];
 float highnotch_deltoid_states[8];
@@ -147,7 +146,7 @@
     scope.set(1,filtered_biceps);  //processed float biceps
     scope.set(2,filtered_average_bi); //processed float deltoid
     //scope.set(2,filtered_deltoid);  //processed float biceps
-    //scope.set(3,filtered_average_del); //processed float deltoid
+    scope.set(3,filtered_average_del); //processed float deltoid
     scope.send();
 
 }
@@ -264,13 +263,13 @@
 {
     speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
     setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; //bepalen van de setpoint
-    if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //Het eerste getal geeft een aantal graden weer, dus alleen dit hoeft aangepast te worden/
-        setpoint1 = (11.3*2.3*2.0*PI/360); //Hier wordt er een grens bepaald voor de hoek.
+    if(setpoint1 > (11*2.3*2.0*PI/360)) { //Het eerste getal geeft een aantal graden weer, dus alleen dit hoeft aangepast te worden/
+        setpoint1 = (11*2.3*2.0*PI/360); //Hier wordt er een grens bepaald voor de hoek.
     }
-    if(setpoint1 < -(11.3*2.3*2.0*PI/360)) {
-        setpoint1 = -(11.3*2.3*2.0*PI/360);
+    if(setpoint1 < -(11*2.3*2.0*PI/360)) {
+        setpoint1 = -((11*2.3*2.0*PI/360));
     }
-    if(setpoint1 <= -((11.3*2.3*2.0*PI/360)-0.1)) {
+    if(setpoint1 <= -((11*2.3*2.0*PI/360)-0.1)) {
         staat1 = 1;
         prev_setpoint1 = setpoint1;
     }
@@ -287,7 +286,7 @@
         setpoint1 = -(11.3*2.3*2.0*PI/360);
     }
     prev_setpoint1 = setpoint1;
-    if(setpoint1 >= (11.3*2.3*2.0*PI/360)-0.1) {
+    if(setpoint1 >= ((11.3*2.3*2.0*PI/360)-0.1)) {
         staat1 = 1;
     }
 }
@@ -315,7 +314,7 @@
     //  setpoint1 = (0*2.3*2.0*PI/360);
     //}
     if(setpoint1 < -(0.0*2.3*2.0*PI/360)) {
-        setpoint1 = -(0.0*2.3*2.0*PI/360);
+        setpoint1 = -((0.0*2.3*2.0*PI/360));
     }
     prev_setpoint1 = setpoint1;
 }
@@ -324,17 +323,17 @@
 void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
 {
     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 > (120*2.0*PI/360)) { //setpoint in graden
-        setpoint2 = (120.0*2.0*PI/360);
+    setpoint2 = -(120*2.0*PI/360);
+    /*setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
+    if(setpoint2 > (100*2.0*PI/360)) { //setpoint in graden
+        setpoint2 = (100.0*2.0*PI/360);
     }
-    if(setpoint2 < -(120.0*2.0*PI/360)) {
-        setpoint2 = -(120.0*2.0*PI/360);
+    if(setpoint2 < -(100.0*2.0*PI/360)) {
+        setpoint2 = -(100.0*2.0*PI/360);
     }
-
+*/
     prev_setpoint2 = setpoint2;
-    if(setpoint2 <= -((120.0*2.0*PI/360)-0.1)) {
+    if(setpoint2 <= -((100.0*2.0*PI/360)-0.1)) {
         staat2 = 1;
     }
 }
@@ -342,16 +341,18 @@
 // Motor2 balletje in het midden slaan
 void arm_mid ()
 {
-    speed2_rad = -4.0;
-    setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
-    if(setpoint2 > (120.0*2.0*PI/360)) {
-        setpoint2 = (120.0*2.0*PI/360);
+    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 > (100*2.0*PI/360)) { //setpoint in graden
+        setpoint2 = (100.0*2.0*PI/360);
     }
-    if(setpoint2 < -(120.0*2.0*PI/360)) {
-        setpoint2 = -(120.0*2.0*PI/360);
+    if(setpoint2 < -(100.0*2.0*PI/360)) {
+        setpoint2 = -(100.0*2.0*PI/360);
     }
+*/
     prev_setpoint2 = setpoint2;
-    if(setpoint2 <= -((120.0*2.0*PI/360)-0.1)) {
+    if(setpoint2 <= -((100.0*2.0*PI/360)-0.1)) {
         staat2 = 1;
     }
 }
@@ -359,16 +360,18 @@
 // Motor2 balletje op het laagst slaan
 void arm_laag ()
 {
-    speed2_rad = -2.0;
-    setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
-    if(setpoint2 > (120*2.0*PI/360)) {
-        setpoint2 = (120*2.0*PI/360);
+    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 > (100*2.0*PI/360)) { //setpoint in graden
+        setpoint2 = (100.0*2.0*PI/360);
     }
-    if(setpoint2 < -(120.0*2.0*PI/360)) {
-        setpoint2 = -(120.0*2.0*PI/360);
+    if(setpoint2 < -(100.0*2.0*PI/360)) {
+        setpoint2 = -(100.0*2.0*PI/360);
     }
+*/
     prev_setpoint2 = setpoint2;
-    if(setpoint2 <= -((120.0*2.0*PI/360)-0.1)) {
+    if(setpoint2 <= -((100.0*2.0*PI/360)-0.1)) {
         staat2 = 1;
     }
 }
@@ -404,9 +407,9 @@
     }
     pwm_motor1.write(abs(pwm_out1));
     if(pwm_out1 > 0) {
-        motordir1 = 0;
+        motordir1 = 1;
     } else {
-        motordir1 = 1;
+        motordir1 = 0;
     }
 
     //MOTOR2
@@ -439,7 +442,6 @@
         }
         if(wait_iterator1 > 1200) {
             staat1 = 2;
-
             batje_begin_rechts();
         }
     }
@@ -466,7 +468,7 @@
         } else if(staat2 == 1) {
             wait_iterator2++;
         }
-        if(wait_iterator2 > 1200) {
+        if(wait_iterator2 > 400) {
             staat2 = 2;
 
             arm_begin();
@@ -505,8 +507,8 @@
 // Hoofdprogramma, hierin staat de aansturing vd LED
 int main()
 {
-    float emg_arm1 = 0.45;
-    float emg_arm2 = 0.45;
+    float emg_arm1 = 0.03;
+    float emg_arm2 = 0.02;
     pwm_motor1.period_us(100);
     motor1.setPosition(0);
     pwm_motor2.period_us(100);
@@ -533,7 +535,7 @@
             do {
                 ShineRed();
             } 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
+            if (filtered_average_bi > 0.035) { // 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
@@ -549,14 +551,14 @@
                         stopblinkgreen();
                         pc.printf("ShineBlue.\n");
                         ShineBlue();
-                        batje_hoek = 2;
+                        //batje_hoek = 2;
                         wait(4);
                         break;
                     } else if (filtered_average_bi > emg_arm1 && filtered_average_del < emg_arm2) { // bi aanspannen --> batje naar rechts
                         stopblinkgreen();
                         pc.printf("ShineRed.\n");
                         ShineRed();
-                        batje_hoek = 1;
+                        //batje_hoek = 1;
                         wait (4);
                         break;
                     }