Revision 92:28fe99803b9c, committed 2015-11-03

Comitter:

jessekaiser

Date:

Tue Nov 03 18:59:28 2015 +0000

Parent:

91:dc73a4b07653

Commit message:

The EMG control for the XY Table in 4 directions. Was made during my Bachelor end assignment.;

Changed in this revision

--- a/main.cpp	Thu Jun 25 15:06:59 2015 +0000
+++ b/main.cpp	Tue Nov 03 18:59:28 2015 +0000
@@ -13,7 +13,7 @@
 //#include "HIDScope.h"
 
 #define P_Gain      0.99
-#define K_Gain      175   //Gain of the filtered EMG signal
+#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
@@ -26,29 +26,29 @@
 #define EMG_tresh3   0.01
 #define EMG_tresh4   0.01
 #define H_Gain  5
-#define Pt_x    0.50
+#define Pt_x    0.80
 #define Pt_y    0.50
 #define error_tresh 0.02
 
 //Motor control
-DigitalOut Dirx(p21);
-PwmOut Stepx(p22);
+DigitalOut Dirx(p25);
+PwmOut Stepx(p26);
 DigitalOut Diry(p23);
-PwmOut Stepy(p24);
+PwmOut Stepy(p28);
 
 //Signal to and from computer
 Serial pc(USBTX, USBRX);
 
 //Position sensors
-AnalogIn Posx(p19);
-AnalogIn Posy(p20);
-DigitalOut Enablex(p25);
-DigitalOut Enabley(p26);
+AnalogIn Posx(p20);
+AnalogIn Posy(p19);
+DigitalOut Enablex(p27);
+DigitalOut Enabley(p39);
 
 //Microstepping
-DigitalOut MS1(p27);
-DigitalOut MS2(p28);
-DigitalOut MS3(p29);
+DigitalOut MS1(p29);
+DigitalOut MS2(p30);
+DigitalOut MS3(p31);
 
 //EMG inputs
 AnalogIn emg1(p15); //EMG bordje bovenop, biceps
@@ -63,7 +63,7 @@
 C12832_LCD lcd;
 
 //Variables for motor control
-float setpoint = 2000; //Frequentie setpoint
+float setpoint = 1750; //Frequentie setpoint
 float step_freq1 = 1;
 float step_freq2 = 1;
 
@@ -146,7 +146,7 @@
     scope.set(3,filtered_deltoid);*/
 }
 
-void looper_motory()
+/*void looper_motory()
 {
 
     emg_y = (filtered_biceps - filtered_triceps);
@@ -179,9 +179,9 @@
         Enabley = 0;
     }
 
-}
+}*/
 
-/*void looper_motorx()
+void looper_motorx()
 {
 
     emg_x = (filtered_pect - filtered_deltoid);
@@ -196,10 +196,10 @@
     speed_old2 = speed2;
 
     if (emg_x > 0) {
-        Dirx = 0;
+        Dirx = 1;
     }
     if (emg_x < 0) {
-        Dirx = 1;
+        Dirx = 0;
     }
     //Speed limit
     if (speed2 > 1) {
@@ -213,7 +213,7 @@
         Enablex = 0;
     }
 
-}*/
+}
 
 int main()
 {
@@ -232,53 +232,58 @@
     wait(1);
     pc.printf("Start homing");
     wait(2);
+    //lcd.cls();
+    wait(1);
     Enablex = 0;
     Enabley = 0;
-    while(errory > error_tresh) {
+    while(errorx > error_tresh) {
 
         Ps_x = Posx.read();
         Ps_y = Posy.read();
         errorx = fabs(Pt_x - Ps_x);
         errory = fabs(Ps_y - Pt_y);
-        
-        pc.printf("%.2f %.2f %.2f \n", errory, Ps_y, hstep_freqy);
-        
-        if (Ps_y > Pt_y && 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;
-            if(hstep_freqy < 2000){
-            Stepy.period(1.0/hstep_freqy);
+       
+        if (Ps_x < Pt_x && errorx > error_tresh) {
+            Dirx = 0;
+            //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;
+            if(hstep_freqx < 2000){
+            Stepx.period(1.0/hstep_freqx);
             wait(0.01);}
-            else {
-            Stepy.period(1.0/setpoint);
+            else{
+            Stepx.period(1.0/setpoint);
+            wait(0.01);
+            }
+            
+        }
+       
+        if (Ps_x > Pt_x && 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;
+            if(hstep_freqx < 2000){
+            Stepx.period(1.0/hstep_freqx);
+            wait(0.01);}
+            else{
+            Stepx.period(1.0/setpoint);
             wait(0.01);
             }
         }
-        
-        if (Ps_y < Pt_y && 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;
-            if(hstep_freqy < setpoint){
-            Stepy.period(1.0/hstep_freqy);
-            wait(0.01);}
-            else {
-            Stepy.period(1.0/setpoint);
-            wait(0.01);
-            }
-        }
-
+         pc.printf("%.2f %.2f %.1f %.0f \n", errorx, Ps_x, cx, hstep_freqx);
     }
     pc.printf("Done");
     wait(2);
     Enablex = 1;
     Enabley = 1;
-    wait(3);
+    wait(2);
     pc.printf("Start EMG Control");
     wait(2);
     Enablex = 0;
@@ -304,11 +309,11 @@
     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
+    //Ticker looptimer2;
+    //looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor
 
     //Microstepping control, now configured as half stepping (MS1=1,MS2=0,MS3=0)
 
@@ -317,7 +322,7 @@
     while (1) {
 
 
-        pc.printf("%.2f %.2f %.2f  \n", Posy.read(), emg_y, step_freq1); //Send signal values to the computer.
+        pc.printf("%.2f %.2f %.2f  \n", Posx.read(), emg_x, step_freq2); //Send signal values to the computer.
         wait(0.01); 
 
     }