Diff: main.cpp

Revision:

29:2b711fc9a5b2

Parent:

28:97b9e50c1180

Child:

30:457e42514c47

--- a/main.cpp	Fri Oct 28 07:56:12 2016 +0000
+++ b/main.cpp	Fri Oct 28 08:53:37 2016 +0000
@@ -11,7 +11,7 @@
 #include "BiQuad.h"
 
 // Definieren van de HIDscope ----------------------------------------
-HIDScope scope(5);               
+HIDScope scope(3);               
 
 
 // Definieren van de Serial en Encoder -------------------------------
@@ -109,16 +109,16 @@
 
 volatile bool flag1 = false, flag2 = false;   // De flags voor de functies aangeroepen door de tickers
 
-const double Kp_1 = 0.07;                    // De PID variablen voor motor 1
+const double Kp_1 = 0.1;                    // De PID variablen voor motor 1
 const double Kd_1 = 0.15;
-const double Ki_1 = 0.1;
+const double Ki_1 = 0.07;
 
 const double Kp_2 = 1.0;                    // De PID variablen voor motor 2
 const double Kd_2 = 0.4;
 const double Ki_2 = 0.1;
 
 const double vrijheid_m1_rad = 0.4*pi;      // Dit is de uiterste postitie vd arm in radialen
-const double vrijheid_m2_meter = 0.2;         // Dit is de helft vd arm, dit is wat het karretje heen en weer kan bewegen
+const double vrijheid_m2_meter = 0.25;         // Dit is de helft vd arm, dit is wat het karretje heen en weer kan bewegen
 
 const double initial_pos_m1 = vrijheid_m1_rad;                // Startin posities van de motoren
 const double initial_pos_m2 = vrijheid_m2_meter; 
@@ -262,10 +262,10 @@
             ref_pos = ref_pos_prev;
             break;
         case 1 :
-            ref_pos = ref_pos_prev + T_motor_function*vrijheid_rad;         // Dit dwingt af dat de ref pos veranderd met ong 0.5 rad/s
+            ref_pos = ref_pos_prev + T_motor_function*vrijheid_rad*0.1;         // Dit dwingt af dat de ref pos veranderd met ong 0.5 rad/s
             break;
         case -1 :
-            ref_pos = ref_pos_prev - T_motor_function*vrijheid_rad;
+            ref_pos = ref_pos_prev - T_motor_function*vrijheid_rad*0.1;
             break;
     }
     if(ref_pos >= vrijheid_rad){
@@ -318,7 +318,9 @@
 double PID_controller(double ref_pos, double mot_pos, double &e_prev, double &e_int, const double Kp, const double Kd, const double Ki){
     double error = ref_pos - mot_pos;       // error uitrekenen
     double error_dif = (error-e_prev)/T_motor_function;      // De error differentieren
-    error_dif = bqc5.step(error_dif);       // Filter biquad poep
+    //scope.set(error_dif,0); 
+    //error_dif = bqc5.step(error_dif);       // Filter biquad
+    //scope.set(error_dif,1); scope.send();
     e_prev = error;                                 // Hier wordt de error opgeslagen voor de volgende keer
     e_int = e_int + T_motor_function*error;             // De error integreren
     return Kp*error + Ki*e_int + Kd*error_dif;       // De uiteindelijke PID output