Revision 71:aee1289bc16a, committed 2015-06-19

Comitter:

jessekaiser

Date:

Fri Jun 19 23:28:40 2015 +0000

Parent:

70:e84629c7dfed

Child:

72:4d01b79ad332

Commit message:

Eindconcept 10.

Changed in this revision

--- a/main.cpp	Fri Jun 19 11:39:12 2015 +0000
+++ b/main.cpp	Fri Jun 19 23:28:40 2015 +0000
@@ -13,14 +13,10 @@
 //#include "HIDScope.h"
 
 #define P_Gain      0.99
-#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
-#define MAX_bi      0.40 //Can be used for normalisation of the EMG signal of the biceps
-#define MAX_tri     0.60
-#define MAX_pect    0.48
-#define MAX_delt    1.07
+#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
 #define EMG_tresh1   0.01
 #define EMG_tresh2   0.01
 #define EMG_tresh3   0.01
@@ -28,7 +24,7 @@
 #define H_Gain  3.5
 #define Pt_x    0.50
 #define Pt_y    0.50
-#define error_tresh 0.03
+#define error_tresh 0.01
 
 //Motor control
 DigitalOut Dirx(p21);
@@ -51,15 +47,15 @@
 DigitalOut MS3(p29);
 
 //EMG inputs
-AnalogIn emg1(p15); //EMG bordje bovenop, biceps
-AnalogIn emg2(p16); //triceps
+AnalogIn emg1(p15); 
+AnalogIn emg2(p16); 
 AnalogIn emg3(p17);
 AnalogIn emg4(p18);
 
 //HIDScope scope(4);
 //Ticker   scopeTimer;
 
-//lcd
+//lcd screen
 C12832_LCD lcd;
 
 //Variables for motor control
@@ -73,7 +69,7 @@
 arm_biquad_casd_df1_inst_f32 lowpass_triceps;
 arm_biquad_casd_df1_inst_f32 lowpass_pect;
 arm_biquad_casd_df1_inst_f32 lowpass_deltoid;
-//lowpass filter settings: Fc = 2 Hz, Fs = 500 Hz, Gain = -3 dB
+//lowpass filter settings: Fc = 2 Hz, Fs = 500 Hz
 float lowpass_const[] = {0.00015514839749793376, 0.00031029679499586753, 0.00015514839749793376, 1.9644602512795832, -0.9650808448695751};
 arm_biquad_casd_df1_inst_f32 highnotch_biceps;
 arm_biquad_casd_df1_inst_f32 highnotch_triceps;
@@ -94,11 +90,11 @@
 
 //global variabels
 float filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid;
-float speed_old1, speed_old2, speed_old3, speed_old4;
-float acc1, acc2, acc3, acc4;
-float force1, force2, force3, force4;
-float speed1, speed2, speed3, speed4;
-float damping1, damping2, damping3, damping4;
+float speed_old1, speed_old2
+float acc1, acc2
+float force1, force2
+float speed1, speed2
+float damping1, damping2
 float emg_x, emg_y;
 float cx = 0;
 float cy = 0;
@@ -178,10 +174,42 @@
     } else {
         Enabley = 0;
     }
+    wait(0.01);
+    
+    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);
+    damping2 = speed2 * Damp;
+    step_freq2 = setpoint * speed2;
+    Stepx.period(1.0/step_freq2);
+    speed_old2 = speed2;
 
+    if (emg_x > 0) {
+        Dirx = 0;
+    }
+    if (emg_x < 0) {
+        Dirx = 1;
+    }
+    //Speed limit
+    if (speed2 > 1) {
+        speed2 = 1;
+        step_freq2 = setpoint;
+    }
+    //EMG treshold
+    if (filtered_pect < EMG_tresh3 && filtered_deltoid < EMG_tresh4) {
+        Enablex = 1; //Enable = 1 turns the motor off.
+    } else {
+        Enablex = 0;
+    }
+    wait(0.01);
 }
 
