File content as of revision 28:09b08753498c:

#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();
    
    /*
        MOVE BY REFERENCE
        
        Movement is done based on the upperArmLengthReference and lowerArmLengthReference instance variables. An error between the current (estimated) arm
        length and reference arm length is used to generate a proportional velocity to remove the error.
    */
    if (MOVE_BY_REFERENCE) {
        // Generate control velocities (rad/s)
        float upperOutput = upperArmController.control(upperArmLengthReference - robot.getUpperArmLength(), tick);
        float lowerOutput = lowerArmController.control(lowerArmLengthReference - robot.getLowerArmLength(), tick);
        // Instruct motors
        robot.setUpperArmVelocity(upperOutput);
        robot.setLowerArmVelocity(lowerOutput);
    }
    /*
        PAINT MOVE UP
        
        Painting movement makes a distinction between the two arms. One arm, which is referred to as the leading arm, controls the y value of the roller.
        Painting upwards will require an increase in the y-coordinate of the roller. The velocity of the leading arm is proportional to the error in y
        values. 
        The leading arm will, by altering the height of the roller, also influence the x coordinate of the roller. The other arm will compensate for this.
        A prediction is made where the other arm will be the next tick (based on the arm's encoder velocity). This is used to make a prediction about the
        location of the roller and which arm length is needed to compensate for the error of x. A PD-Controller then outputs a velocity to the compensating
        arm, based on the length which is required to set x to its desired value.
        
    */
    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;
        float upperOutput = upperXController.control(compensatingArmError, tick);
        

        // Instruct arm motors
        robot.setUpperArmVelocity(upperOutput);
        robot.setLowerArmVelocity(lowerOutput);
        
    }
    
    /*
        PAINT MOVE DOWN
        
        Very similar to paint move up.^
    */
    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;
}

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