Revision 91:dc73a4b07653, committed 2015-06-25

Comitter:

jessekaiser

Date:

Thu Jun 25 15:06:59 2015 +0000

Parent:

90:399b877f8a77

Child:

92:28fe99803b9c

Commit message:

Y motor control + homing

Changed in this revision

--- a/main.cpp	Thu Jun 25 14:52:11 2015 +0000
+++ b/main.cpp	Thu Jun 25 15:06:59 2015 +0000
@@ -13,7 +13,7 @@
 //#include "HIDScope.h"
 
 #define P_Gain      0.99
-#define K_Gain      150   //Gain of the filtered EMG signal
+#define K_Gain      175   //Gain of the filtered EMG signal
 #define Damp        5    //Deceleration of the motor
 #define Mass        1    // Mass value
 #define dt          0.01 //Sample frequency
@@ -25,30 +25,30 @@
 #define EMG_tresh2   0.01
 #define EMG_tresh3   0.01
 #define EMG_tresh4   0.01
-#define H_Gain  8
-#define Pt_x    0.70
+#define H_Gain  5
+#define Pt_x    0.50
 #define Pt_y    0.50
 #define error_tresh 0.02
 
 //Motor control
-DigitalOut Dirx(p25);
-PwmOut Stepx(p26);
+DigitalOut Dirx(p21);
+PwmOut Stepx(p22);
 DigitalOut Diry(p23);
-PwmOut Stepy(p28);
+PwmOut Stepy(p24);
 
 //Signal to and from computer
 Serial pc(USBTX, USBRX);
 
 //Position sensors
-AnalogIn Posx(p20);
-AnalogIn Posy(p19);
-DigitalOut Enablex(p27);
-DigitalOut Enabley(p39);
+AnalogIn Posx(p19);
+AnalogIn Posy(p20);
+DigitalOut Enablex(p25);
+DigitalOut Enabley(p26);
 
 //Microstepping
-DigitalOut MS1(p29);
-DigitalOut MS2(p30);
-DigitalOut MS3(p31);
+DigitalOut MS1(p27);
+DigitalOut MS2(p28);
+DigitalOut MS3(p29);
 
 //EMG inputs
 AnalogIn emg1(p15); //EMG bordje bovenop, biceps
@@ -63,7 +63,7 @@
 C12832_LCD lcd;
 
 //Variables for motor control
-float setpoint = 1750; //Frequentie setpoint
+float setpoint = 2000; //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 = 1;
+        Dirx = 0;
     }
     if (emg_x < 0) {
-        Dirx = 0;
+        Dirx = 1;
     }
     //Speed limit
     if (speed2 > 1) {
@@ -213,7 +213,7 @@
         Enablex = 0;
     }
 
-}
+}*/
 
 int main()
 {
@@ -232,58 +232,53 @@
     wait(1);
     pc.printf("Start homing");
     wait(2);
-    //lcd.cls();
-    wait(1);
     Enablex = 0;
     Enabley = 0;
-    while(errorx > error_tresh) {
+    while(errory > error_tresh) {
 
         Ps_x = Posx.read();
         Ps_y = Posy.read();
         errorx = fabs(Pt_x - Ps_x);
         errory = fabs(Ps_y - Pt_y);
-       
-        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);
+        
+        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);
             wait(0.01);}
-            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);
+            else {
+            Stepy.period(1.0/setpoint);
             wait(0.01);
             }
         }
-         pc.printf("%.2f %.2f %.1f %.0f \n", errorx, Ps_x, cx, hstep_freqx);
+        
+        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("Done");
     wait(2);
     Enablex = 1;
     Enabley = 1;
-    wait(2);
+    wait(3);
     pc.printf("Start EMG Control");
     wait(2);
     Enablex = 0;
@@ -309,11 +304,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)
 
@@ -322,7 +317,7 @@
     while (1) {
 
 
-        pc.printf("%.2f %.2f %.2f  \n", Posx.read(), emg_x, step_freq2); //Send signal values to the computer.
+        pc.printf("%.2f %.2f %.2f  \n", Posy.read(), emg_y, step_freq1); //Send signal values to the computer.
         wait(0.01); 
 
     }