KinectStick.cpp

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. 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 "KinectStick.h"

#include "DriverStation.h"
#include "Joystick.h"
#include "NetworkCommunication/FRCComm.h"
#include "NetworkCommunication/UsageReporting.h"
#include "Utility.h"
#include "WPIErrors.h"

uint32_t KinectStick::_recentPacketNumber = 0;
KinectStick::KinectStickData KinectStick::_sticks;

#define kJoystickBundleID kFRC_NetworkCommunication_DynamicType_Kinect_Joystick
#define kTriggerMask 1
#define kTopMask 2

KinectStick::KinectStick(int id)
{
        if (id != 1 && id != 2)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "KinectStick ID must be 1 or 2");
                return;
        }
        m_id = id;

        nUsageReporting::report(nUsageReporting::kResourceType_KinectStick, id);
}

float KinectStick::GetX(JoystickHand hand)
{
        return GetRawAxis(Joystick::kDefaultXAxis);
}

float KinectStick::GetY(JoystickHand hand)
{
        return GetRawAxis(Joystick::kDefaultYAxis);
}

float KinectStick::GetZ()
{
        return GetRawAxis(Joystick::kDefaultZAxis);
}

float KinectStick::GetTwist()
{
        return GetRawAxis(Joystick::kDefaultTwistAxis);
}

float KinectStick::GetThrottle()
{
        return GetRawAxis(Joystick::kDefaultThrottleAxis);
}

float KinectStick::GetRawAxis(uint32_t axis)
{
        if (StatusIsFatal()) return 0.0;

        GetData();
        float value = ConvertRawToFloat(_sticks.formatted.rawSticks[m_id - 1].axis[axis-1]);
        return value;
}

bool KinectStick::GetTrigger(JoystickHand hand)
{
        return GetRawButton(kTriggerMask);
}

bool KinectStick::GetTop(JoystickHand hand)
{
        return GetRawButton(kTopMask);
}

bool KinectStick::GetBumper(JoystickHand hand)
{
        // TODO: Should this even be in GenericHID?  Is 4 an appropriate mask value (button 3)?
        return GetRawButton(4);
}

bool KinectStick::GetRawButton(uint32_t button)
{
        if (StatusIsFatal()) return false;

        GetData();
        return (_sticks.formatted.rawSticks[m_id - 1].buttons & (1 << button)) != 0;
}

void KinectStick::GetData()
{
        uint32_t packetNumber = DriverStation::GetInstance()->GetPacketNumber();
        if (_recentPacketNumber != packetNumber)
        {
                _recentPacketNumber = packetNumber;
                int retVal = getDynamicControlData(kJoystickBundleID, _sticks.data, sizeof(_sticks.data), 5);
                if (retVal == 0)
                {
                        wpi_assert(_sticks.formatted.size == sizeof(_sticks.data) - 1);
                }
        }
}

float KinectStick::ConvertRawToFloat(int8_t value)
{
        float result;

        if (value < 0)
                result = ((float) value) / 128.0;
        else
                result = ((float) value) / 127.0;

        wpi_assert(result <= 1.0 && result >= -1.0);

        if (result > 1.0)
                result = 1.0;
        else if (result < -1.0)
                result = -1.0;

        return result;
}