Diff: main.cpp

Revision:

7:20757784f5bf

Parent:

6:d03e4fa3a2a5

Child:

8:69b7085f5343

diff -r d03e4fa3a2a5 -r 20757784f5bf main.cpp
--- a/main.cpp	Wed Jan 13 09:15:12 2016 +0000
+++ b/main.cpp	Thu Jan 21 09:51:38 2016 +0000
@@ -12,12 +12,20 @@
 #include "MODSERIAL.h"
 #include "arm_math.h"
 #include "HIDScope.h"
-
+//factors proportional control
+#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
+//thresholds
 #define EMG_tresh1   0.015
 #define EMG_tresh2   0.015
 #define EMG_tresh3   0.015
 #define EMG_tresh4   0.015
 
+//button for control switching
+InterruptIn button1(PTC6);
+
 MODSERIAL pc(USBTX,USBRX);
 //joystick control
 AnalogIn X_control(A1);
@@ -146,6 +154,7 @@
 float hstep_freqy = 1;
 float emg_y_abs = 0;
 float emg_x_abs = 0;
+bool proportional = 1;
 
 void looper_emg()
 {
@@ -199,6 +208,32 @@
 
     emg_y = (filtered_biceps - filtered_triceps);
     
+    switch(proportional)
+    {
+        case 0:
+    //proportional control
+    emg_y_abs = fabs(emg_y);
+    force1 = emg_y_abs*K_Gain;
+    force1 = force1 - damping1;
+    acc1 = force1/Mass;
+    speed1 = speed_old1 + (acc1 * dt);
+    damping1 = speed1 * Damp;
+    step_freq1 = setpoint * speed1;
+    Stepy.period(1.0/step_freq1);
+    speed_old1 = speed1;
+    
+        //Speed limit
+    if (speed1 > 1) {
+        speed1 = 1;
+        step_freq1 = setpoint;
+    }
+        break;
+    case 1:
+    //precision control
+    Stepy.period(0.000625);//frequency of 1600 Hz
+    break;
+    default:
+    }
     if (emg_y > 0) {//downward movement
         Diry = 0;
     }
@@ -218,7 +253,35 @@
 {
 
     emg_x = (filtered_pect - filtered_deltoid);
-
+    switch(proportional)
+    {
+        case 0:
+        //proportional control
+        Ledr=0;
+        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;
+    
+        //speed limit
+        if (speed2 > 1) {
+            speed2 = 1;
+            step_freq2 = setpoint;
+        }   
+        break;
+    
+        case 1:
+        //precision control 
+        Ledr=1;
+        Stepx.period(0.000625);//frequency of 1600 Hz
+        break;
+        default:
+        }
     if (emg_x > 0) {//left movement
         Dirx = 0;
     }
@@ -234,6 +297,11 @@
     }
 }
 
+void changecontrol(){
+        proportional = !proportional;//code for changing speed control
+        }
+
+
 int main()
 {
     pc.baud(115200);
@@ -251,8 +319,8 @@
 Enabley.write(1);
 Stepx.write(0.5f);
 Enablex.write(1);
-Stepy.period(0.000625);//use period change for speed adjustments
-Stepx.period(0.000625);//frequency of 1600 Hz
+//Stepy.period(0.000625);//use period change for speed adjustments
+//Stepx.period(0.000625);//frequency of 1600 Hz
 
 
 /*//code for controlling the mechanism with a joystick
@@ -315,5 +383,7 @@
 
     looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor
 
-    while(1){};
+    while(1){
+        button1.fall(changecontrol);
+        };
 }
\ No newline at end of file