Diff: main.cpp

Revision:

19:6567ed67d6ee

Parent:

18:00379f0ecc7f

Child:

20:bde79d7a4091

--- a/main.cpp	Wed Nov 01 12:59:02 2017 +0000
+++ b/main.cpp	Wed Nov 01 13:59:04 2017 +0000
@@ -345,6 +345,8 @@
 
 float          referenceVelocity3; 
 float          motorValue3; 
+
+
  
 Ticker          controlmotor1; // één ticker van maken?
 Ticker          controlmotor2; // één ticker van maken?
@@ -362,15 +364,17 @@
     
     int 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 (-10<error_o && error_o<10){
+    if (-100<error_o && error_o<100){
         motorValue1 = 0;
         }
     else {
     motorValue1 = 0.05*P1(error_o, kpo);
     }    
     
+    
+    
     if (motorValue1 >=0) motor1DirectionPin=0;
         else motor1DirectionPin=1;
     if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
@@ -393,7 +397,7 @@
     
     //pc.printf("Position_b = %i         reference_b=%i         Error_b=%i      " ,position_b,reference_b,error_b);
         
-    if (-10<error_b && error_b<10){
+    if (-100<error_b && error_b<100){
         motorValue2 = 0;
         }
     else {
@@ -420,10 +424,10 @@
         
     int 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);
+    pc.printf("Position_r = %i          reference_r=%i              Error_r=%i\n\r" ,position_r,reference_r,error_r);
     
     
-    if (-10<error_r && error_r<10){
+    if (-100<error_r && error_r<100){
         motorValue3 = 0;
         
         }
@@ -546,12 +550,12 @@
 }
 
 void tiller(){
-    dcountb = -8148;
-    dcounto = -12487;
-    dcountr = -7386;
+    int reference_o = hcounto-12487;
+    int reference_b = hcountb-8148;
+    int reference_r = hcountr-7386;
     pc.printf("Tiller");
-    Vex = 20;
-    Vey = 20;
+/*    Vex = 20;
+    Vey = 20;*/
     controlmotor1.attach(&MotorController1, 0.01);
     controlmotor2.attach(&MotorController2, 0.01);
     controlmotor3.attach(&MotorController3, 0.01);
@@ -572,7 +576,7 @@
 {
     pc.baud(115200);
     wait(1.0f);
-    //tiller();
+    tiller();
     getbqChain();
     threshold_timerR.attach(&Threshold_samplingBR, 0.002);
     threshold_timerL.attach(&Threshold_samplingBL, 0.002);