File content as of revision 25:bdb854127c11:

#include "robot.h"

/*
    Pins
*/

const PinName upperMotorDir = D7;
const PinName upperMotorPWM = D6;

const PinName lowerMotorDir = D4;
const PinName lowerMotorPWM = D5;

const PinName upperEncoderPin1 = D13;
const PinName upperEncoderPin2 = D12;

const PinName lowerEncoderPin1 = D10;
const PinName lowerEncoderPin2 = D9;

// Arm length when the system is turned on
const float initialArmLength = L_min;

// Limit the velocities of both arms
const float maxUpperArmVelocity = 2.1; // Rad/s
const float maxLowerArmVelocity = 3;   // Rad/s

/*
    Constructor
*/

Robot::Robot():
    upperArm(initialArmLength, maxUpperArmVelocity, upperMotorPWM, upperMotorDir, upperEncoderPin1, upperEncoderPin2),
    lowerArm(initialArmLength, maxLowerArmVelocity, lowerMotorPWM, lowerMotorDir, lowerEncoderPin1, lowerEncoderPin2) {
    killed = false;
}

/*
    Methods
*/

// Update the arms
void Robot::update() {
    upperArm.update();
    lowerArm.update();
}

// Get upper arm encoder velocity
float Robot::getUpperArmEncoderVelocity()  {
    return upperArm.getEncoderVelocity();
}

// Get lower arm encoder velocity
float Robot::getLowerArmEncoderVelocity()  {
    return lowerArm.getEncoderVelocity();
}

// Set upper arm velocity (rad/s)
void Robot::setUpperArmVelocity(float referenceVelocity) {
    if (!killed) {
        upperArm.setVelocity(referenceVelocity);
    }
}

// Set lower arm velocity (rad/s) (multiplied with -1 because for some reason the directions are different for the two arms)
void Robot::setLowerArmVelocity(float referenceVelocity) {
    if (!killed) {
        lowerArm.setVelocity(-1 * referenceVelocity);
    }
}

// Turn off both arms
void Robot::kill() {
    killed = true;
    upperArm.kill();
    lowerArm.kill();
}

// Indicates if the robot has been killed
bool Robot::isKilled() {
    return killed;
}

// Get upper arm length
float Robot::getUpperArmLength(){
    return upperArm.getLength();
}

// Get lower arm length
float Robot::getLowerArmLength(){
    return lowerArm.getLength();    
}