Diff: main.cpp

Revision:

4:6d88a281192f

Parent:

3:eb5b57162e38

Child:

5:cd1c63ffdc1a

--- a/main.cpp	Fri Oct 23 07:36:45 2015 +0000
+++ b/main.cpp	Fri Oct 23 07:49:51 2015 +0000
@@ -28,13 +28,15 @@
 DigitalOut ledY(D3);
 HIDScope scope(2);
 
-const double motor1_Kp = 1;                   //4200=1 rondje
 double m1_Kp = 0.0005, m1_Ki = 0.0, m1_Kd = 0.0;
 const double m1_Ts = 0.01;
 double m1_err_int = 0, m1_prev_err = 0;
 double m1_u_prev = 0;
 
-const int grens = 50;
+double m2_Kp = 0.0005, m2_Ki = 0.0, m2_Kd = 0.0;
+const double m2_Ts = 0.01;
+double m2_err_int = 0, m2_prev_err = 0;
+double m2_u_prev = 0;
 
 /*
 Gain = 0.002356
@@ -44,7 +46,11 @@
 
 const double m1_f_a1 = -1.94042975320, m1_f_a2 = 0.94215346226, m1_f_b0 = 1.0*0.000431, m1_f_b1 = 2.0*0.000431, m1_f_b2 = 1.0*0.000431; 
                                     //dit zijn de constanten die bij de filter horen (de a0 t/m b2) a0 is altijd 1
-double m1_f_v1 = 0, m1_f_v2 = 0;    //de variabelen van de filter 
+double m1_f_v1 = 0, m1_f_v2 = 0;    //de variabelen van de filter
+
+const double m2_f_a1 = -1.94042975320, m2_f_a2 = 0.94215346226, m2_f_b0 = 1.0*0.000431, m2_f_b1 = 2.0*0.000431, m2_f_b2 = 1.0*0.000431; 
+                                    //dit zijn de constanten die bij de filter horen (de a0 t/m b2) a0 is altijd 1
+double m2_f_v1 = 0, m2_f_v2 = 0;    //de variabelen van de filter 
 
 int reference1;
 int reference2;
@@ -161,7 +167,28 @@
     }
     motor1speed = fabs(u);
     pc.printf("u = %.2f \r\n", u); 
+}
+
+void motor2_PID_Controller()
+{
+    //reference2 = potmeter2.read()*4200;                     //draaiknop uitlezen, tussen 0 en 1
+    float hoek2 = bereken_hoek2(countsX,countsY);
+    reference2=hoek2;
+    double position =(encoder2.getPosition());              // waarde tussen 0 en 4200
     
+    scope.set(2,reference2);
+    scope.set(3,position);
+    
+    difference2 = reference2 - position;
+    constanteBepalen();
+    double u = PID( reference2 - position, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int,m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2 );
+    if (u < 0) {
+        motor2direction = 1;                            //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
+    } else if (u>=0) {
+        motor2direction = 0;
+    }
+    motor2speed = fabs(u);
+    pc.printf("u = %.2f \r\n", u); 
 }
 
 //-------------------------- POSITIEBEPALING --------------------------//