Diff: controller.cpp

Revision:

12:8295c02d740f

Parent:

7:a80cb6b06320

Child:

14:551049a798a3

--- a/controller.cpp	Wed Nov 02 16:29:13 2016 +0000
+++ b/controller.cpp	Thu Nov 03 13:08:44 2016 +0000
@@ -6,17 +6,29 @@
 
 RobotController::RobotController():
         upperArmController(0.2, 0, 0),
-        lowerArmController(0.3, 0, 0),
+        lowerArmController(1.0, 0, 0),
+        upperXController(6, 0, 0),
+//        lowerXController(1.9, 0, 0),
+        lowerXController(1, 0, 0.01),
         debug(USBTX, USBRX),
         robot() {
-                
-    upperArmLengthReference = 0;
-    lowerArmLengthReference = 0;
+
+    // Set initial arm length as reference value
+    upperArmLengthReference = robot.getUpperArmLength();
+    lowerArmLengthReference = robot.getLowerArmLength();
     
+    // Set initial roller position as reference value
+    float x,y; // Can't pass volatiles by reference
+    getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
+    referenceX = x;
+    referenceY = y;
+    
+    // Set movement flags
     MOVE_BY_REFERENCE = false;
     PAINT_MOVE_UP = false;
     PAINT_MOVE_DOWN = false;
     
+    // Start ticker
     ticker.attach(this, &RobotController::doTick, tick);
     
 }
@@ -37,10 +49,51 @@
     }
     
     if (PAINT_MOVE_UP) {
+        float x,y;
+        getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
+        
+        float xError = referenceX - x;
+        float yError = topY - y;
+        
+        float upperOutput = upperXController.control(xError, tick);
+        float lowerOutput = lowerArmController.control(yError, tick);
+        
+        robot.setUpperArmVelocity(-1 * upperOutput);
+        robot.setLowerArmVelocity(lowerOutput);
         
     }
     
     if (PAINT_MOVE_DOWN) {
+        // Get current roller location and arm lengths
+        float x,y;
+        float upperArmLength = robot.getUpperArmLength();
+        float lowerArmLength = robot.getLowerArmLength();
+        getRollerPositionForArmLengths(upperArmLength, lowerArmLength, x, y);
+        
+        // Get controller output for leading arm (upper arm)
+        float yError = bottomY - y;
+        float upperOutput = -1 * upperArmController.control(yError, tick);
+        
+        // Predict influence on x position
+        float futureUpperArmLength = robot.getUpperArmLength() + robot.getUpperArmEncoderVelocity() * gearRadius * tick;
+        float futureX, futureY;
+        getRollerPositionForArmLengths(futureUpperArmLength, robot.getLowerArmLength(), futureX, futureY);
+        
+        // Calculate lower arm length that is needed to compensate for the leading arm
+        float upper,lower;
+        getArmLengthsForRollerPosition(referenceX, 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);
+        
+        robot.setUpperArmVelocity(upperOutput);
+        robot.setLowerArmVelocity(lowerOutput);
         
     }
 }
@@ -52,35 +105,71 @@
 
 // Set the reference lengths for the arms that correspond to this roller coordinate
 void RobotController::moveTo(float x, float y) {
+    clearFlags();
     float upper, lower;
     getArmLengthsForRollerPosition(x, y, upper, lower); // Cant pass volatiles by reference
     upperArmLengthReference = upper;
     lowerArmLengthReference = lower;
-    
-    PAINT_MOVE_UP = false;
-    PAINT_MOVE_DOWN = false;
+
     MOVE_BY_REFERENCE = true;
 }
 
 // Set the arm lengths
 void RobotController::setArmLengths(float upper, float lower) {
+    clearFlags();
+    
     upperArmLengthReference = upper;
     lowerArmLengthReference = lower;
     
-    PAINT_MOVE_UP = false;
-    PAINT_MOVE_DOWN = false;
     MOVE_BY_REFERENCE = true;
     
     debug.printf("Set arm length reference to: Upper %f, Lower %f\n\r", upper, lower);
 }
 
-
+// Make a painting movement upwards
+void RobotController::paintUp() {
+    clearFlags();
+    float x,y;
+    getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
+    referenceX = x;
+    referenceY = y;
 
-
+    PAINT_MOVE_UP = true;
+    
+}
 
-
+// Make a painting movement downwards
+void RobotController::paintDown() {
+    clearFlags();
+    float x,y;
+    getRollerPositionForArmLengths(robot.getUpperArmLength(), robot.getLowerArmLength(), x, y);
+    referenceX = x;
+    referenceY = y;
+    
+    PAINT_MOVE_DOWN = true;
+    
+}
 
+// Move to max y
+void RobotController::goToTop() {
+    float upper, lower;
+    getArmLengthsForRollerPosition(topX, topY, upper, lower);
+    setArmLengths(upper, lower);
+}
 
+// Move to min y
+void RobotController::goToBottom() {
+    float upper, lower;
+    getArmLengthsForRollerPosition(bottomX, bottomY, upper, lower);
+    setArmLengths(upper, lower);
+}
+
+// Set all movement flags to false
+void RobotController::clearFlags() {
+    MOVE_BY_REFERENCE = false;
+    PAINT_MOVE_UP = false;
+    PAINT_MOVE_DOWN = false;
+}
 
 /*
     Robot queries
@@ -98,13 +187,9 @@
     return robot.isKilled();
 }
 
-// Get robot pointer
+// Get the robot instance
 Robot* RobotController::getRobot() {
     return &robot;
 }
 
 
-
-
-
-