RobotArms.cpp

Go to the documentation of this file.

#include "RobotArms.h"

RobotArms::RobotArms() {
    // TODO Auto-generated constructor stub
    m_DoubleSolenoid = NULL;
    m_ControlStick = NULL;
    m_ArmMotor = NULL;
    m_TopLimit = NULL;
    m_BottomLimit = NULL;
    m_Limit = NULL;
    m_DriveCounter = NULL;

    k_minCount = 0;
    k_maxCount = 0;

    openArms = false;
    liftSpeed = 0;

}

RobotArms::~RobotArms() {
    // Delete allocated member objects
    delete m_DoubleSolenoid; m_DoubleSolenoid = NULL;
    delete m_ControlStick; m_ControlStick = NULL;
    delete m_ArmMotor; m_ArmMotor = NULL;
    delete m_Limit; m_Limit = NULL;
    delete m_TopLimit; m_TopLimit = NULL;
    delete m_BottomLimit; m_BottomLimit = NULL;
}
void RobotArms::ComponentInit() {
    if (!m_ArmMotor) m_ArmMotor = new Victor(kArmMotorChannel);
    if (!m_DoubleSolenoid) m_DoubleSolenoid = new DoubleSolenoid(kSolenoidChannel1, kSolenoidChannel2);
    if (!m_ControlStick) m_ControlStick = new Joystick(kStickChannel);
    if (!m_TopLimit) m_TopLimit = new DigitalInput(kTopLimitChannel);
    if (!m_BottomLimit) m_BottomLimit = new DigitalInput(kBottomLimitChannel);
    if (!m_Limit) m_Limit = new DigitalInput(kLimitChannel);

    CalibrateArms();
}
void RobotArms::Sense() {
    if (m_ControlStick->GetRawAxis(2) >= 0.1){
        liftSpeed = 0.5;
    }
    else if (m_ControlStick->GetRawAxis(2) <= -0.1){
        liftSpeed = -0.5;
    }
    else {
        liftSpeed = 0;
    }

    if (m_ControlStick->GetRawButton(5)|| m_ControlStick->GetRawButton(6)){
        openArms = true;
    }
    else{
        openArms = false;
    }
}

void RobotArms::DoWork() {
    m_ArmMotor->Set(liftSpeed);

    if (openArms){
        m_DoubleSolenoid->Set(DoubleSolenoid::kForward);
    }
    else{
        m_DoubleSolenoid->Set(DoubleSolenoid::kReverse);
    }

}

void RobotArms::CalibrateArms(){
    liftSpeed = -0.5;
    while (!m_BottomLimit->Get()){
        m_ArmMotor->Set(liftSpeed);
    }
    m_ArmMotor->StopMotor();

    liftSpeed = 0.5;
    // TODO Set Encoder Value to Zero
    while (!m_TopLimit->Get()){
        m_ArmMotor->Set(liftSpeed);
    }
    m_ArmMotor->StopMotor();

    //TODO Set Maxlift to encoder value
    liftSpeed = -0.5;
    while (!m_BottomLimit->Get()){
        m_ArmMotor->Set(liftSpeed);
    }
    m_ArmMotor->StopMotor();
    liftSpeed = 0.0;
}