MecanumDrive.cpp

Go to the documentation of this file.

#include <MecanumDrive.h>

MecanumDrive::MecanumDrive()
{
    m_Drive = NULL;
    m_DriveStick = NULL;
    m_ValueX = m_ValueY = m_ValueRotate = NULL;
    m_Gyro = NULL;
    m_GyroButton = NULL;
    m_GyroButtonIsPressed = false;
    m_gyroEnabled = false;
    m_gyroAngle = kNoGyro;
    m_SmoothAccel = false;
}

MecanumDrive::~MecanumDrive() {
    // Delete allocated member objects
    delete m_Drive; m_Drive = NULL;
    delete m_DriveStick; m_DriveStick = NULL;
    delete m_ValueX; m_ValueX = NULL;
    delete m_ValueY; m_ValueY = NULL;
    delete m_ValueRotate; m_ValueRotate = NULL;
    delete m_Gyro; m_Gyro = NULL;
    delete m_GyroButton; m_GyroButton = NULL;
}

/*
 * Called once, to perform initialization that isn't appropriate for constructor.
 */
void MecanumDrive::ComponentInit() {
    // Initialize Drive w/ motor safety
    if (!m_Drive) m_Drive = new RobotDrive (kFrontLeftCh, kBackLeftCh, kFrontRightCh, kBackRightCh);
    m_Drive->SetExpiration(kSafetyTimeout);
    m_Drive->SetSafetyEnabled(true);

    // Initialize Controller
    if (!m_DriveStick) m_DriveStick = new Joystick(kDriveStickCh);

    // Initialize SmoothAxis es. SmoothAxes? Whatever.
    if (!m_ValueX) m_ValueX = new SmoothAxis();
    if (!m_ValueY) m_ValueY = new SmoothAxis();
    if (!m_ValueRotate) m_ValueRotate = new SmoothAxis();

    // Initialize Gyroscope
    if (!m_Gyro) m_Gyro = new Gyro(kGyroCh);

    // Initialize Gyro Button
    if (!m_GyroButton) m_GyroButton = new JoystickButton(m_DriveStick,kGyroButton);
    m_GyroButtonIsPressed = false;

    SetSmoothAccel(m_SmoothAccel);  // Delegate initialization of SmoothAxiseseses
}

/*
 * Component-specific sensory input collector.
 * Read joystick positions and button states.
 * TODO: support zero-calibration of controller.
 */
void MecanumDrive::Sense() {
    m_ValueX->SetTarget( m_DriveStick->GetX(GenericHID::kRightHand) );
    m_ValueY->SetTarget( m_DriveStick->GetY(GenericHID::kRightHand) );
    m_ValueRotate->SetTarget( m_DriveStick->GetX(GenericHID::kLeftHand) );

    /*
     * Read GyroButton. Simple logic to debounce the button (register
     * a change of state just once) by acting the off-to-on transition
     * only.
     *
     * It would have been nice to use the WhenPressed() method, but
     * that would require us to implement a Command for this operation,
     * which is not worth the effort.
     */
    if (m_GyroButton->Get()) {
        // Button is pressed - Check if this is the off-to-on transition.
        if (!m_GyroButtonIsPressed) {
            // This is it!
            m_GyroButtonIsPressed = true;
            // Toggle enablement
            m_gyroEnabled = !m_gyroEnabled;
        }
    }
    else {
        // Button isn't pressed.
        m_GyroButtonIsPressed = false;
    }

    // Read gyroscope, if enabled.
    m_gyroAngle = isGyroEnabled() ? m_Gyro->GetAngle() : kNoGyro;
}

/*
 * Component-specific worker function.
 */
void MecanumDrive::DoWork() {
    m_Drive->MecanumDrive_Cartesian(m_ValueY->GetValue(),
            m_ValueX->GetValue(), m_ValueRotate->GetValue(), m_gyroAngle);
}

/*
 * Set Gyroscope enablement.
 */
void MecanumDrive::setGyroEnabled(bool gyroEnabled) {
    // Documentation for the built-in gyro states that the gyro object should be instantiated
    // from the constructor of the Robot class, to allow for calibration while stationary.
    // However, this conflicts with other instructions stating that the hardware may not be
    // ready for operation at that time, and recommending that objects be instantiated in the
    // RobotInit function. So, to be safe, we instantiate in RobotInit, but ensure that we
    // calibrate when enabling use of the gyro. There you go.
    m_Gyro->Reset();  // calibrate a new 0 heading
    m_gyroEnabled = gyroEnabled;
}

/*
 * Set Smooth Acceleration feature.
 */
void MecanumDrive::SetSmoothAccel(bool enable) {
    m_SmoothAccel = enable;
    m_ValueX->setEnabled(enable);
    m_ValueY->setEnabled(enable);
    m_ValueRotate->setEnabled(enable);
    // Todo: report state on display.
}