Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Repaired simulation build on linux

Browse Source 
Reverted to old driverstation and joystick code because we're not ready
for windows drive station yet

updated paths to reflect new wpilib organization
fixed name of gazebo topic (if you want /gazebo/frc/time use ~/time)
included network tables in wpilibJavaSim
Added ds script, and improved frcsim script
always start gazebo with verbose

Change-Id: I3c54b7000019a5985079a88200896a8069e69b86
This commit is contained in:
Peter_Mitrano
2015-11-29 00:57:27 -05:00
parent 0d062fba3a
commit d69803804f
15 changed files with 582 additions and 802 deletions
Download Patch File Download Diff File Expand all files Collapse all files

317
wpilibc/simulation/src/Joystick.cpp
View File

@@ -1,16 +1,14 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.
*/
/* 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 "NetworkCommunication/UsageReporting.h"
#include "WPIErrors.h"
#include <math.h>
#include <string.h>
#include <memory>
const uint32_t Joystick::kDefaultXAxis;
const uint32_t Joystick::kDefaultYAxis;
@@ -26,21 +24,19 @@ static bool joySticksInitialized = false;
* Construct an instance of a joystick.
* The joystick index is the usb port on the drivers station.
*
* @param port The port on the driver station that the joystick is plugged into
* (0-5).
* @param port The port on the driver station that the joystick is plugged into.
*/
Joystick::Joystick(uint32_t port)
: Joystick(port, kNumAxisTypes, kNumButtonTypes) {
m_axes[kXAxis] = kDefaultXAxis;
m_axes[kYAxis] = kDefaultYAxis;
m_axes[kZAxis] = kDefaultZAxis;
m_axes[kTwistAxis] = kDefaultTwistAxis;
m_axes[kThrottleAxis] = kDefaultThrottleAxis;
: Joystick(port, 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;
HALReport(HALUsageReporting::kResourceType_Joystick, port);
m_buttons[kTriggerButton] = kDefaultTriggerButton;
m_buttons[kTopButton] = kDefaultTopButton;
}
/**
@@ -53,108 +49,111 @@ Joystick::Joystick(uint32_t port)
* @param numAxisTypes The number of axis types in the enum.
* @param numButtonTypes The number of button types in the enum.
*/
Joystick::Joystick(uint32_t port, uint32_t numAxisTypes,
uint32_t numButtonTypes)
: m_ds(DriverStation::GetInstance()),
m_port(port),
m_axes(numAxisTypes),
m_buttons(numButtonTypes) {
if (!joySticksInitialized) {
for (auto& joystick : joysticks) joystick = nullptr;
joySticksInitialized = true;
}
if (m_port >= DriverStation::kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
} else {
joysticks[m_port] = this;
}
Joystick::Joystick(uint32_t port, uint32_t numAxisTypes, uint32_t numButtonTypes)
: m_port (port),
m_ds(DriverStation::GetInstance())
{
if ( !joySticksInitialized )
{
for (unsigned i = 0; i < DriverStation::kJoystickPorts; i++)
joysticks[i] = nullptr;
joySticksInitialized = true;
}
joysticks[m_port] = this;
m_axes = std::make_unique<uint32_t[]>(numAxisTypes);
m_buttons = std::make_unique<uint32_t[]>(numButtonTypes);
}
Joystick *Joystick::GetStickForPort(uint32_t port) {
Joystick *stick = joysticks[port];
if (stick == nullptr) {
stick = new Joystick(port);
joysticks[port] = stick;
}
return stick;
Joystick * Joystick::GetStickForPort(uint32_t port)
{
Joystick *stick = joysticks[port];
if (stick == nullptr)
{
stick = new Joystick(port);
joysticks[port] = stick;
}
return stick;
}
/**
* Get the X value of the joystick.
* This depends on the mapping of the joystick connected to the current port.
* @param hand This parameter is ignored for the Joystick class and is only here
* to complete the GenericHID interface.
*/
float Joystick::GetX(JoystickHand hand) const {
return GetRawAxis(m_axes[kXAxis]);
float Joystick::GetX(JoystickHand hand) const
{
return GetRawAxis(m_axes[kXAxis]);
}
/**
* Get the Y value of the joystick.
* This depends on the mapping of the joystick connected to the current port.
* @param hand This parameter is ignored for the Joystick class and is only here
* to complete the GenericHID interface.
*/
float Joystick::GetY(JoystickHand hand) const {
return GetRawAxis(m_axes[kYAxis]);
float Joystick::GetY(JoystickHand hand) const
{
return GetRawAxis(m_axes[kYAxis]);
}
/**
* Get the Z value of the current joystick.
* This depends on the mapping of the joystick connected to the current port.
*/
float Joystick::GetZ() const { return GetRawAxis(m_axes[kZAxis]); }
float Joystick::GetZ() const
{
return GetRawAxis(m_axes[kZAxis]);
}
/**
* Get the twist value of the current joystick.
* This depends on the mapping of the joystick connected to the current port.
*/
float Joystick::GetTwist() const { return GetRawAxis(m_axes[kTwistAxis]); }
float Joystick::GetTwist() const
{
return GetRawAxis(m_axes[kTwistAxis]);
}
/**
* Get the throttle value of the current joystick.
* This depends on the mapping of the joystick connected to the current port.
*/
float Joystick::GetThrottle() const {
return GetRawAxis(m_axes[kThrottleAxis]);
float Joystick::GetThrottle() const
{
return GetRawAxis(m_axes[kThrottleAxis]);
}
/**
* Get the value of the axis.
*
* @param axis The axis to read, starting at 0.
* @param axis The axis to read [1-6].
* @return The value of the axis.
*/
float Joystick::GetRawAxis(uint32_t axis) const {
return m_ds.GetStickAxis(m_port, axis);
float Joystick::GetRawAxis(uint32_t axis) const
{
return m_ds.GetStickAxis(m_port, axis);
}
/**
* For the current joystick, return the axis determined by the argument.
*
* This is for cases where the joystick axis is returned programatically,
* otherwise one of the
* This is for cases where the joystick axis is returned programatically, otherwise one of the
* previous functions would be preferable (for example GetX()).
*
* @param axis The axis to read.
* @return The value of the axis.
*/
float Joystick::GetAxis(AxisType axis) const {
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;
}
float Joystick::GetAxis(AxisType axis) const
{
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;
}
}
/**
@@ -162,12 +161,12 @@ float Joystick::GetAxis(AxisType axis) const {
*
* Look up which button has been assigned to the trigger and read its state.
*
* @param hand This parameter is ignored for the Joystick class and is only here
* to complete the GenericHID interface.
* @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface.
* @return The state of the trigger.
*/
bool Joystick::GetTrigger(JoystickHand hand) const {
return GetRawButton(m_buttons[kTriggerButton]);
bool Joystick::GetTrigger(JoystickHand hand) const
{
return GetRawButton(m_buttons[kTriggerButton]);
}
/**
@@ -175,45 +174,45 @@ bool Joystick::GetTrigger(JoystickHand hand) const {
*
* Look up which button has been assigned to the top and read its state.
*
* @param hand This parameter is ignored for the Joystick class and is only here
* to complete the GenericHID interface.
* @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface.
* @return The state of the top button.
*/
bool Joystick::GetTop(JoystickHand hand) const {
return GetRawButton(m_buttons[kTopButton]);
bool Joystick::GetTop(JoystickHand hand) const
{
return GetRawButton(m_buttons[kTopButton]);
}
/**
* This is not supported for the Joystick.
* This method is only here to complete the GenericHID interface.
*/
bool Joystick::GetBumper(JoystickHand hand) const {
// Joysticks don't have bumpers.
return false;
bool Joystick::GetBumper(JoystickHand hand) const
{
// Joysticks don't have bumpers.
return false;
}
/**
* Get the button value (starting at button 1)
* Get the button value for buttons 1 through 12.
*
* The buttons are returned in a single 16 bit value with one bit representing
* the state
* The buttons are returned in a single 16 bit value with one bit representing the state
* of each button. The appropriate button is returned as a boolean value.
*
* @param button The button number to be read (starting at 1)
* @param button The button number to be read.
* @return The state of the button.
**/
bool Joystick::GetRawButton(uint32_t button) const {
return m_ds.GetStickButton(m_port, button);
bool Joystick::GetRawButton(uint32_t button) const
{
return m_ds.GetStickButton(m_port, button);
}
/**
* Get the state of a POV on the joystick.
*
* @param pov The index of the POV to read (starting at 0)
* @return the angle of the POV in degrees, or -1 if the POV is not pressed.
*/
* Get the state of a POV on the joystick.
*
* @return the angle of the POV in degrees, or -1 if the POV is not pressed.
*/
int Joystick::GetPOV(uint32_t pov) const {
return m_ds.GetStickPOV(m_port, pov);
return 0; // TODO
}
/**
@@ -224,75 +223,27 @@ int Joystick::GetPOV(uint32_t pov) const {
* @param button The type of button to read.
* @return The state of the button.
*/
bool Joystick::GetButton(ButtonType button) const {
switch (button) {
case kTriggerButton:
return GetTrigger();
case kTopButton:
return GetTop();
default:
return false;
}
bool Joystick::GetButton(ButtonType button) const
{
switch (button)
{
case kTriggerButton: return GetTrigger();
case kTopButton: return GetTop();
default:
return false;
}
}
/**
* Get the number of axis for a joystick
*
* @return the number of axis for the current joystick
*/
int Joystick::GetAxisCount() const { return m_ds.GetStickAxisCount(m_port); }
/**
* Get the value of isXbox for the joystick.
*
* @return A boolean that is true if the joystick is an xbox controller.
*/
bool Joystick::GetIsXbox() const { return m_ds.GetJoystickIsXbox(m_port); }
/**
* Get the HID type of the controller.
*
* @return the HID type of the controller.
*/
Joystick::HIDType Joystick::GetType() const {
return static_cast<HIDType>(m_ds.GetJoystickType(m_port));
}
/**
* Get the name of the joystick.
*
* @return the name of the controller.
*/
std::string Joystick::GetName() const { return m_ds.GetJoystickName(m_port); }
// int Joystick::GetAxisType(uint8_t axis) const
//{
// return m_ds.GetJoystickAxisType(m_port, axis);
//}
/**
* Get the number of axis for a joystick
*
* @return the number of buttons on the current joystick
*/
int Joystick::GetButtonCount() const {
return m_ds.GetStickButtonCount(m_port);
}
/**
* Get the number of axis for a joystick
*
* @return then umber of POVs for the current joystick
*/
int Joystick::GetPOVCount() const { return m_ds.GetStickPOVCount(m_port); }
/**
* Get the channel currently associated with the specified axis.
*
* @param axis The axis to look up the channel for.
* @return The channel fr the axis.
*/
uint32_t Joystick::GetAxisChannel(AxisType axis) const { return m_axes[axis]; }
uint32_t Joystick::GetAxisChannel(AxisType axis)
{
return m_axes[axis];
}
/**
* Set the channel associated with a specified axis.
@@ -300,8 +251,9 @@ uint32_t Joystick::GetAxisChannel(AxisType axis) const { return m_axes[axis]; }
* @param axis The axis to set the channel for.
* @param channel The channel to set the axis to.
*/
void Joystick::SetAxisChannel(AxisType axis, uint32_t channel) {
m_axes[axis] = channel;
void Joystick::SetAxisChannel(AxisType axis, uint32_t channel)
{
m_axes[axis] = channel;
}
/**
@@ -311,7 +263,7 @@ void Joystick::SetAxisChannel(AxisType axis, uint32_t channel) {
* @return The magnitude of the direction vector
*/
float Joystick::GetMagnitude() const {
return sqrt(pow(GetX(), 2) + pow(GetY(), 2));
return sqrt(pow(GetX(),2) + pow(GetY(),2) );
}
/**
@@ -320,58 +272,19 @@ float Joystick::GetMagnitude() const {
*
* @return The direction of the vector in radians
*/
float Joystick::GetDirectionRadians() const { return atan2(GetX(), -GetY()); }
float Joystick::GetDirectionRadians() const {
return atan2(GetX(), -GetY());
}
/**
* Get the direction of the vector formed by the joystick and its origin
* in degrees
*
* uses acos(-1) to represent Pi due to absence of readily accessible Pi
* uses acos(-1) to represent Pi due to absence of readily accessable Pi
* constant in C++
*
* @return The direction of the vector in degrees
*/
float Joystick::GetDirectionDegrees() const {
return (180 / acos(-1)) * GetDirectionRadians();
}
/**
* Set the rumble output for the joystick. The DS currently supports 2 rumble
* values,
* left rumble and right rumble
* @param type Which rumble value to set
* @param value The normalized value (0 to 1) to set the rumble to
*/
void Joystick::SetRumble(RumbleType type, float value) {
if (value < 0)
value = 0;
else if (value > 1)
value = 1;
if (type == kLeftRumble)
m_leftRumble = value * 65535;
else
m_rightRumble = value * 65535;
HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
}
/**
* Set a single HID output value for the joystick.
* @param outputNumber The index of the output to set (1-32)
* @param value The value to set the output to
*/
void Joystick::SetOutput(uint8_t outputNumber, bool value) {
m_outputs =
(m_outputs & ~(1 << (outputNumber - 1))) | (value << (outputNumber - 1));
HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
}
/**
* Set all HID output values for the joystick.
* @param value The 32 bit output value (1 bit for each output)
*/
void Joystick::SetOutputs(uint32_t value) {
m_outputs = value;
HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
return (180/acos(-1))*GetDirectionRadians();
}