Diff: main.cpp

Revision:

72:4d01b79ad332

Parent:

71:aee1289bc16a

Child:

73:8cc2826ab1c4

--- a/main.cpp	Fri Jun 19 23:28:40 2015 +0000
+++ b/main.cpp	Mon Jun 22 08:41:15 2015 +0000
@@ -47,8 +47,8 @@
 DigitalOut MS3(p29);
 
 //EMG inputs
-AnalogIn emg1(p15); 
-AnalogIn emg2(p16); 
+AnalogIn emg1(p15);
+AnalogIn emg2(p16);
 AnalogIn emg3(p17);
 AnalogIn emg4(p18);
 
@@ -90,11 +90,11 @@
 
 //global variabels
 float filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid;
-float speed_old1, speed_old2
-float acc1, acc2
-float force1, force2
-float speed1, speed2
-float damping1, damping2
+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;
@@ -142,7 +142,7 @@
     scope.set(3,filtered_deltoid);*/
 }
 
-void looper_motory()
+/*void looper_motor()
 {
 
     emg_y = (filtered_biceps - filtered_triceps);
@@ -175,7 +175,7 @@
         Enabley = 0;
     }
     wait(0.01);
-    
+
     emg_x = (filtered_pect - filtered_deltoid);
     emg_x_abs = fabs(emg_x);
     force2 = emg_x_abs*K_Gain;
@@ -209,7 +209,7 @@
 
 
 
-/*void looper_motorx()
+void looper_motorx()
 {
 
     emg_x = (filtered_pect - filtered_deltoid);
@@ -247,7 +247,7 @@
 {
     // 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;
@@ -264,13 +264,15 @@
         wait(1);
         Enablex = 0;
         Enabley = 0;
+
+        //Homing of the motor, so you start from the same position every time.
         while(errorx > error_tresh || errory > error_tresh) {
 
             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);
+            lcd.printf("%.2f %.2f \n", Stepx.read(), Stepy.read());
 
 
             if (Ps_x < 0.50 && errorx > error_tresh) {
@@ -325,7 +327,7 @@
         wait(1);
         Enablex = 0;
         Enabley = 0;
-    */
+    
     MS1 = 1;
     MS2 = 0;
     MS3 = 0;
@@ -346,11 +348,11 @@
     arm_biquad_cascade_df1_init_f32(&highnotch_deltoid, 2 , highnotch_const, highnotch_deltoid_states);
     emgtimer.attach(looper_emg, 0.01);
 
-   //Ticker looptimer1;
+    //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
+    //Ticker looptimer2;
+    //looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor
 
     //Microstepping control, now configured as half stepping (MS1=1,MS2=0,MS3=0)
 
@@ -358,10 +360,72 @@
 
     while (1) {
 
-    
+       
+        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);
+        damping1 = speed1 * Damp;
+        step_freq1 = setpoint * speed1;
+      
+        Stepy.period(1.0/step_freq1);
+        speed_old1 = speed1;
+
+        if (emg_y > 0) {
+            Diry = 1;
+        }
+
+        if (emg_y < 0) {
+            Diry = 0;
+        }
+        //Speed limit
+        if (speed1 > 1) {
+            speed1 = 1;
+            step_freq1 = setpoint;
+        }
+        //EMG treshold
+        if (filtered_biceps < EMG_tresh1 && filtered_triceps < EMG_tresh2) {
+            Enabley = 1; //Enable = 1 turns the motor off.
+        } 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);
+
         //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", Stepx.read(), step_freq1, Stepy.read(), step_freq2);
         wait(0.01);