Diff: main.cpp

Revision:

5:1319d1e1a472

Parent:

4:9e18e041f7a0

Child:

6:075b5c7c6220

--- a/main.cpp	Thu Nov 02 13:37:30 2017 +0000
+++ b/main.cpp	Thu Nov 02 14:57:25 2017 +0000
@@ -308,6 +308,15 @@
 float          countzo;
 float          countzb;
 float          countzr;
+int reference_o;
+int position_o;
+int error_o;
+int reference_b;
+int position_b;
+int error_b;
+int reference_r;
+int position_r;
+int error_r;
 
 float          hcounto;
 float          dcounto;
@@ -352,12 +361,12 @@
 
 void MotorController1()
 {
-    int         reference_o = (int) (counto-hcounto);
-    int         position_o = Encoder1.getPulses();
+             reference_o = (int) (counto-hcounto);
+             position_o = Encoder1.getPulses();
     
-    int error_o = reference_o - position_o;
+             error_o = reference_o - position_o;
     
-    //pc.printf("Position_o = %i          reference_o=%i              Error_o=%i\n\r" ,position_o,reference_o,error_o);
+    pc.printf("Position_o = %i          reference_o=%i              Error_o=%i\n\r" ,position_o,reference_o,error_o);
     
     if (-20<error_o && error_o<20){
         motorValue1 = 0;
@@ -383,10 +392,10 @@
 void MotorController2()
 {
     
-    int reference_b = (int) (-(countb-hcountb));
-    int position_b = Encoder2.getPulses();
+     reference_b = (int) (-(countb-hcountb));
+     position_b = Encoder2.getPulses();
 
-    int error_b = reference_b - position_b;
+     error_b = reference_b - position_b;
     
     //pc.printf("Position_b = %i         reference_b=%i         Error_b=%i      " ,position_b,reference_b,error_b);
         
@@ -412,10 +421,10 @@
 
 void MotorController3()
 {
-    int reference_r = (int) (-(countr-hcountr));
-    int position_r = Encoder3.getPulses();
+     reference_r = (int) (-(countr-hcountr));
+     position_r = Encoder3.getPulses();
         
-    int error_r = reference_r - position_r;
+     error_r = reference_r - position_r;
     
     //pc.printf("Position_r = %i          reference_r=%i              Error_r=%i\n\r" ,position_r,reference_r,error_r);
     
@@ -535,11 +544,14 @@
     countzo=rotzo*4200;
     countzb=rotzb*4200;
     countzr=rotzr*4200;
-    counto=countzo+hcounto; //+ gemaakt van een min
-    countb=countzb+hcountb;
-    countr=countzr+hcountr;
+    counto=countzo+hcounto+reference_o; //+ gemaakt van een min
+    countb=countzb+hcountb+reference_b;
+    countr=countzr+hcountr+reference_r;
     
+    pc.printf("dLod=%.2f         dLbd=%.2f       dLrd=%.2f\n\r",dLod,dLbd,dLrd);
+    pc.printf("hcounto=%.2f         hcountb=%.2f       hcountr=%.2f\n\r",hcounto,hcountb,hcountr);
     pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr);       // hier moet komen te staan hoe het zakken gaat
+    pc.printf("Position_o = %i          reference_o=%i              Error_o=%i\n\r" ,position_o,reference_o,error_o);
     }
 }