File content as of revision 18:1c9dc6caab9d:

#include "controller.h"

/*
    Constructor
*/

RobotController::RobotController():
        upperArmController(0.2, 0, 0),
        lowerArmController(1.0, 0, 0),
        upperXController(1.2, 0, 0.03),
        lowerXController(0.6, 0, 0.03),
        debug(USBTX, USBRX),
        robot() {

    // 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);
    
}

/*
    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 (PAINT_MOVE_UP) {
        // 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 (lower arm)
        float yError = topY - y;
        float lowerOutput = lowerArmController.control(yError, tick);
        
        // 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);
        
    }
    
    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 = upperArmLength + robot.getUpperArmEncoderVelocity() * gearRadius * tick;
        float futureX, futureY;
        getRollerPositionForArmLengths(futureUpperArmLength, lowerArmLength, futureX, futureY);
        
        // Calculate lower arm length that is needed to compensate for the leading arm
        float upper,lower;
        getArmLengthsForRollerPosition(bottomX, futureY, upper, lower);
        
        // Calculate compensating arm velocity based on error
        float compensatingArmError = lower - lowerArmLength;
        float lowerOutput = lowerXController.control(compensatingArmError, tick);
        
        // Instruct arm motors
        robot.setUpperArmVelocity(upperOutput);
        robot.setLowerArmVelocity(lowerOutput);
        
    }
}

/*
    Public methods
*/


// 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;

    MOVE_BY_REFERENCE = true;
}

// Set the arm lengths
void RobotController::setArmLengths(float upper, float lower) {
    clearFlags();
    
    upperArmLengthReference = upper;
    lowerArmLengthReference = lower;
    
    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
*/

float RobotController::getUpperArmLength() {
    return robot.getUpperArmLength();
}

float RobotController::getLowerArmLength() {
    return robot.getLowerArmLength();
}

bool RobotController::isKilled()   {
    return robot.isKilled();
}

// Get the robot instance
Robot* RobotController::getRobot() {
    return &robot;
}