Revision 57:0a278c60d28b, committed 2015-06-15

Comitter:

jessekaiser

Date:

Mon Jun 15 10:37:57 2015 +0000

Parent:

56:6ea03cce1175

Child:

58:3ea066215c31

Commit message:

Eindconcept 6.;

Changed in this revision

--- a/main.cpp	Thu Jun 11 10:07:53 2015 +0000
+++ b/main.cpp	Mon Jun 15 10:37:57 2015 +0000
@@ -10,18 +10,20 @@
 #include "mbed.h"
 #include "C12832_lcd.h"
 #include "arm_math.h"
-#include "HIDScope.h"
+//#include "HIDScope.h"
 
-#define K_Gain      14      //Gain of the filtered EMG signal
+#define K_Gain      60    //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.04    //Can be used for normalisation of the EMG signal of the biceps
-#define MAX_tri     0.04
-#define MAX_pect    0.04
-#define MAX_delt    0.04
-#define MIN_freq    500     //The motor turns off below this frequency
-#define EMG_tresh   0.01
+#define dt          0.10   //Sample frequency
+#define MAX_bi      0.09 //Can be used for normalisation of the EMG signal of the biceps
+#define MAX_tri     0.10
+#define MAX_pect    0.10
+#define MAX_delt    0.05
+#define EMG_tresh1   0.03
+#define EMG_tresh2   0.03
+#define EMG_tresh3   0.02
+#define EMG_tresh4   0.03
 
 //Motor control
 DigitalOut Dirx(p21);
@@ -35,30 +37,28 @@
 //Position sensors
 AnalogIn Posx(p19);
 AnalogIn Posy(p20); 
-DigitalOut Enablex(p25); //Connected to green led
-DigitalOut Enabley(p26); //Connected to blue led
+DigitalOut Enablex(p25); 
+DigitalOut Enabley(p26); 
 
 //Microstepping
 DigitalOut MS1(p27);
 DigitalOut MS2(p28);
 DigitalOut MS3(p29);
 
-//Potmeter and EMG
-
-
+//EMG inputs
 AnalogIn emg1(p15); //EMG bordje bovenop, biceps
 AnalogIn emg2(p16); //triceps
 AnalogIn emg3(p17);
 AnalogIn emg4(p18);
 
-HIDScope scope(4);
-Ticker   scopeTimer;
+//HIDScope scope(4);
+//Ticker   scopeTimer;
 
 //lcd
 C12832_LCD lcd;
 
 //Variables for motor control
-float setpoint = 1000; //Frequentie setpint
+float setpoint = 1000; //Frequentie setpoint
 float step_freq1 = 1;
 float step_freq2 = 1;
 float step_freq3 = 1;
@@ -90,6 +90,8 @@
 
 //global variabels
 float filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid;
+float norm_biceps, norm_triceps, norm_pect, norm_deltoid;
+float gain_biceps, gain_triceps, gain_pect, gain_deltoid;
 float speed_old1, speed_old2, speed_old3, speed_old4;
 float acc1, acc2, acc3, acc4;
 float force1, force2, force3, force4;
