Revision 4:de0923cf6bcc, committed 2017-11-02

Comitter:

CasperBerkhout

Date:

Thu Nov 02 12:11:41 2017 +0000

Parent:

3:455b79d42636

Child:

5:224c1fe73a8f

Child:

6:9e8847a0492c

Commit message:

Better D action LPF added.

Changed in this revision

--- a/main.cpp	Thu Nov 02 11:01:24 2017 +0000
+++ b/main.cpp	Thu Nov 02 12:11:41 2017 +0000
@@ -27,7 +27,7 @@
 DigitalIn LimSW2(D8);
 DigitalIn HomStart(D3); // - left button
 
-BiQuad bqlowpass(0,    0.611,    0.611,  1,    0.222);
+BiQuad bqlowpass(0.4354, 0.8709, 0.4354, 0.5179, 0.2238);
 //create scope objects - note: script won't run when HID usb port is not connected
 //HIDScope scope(5); //set # of channels
 
@@ -53,8 +53,8 @@
 
 double M2_home; 
 double M2_error_pos = 0;
-float M2_Kp = 1;
-float M2_Ki = 1.5;
+float M2_Kp = 1.5;
+float M2_Ki = 0.5;
 float M2_Kd = 0;
 double M2_e_int=0;
 double M2_e_prior=0;
@@ -102,7 +102,7 @@
      M1_direction.write(1);
      }
     else{M1_speed.write(0);}
-    pc.printf("Motor1 set speed = %f \n\r",set_speed);
+    
  }
  
  void M2_control(){
@@ -116,6 +116,7 @@
      M2_direction.write(1);
      }
     else{M2_speed.write(0);}       
+    pc.printf("M2 integral = %f\n\r", M2_e_int);
   }
  
  void homing_system () {
@@ -128,11 +129,12 @@
     
        while(1){
             if (HomStart.read() == 0){
-                  wait(0.01);
-                  double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; 
-                  double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; 
                   setpoint += 0.003; //move setpoint 0.2 radian per second (at 100hz)
-                  
+                  pc.printf("Homing... \n\r");
+            }     
+            
+            double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; 
+            double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416;  
             if(LimSW1.read() == 0){
                   M1_error_pos = 1.5*setpoint - M1_rel_pos;
             }
@@ -141,8 +143,8 @@
             }
             M1_control();
             M2_control();
-            pc.printf("starting M1 \n\r");
-        }
+            
+        
         if(LimSW1.read() ==  1){
           M1_error_pos = 0;
           M1_speed.write(0);
@@ -161,9 +163,10 @@
                 wait(0.5);
                 M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
                 M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
-                break;
-                //while(1); //stop after homing.
+                //break;
+                while(1); //stop after homing.
         }
+        wait(0.01);
        }
 
 }