Revision 6:98a27fef0223, committed 2014-11-01

Comitter:

DominiqueC

Date:

Sat Nov 01 12:36:57 2014 +0000

Parent:

5:5085197c02be

Child:

7:c2c3d1ade6bd

Commit message:

Eindscript deltoid ;

Changed in this revision

--- a/main.cpp	Sat Nov 01 12:29:20 2014 +0000
+++ b/main.cpp	Sat Nov 01 12:36:57 2014 +0000
@@ -41,45 +41,45 @@
 DigitalOut motordir2(M2_DIR);
 DigitalOut motordir1(M1_DIR);
 AnalogIn emg0(PTB0); //Biceps
-AnalogIn emg1(PTB1); //Triceps
+AnalogIn emg1(PTB1); //deltoid
 
 MODSERIAL pc(USBTX,USBRX,64,1024);
 
 
 float emg0_value_f32,filtered_emg0_notch,filtered_emg0_notch_highpass,filtered_emg0_notch_highpass_lowpass,filtered_emg0_eindsignaal_abs,envelop_emg0,pwm_to_motor1,max_value_biceps,min_value_biceps; //variable to store value in for biceps
-float emg1_value_f32,filtered_emg1_notch,filtered_emg1_notch_highpass,filtered_emg1_notch_highpass_lowpass,filtered_emg1_eindsignaal_abs,envelop_emg1,pwm_to_motor2,max_value_triceps,min_value_triceps,metingstatus; //variable to store value in for triceps
+float emg1_value_f32,filtered_emg1_notch,filtered_emg1_notch_highpass,filtered_emg1_notch_highpass_lowpass,filtered_emg1_eindsignaal_abs,envelop_emg1,pwm_to_motor2,max_value_deltoid,min_value_deltoid,metingstatus; //variable to store value in for deltoid
 
 arm_biquad_casd_df1_inst_f32 notch_biceps;
-arm_biquad_casd_df1_inst_f32 notch_triceps;
+arm_biquad_casd_df1_inst_f32 notch_deltoid;
 // constants for 50 Hz notch (bandbreedte 2 Hz)
 float notch_const[] = {0.9695312529087462, -0.0, 0.9695312529087462, 0.0, -0.9390625058174924}; //constants for 50Hz notch
 //state values
 float notch_biceps_states[4];
-float notch_triceps_states[4];
+float notch_deltoid_states[4];
 
 arm_biquad_casd_df1_inst_f32 highpass_biceps;
-arm_biquad_casd_df1_inst_f32 highpass_triceps;
+arm_biquad_casd_df1_inst_f32 highpass_deltoid;
 //constants for 20Hz highpass
 float highpass_const[] = {0.638945525159022, -1.277891050318045, 0.638945525159022, 1.142980502539901, -0.412801598096189};
 //state values
 float highpass_biceps_states[4];
-float highpass_triceps_states[4];
+float highpass_deltoid_states[4];
 
 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
-arm_biquad_casd_df1_inst_f32 lowpass_triceps;
+arm_biquad_casd_df1_inst_f32 lowpass_deltoid;
 //constants for 80Hz lowpass
 float lowpass_const[] = {0.638945525159022, 1.277891050318045, 0.638945525159022, -1.142980502539901, -0.412801598096189};
 //state values
 float lowpass_biceps_states[4];
-float lowpass_triceps_states[4];
+float lowpass_deltoid_states[4];
 
 arm_biquad_casd_df1_inst_f32 envelop_biceps;
-arm_biquad_casd_df1_inst_f32 envelop_triceps;
+arm_biquad_casd_df1_inst_f32 envelop_deltoid;
 //constants for envelop
 float envelop_const[] = {0.005542711916075981, 0.011085423832151962, 0.005542711916075981, 1.7786300789392977, -0.8008009266036016};
 // state values
 float envelop_biceps_states[4];
-float envelop_triceps_states[4];
+float envelop_deltoid_states[4];
 
 enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN,HOME}; //verschillende stadia definieren voor gebruik in CASES
 uint8_t state=RUST;
@@ -139,12 +139,12 @@
     filtered_emg0_eindsignaal_abs = fabs(filtered_emg0_notch_highpass_lowpass);  //gelijkrichter
     arm_biquad_cascade_df1_f32(&envelop_biceps, &filtered_emg0_eindsignaal_abs, &envelop_emg0, 1 );
 
-    //process emg triceps
-    arm_biquad_cascade_df1_f32(&notch_triceps, &emg1_value_f32, &filtered_emg1_notch, 1 );
-    arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 );
-    arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 );
+    //process emg deltoid
+    arm_biquad_cascade_df1_f32(&notch_deltoid, &emg1_value_f32, &filtered_emg1_notch, 1 );
+    arm_biquad_cascade_df1_f32(&highpass_deltoid, &filtered_emg1_notch, &filtered_emg1_notch_highpass, 1 );
+    arm_biquad_cascade_df1_f32(&lowpass_deltoid, &filtered_emg1_notch_highpass, &filtered_emg1_notch_highpass_lowpass, 1 );
     filtered_emg1_eindsignaal_abs = fabs(filtered_emg1_notch_highpass_lowpass);  //gelijkrichter
-    arm_biquad_cascade_df1_f32(&envelop_triceps, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 );
+    arm_biquad_cascade_df1_f32(&envelop_deltoid, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 );
 }
 
 
@@ -162,10 +162,10 @@
     arm_biquad_cascade_df1_init_f32(&notch_biceps,1 , notch_const, notch_biceps_states);
     arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states);
     arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states);