@@ -124,17 +126,19 @@
     filtered_deltoid = fabs(filtered_deltoid);
     arm_biquad_cascade_df1_f32(&lowpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
 
-    /*send value to PC. */
+    /*send value to PC. 
     scope.set(0,filtered_biceps); //Filtered EMG signal
     scope.set(1,filtered_triceps);
     scope.set(2,filtered_pect);
-    scope.set(3,filtered_deltoid);
+    scope.set(3,filtered_deltoid);*/
 }
 
 void looper_motory()
 {
     //Forward
-    force1 = K_Gain*(filtered_biceps/MAX_bi);
+    norm_biceps = (filtered_biceps/MAX_bi);
+    gain_biceps = K_Gain*norm_biceps;
+    force1 = gain_biceps;
     force1 = force1 - damping1;
     acc1 = force1/Mass;
     speed1 = speed_old1 + (acc1 * dt);
@@ -143,20 +147,23 @@
     speed_old1 = speed1;
 
     //Achteruit triceps
-    force2 = K_Gain*(filtered_triceps/MAX_tri);
+    norm_triceps = (filtered_triceps/MAX_tri); 
+    gain_triceps = K_Gain*norm_triceps;
+    force2 = gain_triceps;
     force2 = force2 - damping2;
     acc2 = force2/Mass;
     speed2 = speed_old2 + (acc2 * dt);
     damping2 = speed2 * Damp;
     step_freq2 = (setpoint*speed2);
     speed_old2 = speed2;
-    if (force1 > force2) {
+    
+    if (filtered_biceps > filtered_triceps || Posy < 0.20) {
         Diry = 1;
         speed2 = 0.01;
         speed_old2 = 0.01;
         Stepy.period(1.0/step_freq1);
     }
-    if (force2 > force1) {
+    if (filtered_triceps > filtered_biceps || Posy > 0.70) {
         Diry = 0;
         speed1 = 0.01;
         speed_old1 = 0.01;
@@ -172,7 +179,7 @@
         step_freq2 = setpoint;
     }
     //EMG treshold
-    if (filtered_biceps < EMG_tresh && filtered_triceps < EMG_tresh) {
+    if (filtered_biceps < EMG_tresh1 && filtered_triceps < EMG_tresh2) {
         Enabley = 1; //Enable = 1 turns the motor off.
         speed1 = 0.01;
         speed_old1 = 0.01;
@@ -187,7 +194,9 @@
 void looper_motorx()
 {
     //To the left
-    force3 = K_Gain*(filtered_pect/MAX_pect);
+    norm_pect = (filtered_pect/MAX_pect);
+    gain_pect = K_Gain*norm_pect;
+    force3 = gain_pect;
     force3 = force3 - damping3;
     acc3 = force3/Mass;
     speed3 = speed_old3 + (acc3 * dt);
@@ -196,7 +205,9 @@
     speed_old3 = speed3;
 
     //To the right
-    force4 = K_Gain*(filtered_deltoid/MAX_delt);
+    norm_deltoid = (filtered_deltoid/MAX_delt);
+    gain_deltoid = K_Gain*norm_deltoid;
+    force4 = gain_deltoid;
     force4 = force4 - damping4;
     acc4 = force4/Mass;
     speed4 = speed_old4 + (acc4 * dt);
@@ -204,13 +215,13 @@
     step_freq4 = (setpoint*speed4);
     speed_old4 = speed4;
     
-    if (force3 > force4) {
+    if (filtered_pect > filtered_deltoid || Posx < 0.30) {
         Dirx = 0;
         speed4 = 0.01;
         speed_old4 = 0.01;
         Stepx.period(1.0/step_freq3);
     }
-    if (force4 > force3) {
+    if (filtered_deltoid > filtered_pect || Posx > 0.86) {
         Dirx = 1;
         speed3 = 0.01;
         speed_old3 = 0.01;
@@ -226,7 +237,7 @@
         step_freq4 = setpoint;
     }
     //EMG treshold
-    if (filtered_pect < EMG_tresh && filtered_deltoid < EMG_tresh) {
+    if (filtered_pect < EMG_tresh3 && filtered_deltoid < EMG_tresh4) {
         Enablex = 1; //Enable = 1 turns the motor off.
         speed3 = 0.01;
         speed_old3 = 0.01;
@@ -240,7 +251,7 @@
 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);
+   // scopeTimer.attach_us(&scope, &HIDScope::send, 2e3);
 
     Ticker emgtimer;    //biceps
     arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1 , lowpass_const, lowpass_biceps_states);
@@ -268,12 +279,19 @@
     MS3 = 0;
     Stepx.write(0.5); // Duty cycle of 50%
     Stepy.write(0.5);
+    
+    //if (Posx < 0.50) {
+      //  Stepx.period(1.0/3000); //if the speed and distance is known. The motor can be brought to a home position.
+        //Need to know rpm to m/s.  
+        
 
     while (1) {
         
-        lcd.printf("x %.2f, y %.2f \n", Posx.read(), Posy.read());
-        //lcd.printf("%.2f, %.2f \n", filtered_biceps, filtered_triceps); //Filtered EMG values
+        //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, 3 %.0f, 4 %.0f \n", step_freq1, step_freq2, step_freq3, step_freq4); //step_freq value of every EMG sensor
-        wait(0.01);
+        //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);
     }
 }