-void looper_motorx()
+
+
+/*void looper_motorx()
 {
 
     emg_x = (filtered_pect - filtered_deltoid);
@@ -213,99 +241,91 @@
         Enablex = 0;
     }
 
-}
+}*/
 
 int main()
 {
     // Attach the HIDScope::send method from the scope object to the timer at 500Hz. Hier wordt de sample freq aangegeven.
     // scopeTimer.attach_us(&scope, &HIDScope::send, 2e3);
-
-    MS1 = 1;
-    MS2 = 0;
-    MS3 = 0;
-
-    Stepx.write(0.5); // Duty cycle of 50%
-    Stepy.write(0.5);
+    /*
+        MS1 = 1;
+        MS2 = 0;
+        MS3 = 0;
 
-    Enablex = 1;
-    Enabley = 1;
-    wait(1);
-    lcd.printf("Start homing");
-    wait(2);
-    lcd.cls();
-    wait(1);
-    Enablex = 0;
-    Enabley = 0;
-    while(errorx > error_tresh || errory > error_tresh) {
+        Stepx.write(0.5); // Duty cycle of 50%
+        Stepy.write(0.5);
 
-        Ps_x = Posx.read();
-        Ps_y = Posy.read();
-        errorx = fabs(Pt_x - Ps_x);
-        errory = fabs(Ps_y - Pt_y);
-        lcd.printf("%.2f %.2f \n", errorx, errory);
-        
-        
-        if (Ps_x < 0.50 && errorx > error_tresh) {
-            Dirx = 0;
-            //errorx = Pt_x - Ps_x;
-            cx = errorx * H_Gain;
+        Enablex = 1;
+        Enabley = 1;
+        wait(1);
+        lcd.printf("Start homing");
+        wait(2);
+        lcd.cls();
+        wait(1);
+        Enablex = 0;
+        Enabley = 0;
+        while(errorx > error_tresh || errory > error_tresh) {
 
-            float hnew_step_freqx;
-            hnew_step_freqx = ((1-P_Gain)*setpoint*cx) + (P_Gain*hstep_freqx);
-            hstep_freqx = hnew_step_freqx;
-            Stepx.period(1.0/hstep_freqx);
-            wait(0.01);
-        }
-        if (Ps_y > 0.50 && errory > error_tresh) {
-            Diry = 0;
-            //errory = Ps_y - Pt_y;
-            cy = errory * H_Gain;
+            Ps_x = Posx.read();
+            Ps_y = Posy.read();
+            errorx = fabs(Pt_x - Ps_x);
+            errory = fabs(Ps_y - Pt_y);
+            lcd.printf("%.2f %.2f \n", errorx, errory);
+
 
-            float hnew_step_freqy;
-            hnew_step_freqy = ((1-P_Gain)*setpoint*cy) + (P_Gain*hstep_freqy);
-            hstep_freqy = hnew_step_freqy;
-            Stepy.period(1.0/hstep_freqy);
-            wait(0.01);
-        }
-
-        if (Ps_x > 0.50 && errorx > error_tresh) {
-            Dirx = 1;
-            //errorx = Pt_x - Ps_x;
-            cx = errorx * H_Gain;
-
-            float hnew_step_freqx;
-            hnew_step_freqx = ((1-P_Gain)*setpoint*cx) + (P_Gain*hstep_freqx);
-            hstep_freqx = hnew_step_freqx;
-            Stepx.period(1.0/hstep_freqx);
-            wait(0.01);
-        }
-        if (Ps_y < 0.50 && errory > error_tresh) {
-            Diry = 1;
-            //errory = Ps_y - Pt_y;
-            cy = errory * H_Gain;
+            if (Ps_x < 0.50 && errorx > error_tresh) {
+                Dirx = 0;
+                cx = errorx * H_Gain;
+                float hnew_step_freqx;
+                hnew_step_freqx = ((1-P_Gain)*setpoint*cx) + (P_Gain*hstep_freqx);
+                hstep_freqx = hnew_step_freqx;
+                Stepx.period(1.0/hstep_freqx);
+                wait(0.01);
+            }
+            if (Ps_y > 0.50 && errory > error_tresh) {
+                Diry = 0;
+                cy = errory * H_Gain;
+                float hnew_step_freqy;
+                hnew_step_freqy = ((1-P_Gain)*setpoint*cy) + (P_Gain*hstep_freqy);
+                hstep_freqy = hnew_step_freqy;
+                Stepy.period(1.0/hstep_freqy);
+                wait(0.01);
+            }
 
-            float hnew_step_freqy;
-            hnew_step_freqy = ((1-P_Gain)*setpoint*cy) + (P_Gain*hstep_freqy);
-            hstep_freqy = hnew_step_freqy;
-            Stepy.period(1.0/hstep_freqy);
-            wait(0.01);
-        }
+            if (Ps_x > 0.50 && errorx > error_tresh) {
+                Dirx = 1;
+                cx = errorx * H_Gain;
+                float hnew_step_freqx;
+                hnew_step_freqx = ((1-P_Gain)*setpoint*cx) + (P_Gain*hstep_freqx);
+                hstep_freqx = hnew_step_freqx;
+                Stepx.period(1.0/hstep_freqx);
+                wait(0.01);
+            }
+            if (Ps_y < 0.50 && errory > error_tresh) {
+                Diry = 1;
+                cy = errory * H_Gain;
+                float hnew_step_freqy;
+                hnew_step_freqy = ((1-P_Gain)*setpoint*cy) + (P_Gain*hstep_freqy);
+                hstep_freqy = hnew_step_freqy;
+                Stepy.period(1.0/hstep_freqy);
+                wait(0.01);
+            }
 
-    }
-    lcd.printf("Done");
-    wait(2);
-    lcd.cls();
-    wait(1);
-    Enablex = 1;
-    Enabley = 1;
-    wait(3);
-    lcd.printf("Start EMG Control");
-    wait(2);
-    lcd.cls();
-    wait(1);
-    Enablex = 0;
-    Enabley = 0;
-
+        }
+        lcd.printf("Done");
+        wait(2);
+        lcd.cls();
+        wait(1);
+        Enablex = 1;
+        Enabley = 1;
+        wait(3);
+        lcd.printf("Start EMG Control");
+        wait(2);
+        lcd.cls();
+        wait(1);
+        Enablex = 0;
+        Enabley = 0;
+    */
     MS1 = 1;
     MS2 = 0;
     MS3 = 0;
@@ -326,8 +346,8 @@
     arm_biquad_cascade_df1_init_f32(&highnotch_deltoid, 2 , highnotch_const, highnotch_deltoid_states);
     emgtimer.attach(looper_emg, 0.01);
 
-    Ticker looptimer1;
-    looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor, why this freq?
+   //Ticker looptimer1;
+    //looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor, why this freq?
 
     Ticker looptimer2;
     looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor
@@ -338,12 +358,12 @@
 
     while (1) {
 
-
+    
         //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 %.2f %.2f  \n", emg_y_abs, step_freq1, filtered_biceps, filtered_triceps);
-        wait(0.01); 
+        lcd.printf("%.2f %.2f %.2f %.2f  \n", Stepx.read(), step_freq1, Stepy.read(), step_freq2);
+        wait(0.01);
 
     }
 }