Diff: main.cpp

Revision:

9:3874b23bb233

Parent:

8:f5065cd04213

Child:

10:952377fbbbfe

--- a/main.cpp	Tue Oct 31 15:48:43 2017 +0000
+++ b/main.cpp	Tue Oct 31 16:24:35 2017 +0000
@@ -506,9 +506,10 @@
 {
     double reference_o = counto-hcounto;
     double position_o = Encoder1.getPulses();
+    
     motorValue1 = P1(reference_o - position_o, kpo);
-    if (motorValue1 >=0) motor1DirectionPin=1;
-        else motor1DirectionPin=0;
+    if (motorValue1 >=0) motor1DirectionPin=0;
+        else motor1DirectionPin=1;
     if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
         else motor1MagnitudePin = fabs(motorValue1);
     }
@@ -552,9 +553,12 @@
 
 void MotorController2()
 {
+    
     double reference_b = countb-hcountb;
     double position_b = Encoder2.getPulses();
+
     motorValue2 = P2(reference_b - position_b, kpb);
+    
     if (motorValue2 >=0) motor2DirectionPin=1;
         else motor2DirectionPin=0;
     if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
@@ -600,9 +604,13 @@
  return errorr*kpr;
  }
 
-void MotorController3(){
+void MotorController3()
+{
     double reference_r = countr-hcountr;
     double position_r = Encoder3.getPulses();
+    pc.printf("reference_r = %0.2f", reference_r);
+    pc.printf("position_r = %0.2f", position_r);
+    
     motorValue3 = P3(reference_r - position_r, kpr);
     if (motorValue3 >=0) motor3DirectionPin=1;
         else motor3DirectionPin=0;