ActuatedJoint.cpp

Go to the documentation of this file.

#include "ActuatedJoint.h"

#include "DebugMacro.h"

ActuatedJoint::ActuatedJoint(int jointID, double jointMin, double jointMax, Drive *drive) : Joint(jointID, jointMin, jointMax) {
    this->drive = drive;
}
//TODO: add in check
ControlMode ActuatedJoint::setMode(ControlMode driveMode_, motorProfile profile) {
    if (driveMode_ == POSITION_CONTROL) {
        if (drive->initPosControl(profile)) {
            driveMode = driveMode_;
            return POSITION_CONTROL;
        }
    }
    if (driveMode_ == VELOCITY_CONTROL) {
        if (drive->initVelControl(profile)) {
            driveMode = driveMode_;
            return POSITION_CONTROL;
        }
    }
    // \todo Implement Torque
    return ERROR;
}

setMovementReturnCode_t ActuatedJoint::setPosition(double desQ) {
    if (driveMode == POSITION_CONTROL) {
        drive->setPos(toDriveUnits(desQ));
        drive->posControlConfirmSP();
        return SUCCESS;
    } else {
        // Replace once complete
        return INCORRECT_MODE;
    }
}

setMovementReturnCode_t ActuatedJoint::setVelocity(double velocity) {
    if (driveMode == VELOCITY_CONTROL) {
        drive->setVel(toDriveUnits(velocity));
        return SUCCESS;
    } else {
        // Replace once complete
        return INCORRECT_MODE;
    }
}

setMovementReturnCode_t ActuatedJoint::setTorque(double torque) {
    /*\todo Replace once complete*/
    return UNKNOWN_ERROR;
}

void ActuatedJoint::readyToSwitchOn() {
    drive->readyToSwitchOn();
}

bool ActuatedJoint::enable() {
    if (drive->getDriveState() == READY_TO_SWITCH_ON) {
        drive->enable();
        return true;
    }
    return false;
}