-    arm_biquad_cascade_df1_init_f32(&notch_triceps,1 , notch_const, notch_triceps_states);
-    arm_biquad_cascade_df1_init_f32(&highpass_triceps,1 ,highpass_const,highpass_triceps_states);
-    arm_biquad_cascade_df1_init_f32(&lowpass_triceps,1 ,lowpass_const,lowpass_triceps_states);
-    arm_biquad_cascade_df1_init_f32(&envelop_triceps,1 ,envelop_const,envelop_triceps_states);
+    arm_biquad_cascade_df1_init_f32(&notch_deltoid,1 , notch_const, notch_deltoid_states);
+    arm_biquad_cascade_df1_init_f32(&highpass_deltoid,1 ,highpass_const,highpass_deltoid_states);
+    arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 ,lowpass_const,lowpass_deltoid_states);
+    arm_biquad_cascade_df1_init_f32(&envelop_deltoid,1 ,envelop_const,envelop_deltoid_states);
     arm_biquad_cascade_df1_init_f32(&envelop_biceps,1 ,envelop_const,envelop_biceps_states);
 
     while(true) {
@@ -179,7 +179,7 @@
 
             case KALIBRATIE: {                                  //kalibreren met maximale inspanning
                 max_value_biceps=0;
-                max_value_triceps=0;
+                max_value_deltoid=0;
                 //maximale inspanning biceps
                 pc.printf("Kalibratie1: Span Biceps!"); //pc scherm
                 wait(5);
@@ -196,26 +196,26 @@
                 pc.printf("max value %f\n\r", max_value_biceps);
                 wait(5);
 
-                //maximale inspanning triceps
-                pc.printf("Kalibratie2: Span Triceps!"); //pc scherm
+                //maximale inspanning deltoid
+                pc.printf("Kalibratie2: Span deltoid!"); //pc scherm
                 wait(5);
                 tijdtimer.start();
                 pc.printf("Meting loopt"); //pc scherm
                 while (tijdtimer.read() <= 3) {
-                    if (envelop_emg1 > max_value_triceps) {
-                        max_value_triceps = envelop_emg1;
+                    if (envelop_emg1 > max_value_deltoid) {
+                        max_value_deltoid = envelop_emg1;
                     }
                 }
                 // tijdtimer.stop();
                 tijdtimer.reset();
-                pc.printf("max value %f\n\r", max_value_triceps);
+                pc.printf("max value %f\n\r", max_value_deltoid);
                 wait(5);
 
                 state = RICHTEN;
                 break;
             }// einde kalibratie case
 
-            case RICHTEN: {                                  //batje richten (gebruik biceps en triceps)
+            case RICHTEN: {                                  //batje richten (gebruik biceps en deltoid)
                 wait(3);
                 pc.printf("Richten");                  //regel 1 LCD scherm
                 pc.printf("Kies goal!");               //regel 2 LCD scherm
@@ -223,20 +223,15 @@
                 float new_pwm_km;
                 wait(5);
                 pc.printf("Meting loopt");
-                float kalibratiewaarde_biceps,kalibratiewaarde_triceps;
-                kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
-                kalibratiewaarde_triceps=(envelop_emg1/max_value_triceps);
-                pc.printf("biceps %f\n\r", kalibratiewaarde_biceps);
-                pc.printf("triceps %f\n\r", kalibratiewaarde_triceps);        //WEGHALEN LATER
-                //if (kalibratiewaarde_biceps > 0.3 && kalibratiewaarde_triceps <= 0.3) { //linker goal!
-                if (kalibratiewaarde_triceps >= 0.35) {
+                float kalibratiewaarde_deltoid;
+                kalibratiewaarde_deltoid=(envelop_emg1/max_value_deltoid);
+                pc.printf("deltoid %f\n\r", kalibratiewaarde_deltoid);        //WEGHALEN LATER
+                if (kalibratiewaarde_deltoid >= 0.35) {
                     setpointkm = -127;          //11,12graden naar links
                     pc.printf("links");
-                    //} else if (kalibratiewaarde_biceps <= 0.3 && kalibratiewaarde_triceps > 0.3) { //rechter goal!
-                } else if (kalibratiewaarde_triceps>0.1 && kalibratiewaarde_triceps<=0.35) {
+                } else if (kalibratiewaarde_deltoid>0.1 && kalibratiewaarde_deltoid<=0.35) {
                     setpointkm = 0;        //11,12graden naar rechts
                     pc.printf("midden");
-                    //} else { //middelste goal!
                 } else {
                     setpointkm = 127;
                     pc.printf("rechts");
@@ -273,10 +268,10 @@
                 float kalibratiewaarde_biceps;
                 kalibratiewaarde_biceps=(envelop_emg0/max_value_biceps);
                 pc.printf("biceps %f\n\r", kalibratiewaarde_biceps);      //WEGHALEN LATER
-                if (kalibratiewaarde_biceps <= 0.3) { //kalibratiewaarde_biceps<0.3 goal onderin
+                if (kalibratiewaarde_biceps <= 0.2) { //kalibratiewaarde_biceps<0.3 goal onderin
                     thetadot=THETADOT0;
                     pc.printf("ONDERSTE GOAL");
-                } else if (kalibratiewaarde_biceps>0.3 && kalibratiewaarde_biceps<=0.6) { //0.3<kalibratiewaarde_biceps<0.6 goal midden
+                } else if (kalibratiewaarde_biceps>0.2 && kalibratiewaarde_biceps<=0.6) { //0.3<kalibratiewaarde_biceps<0.6 goal midden
                     thetadot=THETADOT1;
                     pc.printf("MIDDELSTE GOAL");
                 } else { //goal bovenin