Diff: controller.cpp

Revision:

18:1c9dc6caab9d

Parent:

14:551049a798a3

Child:

25:bdb854127c11

Child:

27:91dc5174333a

--- a/controller.cpp	Thu Nov 03 13:15:00 2016 +0000
+++ b/controller.cpp	Thu Nov 03 16:29:31 2016 +0000
@@ -7,9 +7,8 @@
 RobotController::RobotController():
         upperArmController(0.2, 0, 0),
         lowerArmController(1.0, 0, 0),
-        upperXController(6, 0, 0),
-//        lowerXController(1.9, 0, 0),
-        lowerXController(0.8, 0, 0.01),
+        upperXController(1.2, 0, 0.03),
+        lowerXController(0.6, 0, 0.03),
         debug(USBTX, USBRX),
         robot() {
 
@@ -49,16 +48,35 @@
     }
     
     if (PAINT_MOVE_UP) {
+        // Get current roller location and arm lengths
         float x,y;
-        getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
+        float upperArmLength = robot.getUpperArmLength();
+        float lowerArmLength = robot.getLowerArmLength();
+        getRollerPositionForArmLengths(upperArmLength, lowerArmLength, x, y);
         
-        float xError = referenceX - x;
+        // Get controller output for leading arm (lower arm)
         float yError = topY - y;
-        
-        float upperOutput = upperXController.control(xError, tick);
         float lowerOutput = lowerArmController.control(yError, tick);
         
-        robot.setUpperArmVelocity(-1 * upperOutput);
+        // Predict influence on x position
+        float futureLowerArmLength = lowerArmLength + robot.getLowerArmEncoderVelocity() * gearRadius * tick;
+        float futureX, futureY;
+        getRollerPositionForArmLengths(upperArmLength, futureLowerArmLength, futureX, futureY);
+        
+        // Calculate upper arm length that is needed to compensate for the leading arm
+        float upper,lower;
+        getArmLengthsForRollerPosition(topX, futureY, upper, lower);
+        
+        // Calculate compensating arm velocity based on error
+        float compensatingArmError = upper - upperArmLength;
+        debug.printf("Upper Arm error: %f\n\r", compensatingArmError);
+        
+        float upperOutput = upperXController.control(compensatingArmError, tick);
+        
+        debug.printf("Resulting reference velocity: %f\n\r", upperOutput);
+        
+        // Instruct arm motors
+        robot.setUpperArmVelocity(upperOutput);
         robot.setLowerArmVelocity(lowerOutput);
         
     }
@@ -75,23 +93,19 @@
         float upperOutput = -1 * upperArmController.control(yError, tick);
         
         // Predict influence on x position
-        float futureUpperArmLength = robot.getUpperArmLength() + robot.getUpperArmEncoderVelocity() * gearRadius * tick;
+        float futureUpperArmLength = upperArmLength + robot.getUpperArmEncoderVelocity() * gearRadius * tick;
         float futureX, futureY;
-        getRollerPositionForArmLengths(futureUpperArmLength, robot.getLowerArmLength(), futureX, futureY);
+        getRollerPositionForArmLengths(futureUpperArmLength, lowerArmLength, futureX, futureY);
         
         // Calculate lower arm length that is needed to compensate for the leading arm
         float upper,lower;
-        getArmLengthsForRollerPosition(referenceX, futureY, upper, lower);
+        getArmLengthsForRollerPosition(bottomX, futureY, upper, lower);
         
         // Calculate compensating arm velocity based on error
         float compensatingArmError = lower - lowerArmLength;
         float lowerOutput = lowerXController.control(compensatingArmError, tick);
         
-        
-//        float xError = referenceX - futureX;
-        
-//        float lowerOutput = -1 * lowerXController.control(xError, tick);
-        
+        // Instruct arm motors
         robot.setUpperArmVelocity(upperOutput);
         robot.setLowerArmVelocity(lowerOutput);