Diff: main.cpp

Revision:

6:c97264a28123

Parent:

5:21e846902e3e

Child:

7:9e965efc430e

--- a/main.cpp	Tue Oct 31 21:09:34 2017 +0000
+++ b/main.cpp	Tue Oct 31 21:37:31 2017 +0000
@@ -42,11 +42,12 @@
 
 // PID gegevens
 double pulses2rad;
+double rad2pulses;
 double position;
 double ref1;
 double ref2;
-double PD1;
-double PD2;
+double PI1;
+double PI2;
 double error1_1=0;
 double error1_2=0;
 double error2_1=0;
@@ -56,11 +57,11 @@
 double error_I_2=0;
 double error_I2_2=0;
 
-double Kp1;   // proportional coefficient motor 1
-double Ki1;   // integrating  coefficient motor 1
+double Kp1 = 0.0001;   // proportional coefficient motor 1
+double Ki1 = 0.0002;   // integrating  coefficient motor 1
 
-double Kp2;   // proportional coefficient motor 2
-double Ki2;   // integrating  coefficient motor 2
+double Kp2 = 0.00015;   // proportional coefficient motor 2
+double Ki2 = 0.0035;   // integrating  coefficient motor 2
 
 void wasd()
 {
@@ -122,56 +123,6 @@
     ref2 = q2*rad2pulses;
     }
 
-void controller_oud(double q1ref, double q2ref)
-    {
-        // error = qset
-        // referentie bepalen    
-        
-        error1 = q1ref - q1_pos;
-        error2 = q2ref - q2_pos;
-
-        //PD 
-        PD1 = Kp*(error1) + Kd*((error1m-error1)/ts) ; // PD sum; PD = proportianal + differential
-        error1m = error1;                              // waarde voor qset wordt opgeslagen (m = memory)
-        PD2 = Kp*(error2) + Kd*((error2m-error2)/ts) ;
-        error2m = error2;
-        
-        //Motor control
-        if (PD1<0 && PD2<0 )
-        {
-        motor1MagnitudePin = fabs(PD1);
-        motor1DirectionPin = 0;
-        motor2MagnitudePin = fabs(PD2);
-        motor2DirectionPin = 1;
-        }
-        else if (PD1>0 && PD2<0)
-        {
-        motor1MagnitudePin = fabs(PD1);
-        motor1DirectionPin = 1;
-        motor2MagnitudePin = fabs(PD2);
-        motor2DirectionPin = 1;
-        }
-        
-        else if (PD1<0 && PD2>0)
-        {
-        motor1MagnitudePin = fabs(PD1);
-        motor1DirectionPin = 0;
-        motor2MagnitudePin = fabs(PD2);
-        motor2DirectionPin = 0;
-        }
-
-        else if (PD1>0 && PD2>0)
-        {
-        motor1MagnitudePin = fabs(PD1);
-        motor1DirectionPin = 1;
-        motor2MagnitudePin = fabs(PD2);
-        motor2DirectionPin = 0;
-        }
-   
-        
-        
-    }
-
 void controller()
 {
     //PID 1
@@ -192,35 +143,42 @@
 error2_2   = error1_2; 
 error_I2_2 = error_I_2;
 
-    //Motor control 1
-    if (PI1<0 &&)
+    //Motor control
+    if (PI1<0 && PI2<0)
     {
     motor1DirectionPin = 0;
     motor1MagnitudePin = fabs(PI1);
-
-    }
-    else if (PI1>0)
-    {
-    motor1DirectionPin = 1;
-    motor1MagnitudePin = fabs(PI1);
-    }
-
-    //Motor control 2
-    if (PI2<0 )
-    {
     motor2DirectionPin = 0;
     motor2MagnitudePin = fabs(PI2);
 
     }
-    else if (PI2>0)
+    else if (PI1>0 && PI2<0)
+    {
+    motor1DirectionPin = 1;
+    motor1MagnitudePin = fabs(PI1);
+    motor2DirectionPin = 0;
+    motor2MagnitudePin = fabs(PI2);
+    }
+
+    else if (PI1<0 && PI2>0)
     {
+    motor1DirectionPin = 0;
+    motor1MagnitudePin = fabs(PI1);
+    motor2DirectionPin = 1;
+    motor2MagnitudePin = fabs(PI2);
+    }
+    
+    else if (PI1>0 && PI2>0)
+    {
+    motor1DirectionPin = 1;
+    motor1MagnitudePin = fabs(PI1);
     motor2DirectionPin = 1;
     motor2MagnitudePin = fabs(PI2);
     }
 }
 
 void aansturing()
-    {
+{
     // encoders uitlezen    
     q1_puls = q1_enc.getPulses();
     q1_pos = q1_puls*pulses2rad;       // berekent positie q1 in radialen
@@ -231,8 +189,8 @@
      kinematics();
                  
      // PD controller       gebruikt PD control en stuurt motor aan
-     controller(q1, q2);    
-    }
+     controller();    
+}
 
 
 int main()
@@ -240,14 +198,12 @@
     pc.baud(115200);
     pulses2rad=(2*pi)/4200;
     rad2pulses=4200/(2*pi);
+    tick_wasd.attach_us(&wasd, 1000);
     tick_sample.attach_us(&aansturing, 1000); //sample frequency 1000 Hz;
-    tick_wasd.attach(&wasd, 0.1);
     led_g = 0; 
      
     while(true)
     {
 
     }
-    
-
 }
\ No newline at end of file