Diff: main.cpp

Revision:

7:435f984781ab

Parent:

6:e0857842e8cd

Child:

8:919ddba2875e

--- a/main.cpp	Mon Oct 17 06:58:57 2016 +0000
+++ b/main.cpp	Mon Oct 17 07:51:26 2016 +0000
@@ -10,7 +10,7 @@
 #include "HIDScope.h"
 
 // Definieren van de HIDscope ----------------------------------------
-HIDScope scope(1);               
+HIDScope scope(3);               
 
 
 // Definieren van de Serial en Encoder -------------------------------
@@ -64,8 +64,8 @@
 volatile bool flag1 = false, flag2 = false;   // De flags voor de functies aangeroepen door de tickers
 
 const double Kp_1 = 1.0000000;                    // De PID variablen voor motor 1
-const double Kd_1 = 1.0000000;
-const double Ki_1 = 1.0000000;
+const double Kd_1 = 0;
+const double Ki_1 = 0;
 
 const double vrijheid_m1_rad = 0.5*pi;      // Dit is de uiterste postitie vd arm in radialen
 
@@ -118,8 +118,10 @@
    
 
 // HIDScope functie ----------------------------------------------------------------------
-void set_scope(double input){
-    scope.set(0, input); 
+void set_scope(double input1, double input2, double input3){
+    scope.set(0, input1); 
+    scope.set(1, input2);
+    scope.set(2, input3);
     scope.send();
 }
 
@@ -187,11 +189,12 @@
         if (flag1){
             flag1 = false;
             PID_output = PID_controller(get_reference_position_m1(vrijheid_m1_rad), get_position_m1(rad_per_count), error_prev, error_int, Kp_1, Kd_1, Ki_1);
-            //pc.printf("%f\r\n",get_reference_position_m1);
+            set_motor1(PID_output);
+            //pc.printf("%f\r\n",error);
         }
         if (flag2){
             flag2 = false;
-            set_scope(PID_output);
+            set_scope(PID_output, get_reference_position_m1(vrijheid_m1_rad), get_position_m1(rad_per_count));
         }
     }