Diff: main.cpp

Revision:

40:0cfd96cb25fa

Parent:

39:191ae0d12bd6

Child:

41:a666a531d52e

diff -r 191ae0d12bd6 -r 0cfd96cb25fa main.cpp
--- a/main.cpp	Mon Jun 01 11:34:57 2015 +0000
+++ b/main.cpp	Wed Jun 03 13:10:34 2015 +0000
@@ -3,13 +3,13 @@
 #include "arm_math.h"
 #include "HIDScope.h"
 
-#define K_Gain      16      //Gain of the filtered EMG signal
-#define Damp        4       //Deceleration of the motor
+#define K_Gain      14      //Gain of the filtered EMG signal
+#define Damp        5       //Deceleration of the motor
 #define Mass        1       // Mass value
 #define dt          0.002   //Sample frequency
-#define MAX_bi      0.08      //Can be used for normalisation of the EMG signal of the biceps
-#define MAX_tri     0.06
-#define MIN_freq    900     //The motor turns off below this frequency
+#define MAX_bi      0.09    //Can be used for normalisation of the EMG signal of the biceps
+#define MAX_tri     0.09
+#define MIN_freq    500     //The motor turns off below this frequency
 
 //Motor control
 DigitalOut Dir(p21);
@@ -38,7 +38,7 @@
 C12832_LCD lcd;
 
 //Variables for motor control
-float setpoint = 9000; //Frequentie setpint
+float setpoint = 4400; //Frequentie setpint
 float step_freq = 1;
 
 //EMG filter
@@ -60,13 +60,11 @@
 //global variabels
 float filtered_biceps;
 float filtered_triceps;
-float speed_old1;
-float speed_old2;
+float speed_old;
 float acc;
 float force1;
 float force2;
-float speed1;
-float speed2; 
+float speed;
 float D;
 
 void looper_emg()
@@ -99,47 +97,38 @@
     
         if (filtered_biceps > filtered_triceps) {
             Dir = 0;
-            //force2 = 0;
-            speed2 = 0;
+            force2 = 0;
             force1 = K_Gain*(filtered_biceps/MAX_bi);
             force1 = force1 - D;
             acc = force1/Mass;
-            speed1 = speed_old1 + (acc * dt);
-            D = speed1 * Damp;
-            step_freq = (setpoint*speed1);
-            Step.period(1.0/step_freq);
-            speed_old1 = speed1;
+            speed = speed_old + (acc * dt);
+            D = speed * Damp;
+    
         } else {
             Dir = 1;
-            //force1 = 0;
-            speed1 = 0;
+            force1 = 0;
             force2 = K_Gain*(filtered_triceps/MAX_tri);
             force2 = force2 - D;
             acc = force2/Mass;
-            speed2 = speed_old2 + (acc * dt);
-            D = speed2 * Damp;
-            step_freq = (setpoint*speed2);
-            Step.period(1.0/step_freq);
-            speed_old2 = speed2;
-           
-            
+            speed = speed_old + (acc * dt);
+            D = speed * Damp;
         }
         //Speed limit
-        /*if (speed > 1) {
+        if (speed > 1) {
             speed = 1;
             step_freq = setpoint;
         } else {
             step_freq = (setpoint*speed);
         }
-        Step.period(1.0/step_freq);
-        speed_old = speed;*/
-        
+       
 
-        if (step_freq < MIN_freq) {
+        if (filtered_biceps < 0.02 && filtered_triceps < 0.02) {
             Enable = 1;
         } else {
             Enable = 0;
         }
+         Step.period(1.0/step_freq);
+         speed_old = speed;
     }
 
     int main() {
@@ -164,7 +153,7 @@
 
         while (1) {
 
-            lcd.printf("Freq %.0f Hz \n", step_freq); //snelheid meting op lcd
+            lcd.printf("Freq %.0f Hz, %.2f \n", step_freq, speed); //snelheid meting op lcd
             //pc.printf("%.3f \n", emg0.read());
             wait(0.01);
         }