Joystick.cpp

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                                                         */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
/*----------------------------------------------------------------------------*/

#include "Joystick.h"
#include "DriverStation.h"
#include "WPIErrors.h"
#include <math.h>

const UINT32 Joystick::kDefaultXAxis;
const UINT32 Joystick::kDefaultYAxis;
const UINT32 Joystick::kDefaultZAxis;
const UINT32 Joystick::kDefaultTwistAxis;
const UINT32 Joystick::kDefaultThrottleAxis;
const UINT32 Joystick::kDefaultTriggerButton;
const UINT32 Joystick::kDefaultTopButton;
static Joystick *joysticks[DriverStation::kJoystickPorts];
static bool joySticksInitialized = false;

Joystick::Joystick(UINT32 port)
        : m_ds (NULL)
        , m_port (port)
        , m_axes (NULL)
        , m_buttons (NULL)
{
        InitJoystick(kNumAxisTypes, kNumButtonTypes);

        m_axes[kXAxis] = kDefaultXAxis;
        m_axes[kYAxis] = kDefaultYAxis;
        m_axes[kZAxis] = kDefaultZAxis;
        m_axes[kTwistAxis] = kDefaultTwistAxis;
        m_axes[kThrottleAxis] = kDefaultThrottleAxis;
        
        m_buttons[kTriggerButton] = kDefaultTriggerButton;
        m_buttons[kTopButton] = kDefaultTopButton;
}

Joystick::Joystick(UINT32 port, UINT32 numAxisTypes, UINT32 numButtonTypes)
        : m_ds (NULL)
        , m_port (port)
        , m_axes (NULL)
        , m_buttons (NULL)
{
        InitJoystick(numAxisTypes, numButtonTypes);
}

void Joystick::InitJoystick(UINT32 numAxisTypes, UINT32 numButtonTypes)
{
        if ( !joySticksInitialized )
        {
                for (unsigned i = 0; i < DriverStation::kJoystickPorts; i++)
                        joysticks[i] = NULL;
                joySticksInitialized = true;
        }
        joysticks[m_port - 1] = this;
        
        m_ds = DriverStation::GetInstance();
        m_axes = new UINT32[numAxisTypes];
        m_buttons = new UINT32[numButtonTypes];
}

Joystick * Joystick::GetStickForPort(UINT32 port)
{
        Joystick *stick = joysticks[port - 1];
        if (stick == NULL)
        {
                stick = new Joystick(port);
                joysticks[port - 1] = stick;
        }
        return stick;
}

Joystick::~Joystick()
{
        delete [] m_buttons;
        delete [] m_axes;
}

float Joystick::GetX(JoystickHand hand)
{
        return GetRawAxis(m_axes[kXAxis]);
}

float Joystick::GetY(JoystickHand hand)
{
        return GetRawAxis(m_axes[kYAxis]);
}

float Joystick::GetZ()
{
        return GetRawAxis(m_axes[kZAxis]);
}

float Joystick::GetTwist()
{
        return GetRawAxis(m_axes[kTwistAxis]);
}

float Joystick::GetThrottle()
{
        return GetRawAxis(m_axes[kThrottleAxis]);
}

float Joystick::GetRawAxis(UINT32 axis)
{
        return m_ds->GetStickAxis(m_port, axis);
}

float Joystick::GetAxis(AxisType axis)
{
        switch(axis)
        {
                case kXAxis: return this->GetX();
                case kYAxis: return this->GetY();
                case kZAxis: return this->GetZ();
                case kTwistAxis: return this->GetTwist();
                case kThrottleAxis: return this->GetThrottle();
                default:
                        wpi_setWPIError(BadJoystickAxis);
                        return 0.0;
        }
}

bool Joystick::GetTrigger(JoystickHand hand)
{
        return GetRawButton(m_buttons[kTriggerButton]);
}

bool Joystick::GetTop(JoystickHand hand)
{
        return GetRawButton(m_buttons[kTopButton]);
}

bool Joystick::GetBumper(JoystickHand hand)
{
        // Joysticks don't have bumpers.
        return false;
}

bool Joystick::GetRawButton(UINT32 button)
{
        return ((0x1 << (button-1)) & m_ds->GetStickButtons(m_port)) != 0;
}

bool Joystick::GetButton(ButtonType button)
{
        switch (button)
        {
        case kTriggerButton: return GetTrigger();
        case kTopButton: return GetTop();
        default:
                return false;
        }
}

UINT32 Joystick::GetAxisChannel(AxisType axis)
{
        return m_axes[axis];
}

void Joystick::SetAxisChannel(AxisType axis, UINT32 channel)
{
        m_axes[axis] = channel;
}

float Joystick::GetMagnitude(){
        return sqrt(pow(GetX(),2) + pow(GetY(),2) );
}

float Joystick::GetDirectionRadians(){
        return atan2(GetX(), -GetY());
}

float Joystick::GetDirectionDegrees(){
        return (180/acos(-1))*GetDirectionRadians();
}