Revision 88:c9146d957102, committed 2015-06-25

Comitter:

jessekaiser

Date:

Thu Jun 25 10:34:00 2015 +0000

Parent:

87:97015f6cb9cc

Child:

89:7882984abbe8

Commit message:

Y-motor works1

Changed in this revision

--- a/main.cpp	Thu Jun 25 10:10:35 2015 +0000
+++ b/main.cpp	Thu Jun 25 10:34:00 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
@@ -31,24 +31,24 @@
 #define error_tresh 0.03
 
 //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
@@ -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);
@@ -213,13 +213,13 @@
         Enablex = 0;
     }
 
-}
+}*/
 
 int main()
 {
-  /*  // Attach the HIDScope::send method from the scope object to the timer at 500Hz. Hier wordt de sample freq aangegeven.
+    // 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;
@@ -230,22 +230,45 @@
     Enablex = 1;
     Enabley = 1;
     wait(1);
-    pc.printf("Start homing");
+    lcd.printf("Start homing");
     wait(2);
-    //lcd.cls();
+    lcd.cls();
     wait(1);
     Enablex = 0;
     Enabley = 0;
-    while(errorx > error_tresh) {
+    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);
+        
         
-   
-        if (Ps_x < Pt_x && errorx > error_tresh) {
+        if (Ps_x < 0.50 && 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;
+            Stepx.period(1.0/hstep_freqx);
+            wait(0.01);
+        }
+        if (Ps_y > 0.50 && errory > error_tresh) {
+            Diry = 0;
+            //errory = Ps_y - Pt_y;
+            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;
+            Stepy.period(1.0/hstep_freqy);
+            wait(0.01);
+        }
+
+        if (Ps_x > 0.50 && errorx > error_tresh) {
             Dirx = 1;
             //errorx = Pt_x - Ps_x;
             cx = errorx * H_Gain;
@@ -256,19 +279,18 @@
             Stepx.period(1.0/hstep_freqx);
             wait(0.01);
         }
-       
-        if (Ps_x > Pt_x && errorx > error_tresh) {
-            Dirx = 0;
-            //errorx = Pt_x - Ps_x;
-            cx = errorx * H_Gain;
+        if (Ps_y < 0.50 && errory > error_tresh) {
+            Diry = 1;
+            //errory = Ps_y - Pt_y;
+            cy = errory * H_Gain;
 
-            float hnew_step_freqx;
-            hnew_step_freqx = ((1-P_Gain)*setpoint*cx) + (P_Gain*hstep_freqx);
-            hstep_freqx = hnew_step_freqx;
-            Stepx.period(1.0/hstep_freqx);
+            float hnew_step_freqy;
+            hnew_step_freqy = ((1-P_Gain)*setpoint*cy) + (P_Gain*hstep_freqy);
+            hstep_freqy = hnew_step_freqy;
+            Stepy.period(1.0/hstep_freqy);
             wait(0.01);
         }
-         pc.printf("%.2f %.2f %.0f %.0f\n", errorx, Ps_x, Dirx, hstep_freqx);
+
     }
     lcd.printf("Done");
     wait(2);
@@ -304,11 +326,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 +339,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); 
 
     }