Revision 67:fba5b64bb295, committed 2015-06-19

Comitter:

jessekaiser

Date:

Fri Jun 19 09:11:25 2015 +0000

Parent:

66:c094d1868b30

Child:

68:2b778b6da923

Commit message:

homing 6;

Changed in this revision

--- a/main.cpp	Thu Jun 18 11:50:07 2015 +0000
+++ b/main.cpp	Fri Jun 19 09:11:25 2015 +0000
@@ -13,7 +13,7 @@
 //#include "HIDScope.h"
 
 #define P_Gain      0.995
-#define K_Gain      25   //Gain of the filtered EMG signal
+#define K_Gain      150   //Gain of the filtered EMG signal
 #define Damp        5    //Deceleration of the motor
 #define Mass        1    // Mass value
 #define dt          0.01 //Sample frequency
@@ -21,8 +21,8 @@
 #define MAX_tri     0.60
 #define MAX_pect    0.48
 #define MAX_delt    1.07
-#define EMG_tresh1   0.02
-#define EMG_tresh2   0.02
+#define EMG_tresh1   0.01
+#define EMG_tresh2   0.01
 #define EMG_tresh3   0.01
 #define EMG_tresh4   0.01
 #define H_Gain  3
@@ -145,9 +145,11 @@
 }
 
 void looper_motory()
-{
-    emg_y = fabs(filtered_biceps - filtered_triceps);
-    force1 = emg_y*K_Gain;
+{   float emg_y_abs;
+    
+    emg_y = (filtered_biceps - filtered_triceps);
+    emg_y_abs = fabs(emg_y);
+    force1 = emg_y_abs*K_Gain;
     force1 = force1 - damping1;
     acc1 = force1/Mass;
     speed1 = speed_old1 + (acc1 * dt);
@@ -180,8 +182,10 @@
 
 void looper_motorx()
 {
-    emg_x = fabs(filtered_pect - filtered_deltoid);
-    force2 = emg_x*K_Gain;
+    float emg_x_abs;
+    emg_x = (filtered_pect - filtered_deltoid);
+    emg_x_abs = fabs(emg_x);
+    force2 = emg_x_abs*K_Gain;
     force2 = force2 - damping2;
     acc2 = force2/Mass;
     speed2 = speed_old2 + (acc2 * dt);
@@ -231,13 +235,13 @@
     wait(1);
     Enablex = 0;
     Enabley = 0;
-    while(errorx > 0.03 || errory > 0.03) {
-        lcd.printf("%.2f %.2f  \n", Ps_x, Ps_y);
+    while(errorx > 0.03 && errory > 0.03) {
+        lcd.printf("%.0f %.2f  \n", hstep_freqx, hstep_freqy );
 
         Ps_x = Posx.read();
         Ps_y = Posy.read();
 
-        if (Ps_x < 0.88) {
+        if (Ps_x < 0.88 && errorx > 0.03) {
             Dirx = 0;
             errorx = Pt_x - Ps_x;
             cx = errorx * H_Gain;
@@ -248,7 +252,7 @@
             Stepx.period(1.0/hstep_freqx);
             wait(0.01);
         }
-        if (Ps_y > 0.25) {
+        if (Ps_y > 0.25 && errory > 0.03) {
             Diry = 0;
             errory = Ps_y - Pt_y;
             cy = errory * H_Gain;
@@ -273,7 +277,7 @@
     lcd.printf("Start EMG Control");
     wait(2);
     lcd.cls();
-    wait(1);
+    wait(1); 
     Enablex = 0;
     Enabley = 0; 
     Ticker emgtimer;    //biceps
@@ -310,7 +314,7 @@
          //lcd.printf("x %.2f, y %.2f \n", Posx.read(), Posy.read());
          //lcd.printf("%.2f, %.2f %.2f %.2f \n", filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid); //Filtered EMG values
          //lcd.printf("1 %.0f, 2 %.0f \n", step_freq1, step_freq2); //step_freq value of every EMG sensor
-         lcd.printf("%.2f %.2f \n", force1, force2);
+         lcd.printf("%.0f %.2f \n", Stepy.read(), emg_y);
          //lcd.printf("%.2f, %.2f %.2f %.2f \n", gain_biceps, gain_triceps, gain_pect, gain_deltoid);
          //lcd.printf("%.2f, %.2f %.2f %.2f \n", norm_biceps, norm_triceps, norm_pect, norm_deltoid);
          wait(0.01);