Diff: controller.cpp

Revision:

7:a80cb6b06320

Parent:

2:fc869e45e672

Child:

12:8295c02d740f

--- a/controller.cpp	Wed Nov 02 09:26:38 2016 +0000
+++ b/controller.cpp	Wed Nov 02 16:29:13 2016 +0000
@@ -1,29 +1,110 @@
 #include "controller.h"
 
-bool constantMovementTo(Arm arm, float length) {
-    const float velocity = 2;
-    float initialLength = arm.getLength();
-    float dLength = length-initialLength;
+/*
+    Constructor
+*/
+
+RobotController::RobotController():
+        upperArmController(0.2, 0, 0),
+        lowerArmController(0.3, 0, 0),
+        debug(USBTX, USBRX),
+        robot() {
+                
+    upperArmLengthReference = 0;
+    lowerArmLengthReference = 0;
+    
+    MOVE_BY_REFERENCE = false;
+    PAINT_MOVE_UP = false;
+    PAINT_MOVE_DOWN = false;
+    
+    ticker.attach(this, &RobotController::doTick, tick);
+    
+}
+
+/*
+    Private Methods
+*/
+
+void RobotController::doTick() {
+    robot.update();
+    
+    if (MOVE_BY_REFERENCE) {
+        float upperOutput = upperArmController.control(upperArmLengthReference - robot.getUpperArmLength(), tick);
+        float lowerOutput = lowerArmController.control(lowerArmLengthReference - robot.getLowerArmLength(), tick);
+        
+        robot.setUpperArmVelocity(upperOutput);
+        robot.setLowerArmVelocity(lowerOutput);
+    }
     
-    if (dLength < 0) {
-        arm.setVelocity(-1 * velocity);
-        while (dLength < 0) {
-            // Wait
-        }
-        arm.setVelocity(0);
-        return true;
-    } else {
-        arm.setVelocity(velocity);
-        while (dLength > 0) {
-            // Wait
-        }
-        arm.setVelocity(0);
-        return true;
+    if (PAINT_MOVE_UP) {
+        
+    }
+    
+    if (PAINT_MOVE_DOWN) {
+        
     }
 }
 
+/*
+    Public methods
+*/
+
+
+// Set the reference lengths for the arms that correspond to this roller coordinate
+void RobotController::moveTo(float x, float y) {
+    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) {
+    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);
+}
+
 
 
 
 
 
+
+
+
+/*
+    Robot queries
+*/
+
+float RobotController::getUpperArmLength() {
+    return robot.getUpperArmLength();
+}
+
+float RobotController::getLowerArmLength() {
+    return robot.getLowerArmLength();
+}
+
+bool RobotController::isKilled()   {
+    return robot.isKilled();
+}
+
+// Get robot pointer
+Robot* RobotController::getRobot() {
+    return &robot;
+}
+
+
+
+
+
+