Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-26 01:51:41 +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

644
wpilibc/simulation/src/DriverStation.cpp
View File

@@ -1,227 +1,81 @@
/*----------------------------------------------------------------------------*/
/* 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 "DriverStation.h"
#include "AnalogInput.h"
#include "Timer.h"
#include "NetworkCommunication/FRCComm.h"
#include "MotorSafetyHelper.h"
#include "simulation/MainNode.h"
//#include "MotorSafetyHelper.h"
#include "Utility.h"
#include "WPIErrors.h"
#include <string.h>
#include "Log.hpp"
#include "boost/mem_fn.hpp"
// set the logging level
TLogLevel dsLogLevel = logDEBUG;
const double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
#define DS_LOG(level) \
if (level > dsLogLevel) ; \
else Log().Get(level)
if (level > dsLogLevel) ; \
else Log().Get(level)
const uint32_t DriverStation::kBatteryChannel;
const uint32_t DriverStation::kJoystickPorts;
const uint32_t DriverStation::kJoystickAxes;
const float DriverStation::kUpdatePeriod = 0.02;
uint8_t DriverStation::m_updateNumber = 0;
/**
* DriverStation constructor.
* DriverStation contructor.
*
* This is only called once the first time GetInstance() is called
*/
DriverStation::DriverStation() {
// All joysticks should default to having zero axes, povs and buttons, so
// uninitialized memory doesn't get sent to speed controllers.
for (unsigned int i = 0; i < kJoystickPorts; i++) {
m_joystickAxes[i].count = 0;
m_joystickPOVs[i].count = 0;
m_joystickButtons[i].count = 0;
m_joystickDescriptor[i].isXbox = 0;
m_joystickDescriptor[i].type = -1;
m_joystickDescriptor[i].name[0] = '\0';
}
// Register that semaphore with the network communications task.
// It will signal when new packet data is available.
HALSetNewDataSem(&m_packetDataAvailableCond);
state = msgs::DriverStationPtr(new msgs::DriverStation());
stateSub = MainNode::Subscribe("~/ds/state",
&DriverStation::stateCallback, this);
// TODO: for loop + boost bind
joysticks[0] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
joysticksSub[0] = MainNode::Subscribe("~/ds/joysticks/0",
&DriverStation::joystickCallback0, this);
joysticks[1] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
joysticksSub[1] = MainNode::Subscribe("~/ds/joysticks/1",
&DriverStation::joystickCallback1, this);
joysticks[2] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
joysticksSub[2] = MainNode::Subscribe("~/ds/joysticks/2",
&DriverStation::joystickCallback2, this);
joysticks[3] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
joysticksSub[3] = MainNode::Subscribe("~/ds/joysticks/5",
&DriverStation::joystickCallback3, this);
joysticks[4] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
joysticksSub[4] = MainNode::Subscribe("~/ds/joysticks/4",
&DriverStation::joystickCallback4, this);
joysticks[5] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
joysticksSub[5] = MainNode::Subscribe("~/ds/joysticks/5",
&DriverStation::joystickCallback5, this);
AddToSingletonList();
}
void DriverStation::Run() {
int period = 0;
while (true) {
{
std::unique_lock<priority_mutex> lock(m_packetDataAvailableMutex);
m_packetDataAvailableCond.wait(lock);
}
GetData();
m_waitForDataCond.notify_all();
if (++period >= 4) {
MotorSafetyHelper::CheckMotors();
period = 0;
}
if (m_userInDisabled) HALNetworkCommunicationObserveUserProgramDisabled();
if (m_userInAutonomous) HALNetworkCommunicationObserveUserProgramAutonomous();
if (m_userInTeleop) HALNetworkCommunicationObserveUserProgramTeleop();
if (m_userInTest) HALNetworkCommunicationObserveUserProgramTest();
}
AddToSingletonList();
}
/**
* Return a reference to the singleton DriverStation.
* @return Pointer to the DS instance
* Return a pointer to the singleton DriverStation.
*/
DriverStation &DriverStation::GetInstance() {
static DriverStation instance;
return instance;
DriverStation& DriverStation::GetInstance()
{
static DriverStation instance;
return instance;
}
/**
* Copy data from the DS task for the user.
* If no new data exists, it will just be returned, otherwise
* the data will be copied from the DS polling loop.
*/
void DriverStation::GetData() {
// Get the status of all of the joysticks
for (uint8_t stick = 0; stick < kJoystickPorts; stick++) {
HALGetJoystickAxes(stick, &m_joystickAxes[stick]);
HALGetJoystickPOVs(stick, &m_joystickPOVs[stick]);
HALGetJoystickButtons(stick, &m_joystickButtons[stick]);
HALGetJoystickDescriptor(stick, &m_joystickDescriptor[stick]);
}
m_newControlData.give();
}
/**
* Read the battery voltage.
* Read the battery voltage. Hardcoded to 12 volts for Simulation.
*
* @return The battery voltage in Volts.
* @return The battery voltage.
*/
float DriverStation::GetBatteryVoltage() const {
int32_t status = 0;
float voltage = getVinVoltage(&status);
wpi_setErrorWithContext(status, "getVinVoltage");
return voltage;
}
/**
* Reports errors related to unplugged joysticks
* Throttles the errors so that they don't overwhelm the DS
*/
void DriverStation::ReportJoystickUnpluggedError(std::string message) {
double currentTime = Timer::GetFPGATimestamp();
if (currentTime > m_nextMessageTime) {
ReportError(message);
m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
}
}
/**
* Returns the number of axes on a given joystick port
*
* @param stick The joystick port number
* @return The number of axes on the indicated joystick
*/
int DriverStation::GetStickAxisCount(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return 0;
}
HALJoystickAxes joystickAxes;
HALGetJoystickAxes(stick, &joystickAxes);
return joystickAxes.count;
}
/**
* Returns the name of the joystick at the given port
*
* @param stick The joystick port number
* @return The name of the joystick at the given port
*/
std::string DriverStation::GetJoystickName(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
}
std::string retVal(m_joystickDescriptor[0].name);
return retVal;
}
/**
* Returns the type of joystick at a given port
*
* @param stick The joystick port number
* @return The HID type of joystick at the given port
*/
int DriverStation::GetJoystickType(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return -1;
}
return (int)m_joystickDescriptor[stick].type;
}
/**
* Returns a boolean indicating if the controller is an xbox controller.
*
* @param stick The joystick port number
* @return A boolean that is true if the controller is an xbox controller.
*/
bool DriverStation::GetJoystickIsXbox(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return false;
}
return (bool)m_joystickDescriptor[stick].isXbox;
}
/**
* Returns the types of Axes on a given joystick port
*
* @param stick The joystick port number and the target axis
* @return What type of axis the axis is reporting to be
*/
int DriverStation::GetJoystickAxisType(uint32_t stick, uint8_t axis) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return -1;
}
return m_joystickDescriptor[stick].axisTypes[axis];
}
/**
* Returns the number of POVs on a given joystick port
*
* @param stick The joystick port number
* @return The number of POVs on the indicated joystick
*/
int DriverStation::GetStickPOVCount(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return 0;
}
HALJoystickPOVs joystickPOVs;
HALGetJoystickPOVs(stick, &joystickPOVs);
return joystickPOVs.count;
}
/**
* Returns the number of buttons on a given joystick port
*
* @param stick The joystick port number
* @return The number of buttons on the indicated joystick
*/
int DriverStation::GetStickButtonCount(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return 0;
}
HALJoystickButtons joystickButtons;
HALGetJoystickButtons(stick, &joystickButtons);
return joystickButtons.count;
float DriverStation::GetBatteryVoltage() const
{
return 12.0; // 12 volts all the time!
}
/**
@@ -232,287 +86,285 @@ int DriverStation::GetStickButtonCount(uint32_t stick) const {
* @param axis The analog axis value to read from the joystick.
* @return The value of the axis on the joystick.
*/
float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return 0;
}
float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis)
{
if (axis < 0 || axis > (kJoystickAxes - 1))
{
wpi_setWPIError(BadJoystickAxis);
return 0.0;
}
if (stick < 0 || stick > 5)
{
wpi_setWPIError(BadJoystickIndex);
return 0.0;
}
if (axis >= m_joystickAxes[stick].count) {
if (axis >= kMaxJoystickAxes) {
wpi_setWPIError(BadJoystickAxis);
}
else {
ReportJoystickUnpluggedError(
"WARNING: Joystick Axis missing, check if all controllers are "
"plugged in\n");
}
return 0.0f;
}
int8_t value = m_joystickAxes[stick].axes[axis];
if (value < 0) {
return value / 128.0f;
} else {
return value / 127.0f;
}
std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
if (joysticks[stick] == nullptr || axis >= joysticks[stick]->axes().size())
{
return 0.0;
}
return joysticks[stick]->axes(axis);
}
/**
* Get the state of a POV on the joystick.
* The state of a specific button (1 - 12) on the joystick.
* This method only works in simulation, but is more efficient than GetStickButtons.
*
* @return the angle of the POV in degrees, or -1 if the POV is not pressed.
* @param stick The joystick to read.
* @param button The button number to check.
* @return If the button is pressed.
*/
int DriverStation::GetStickPOV(uint32_t stick, uint32_t pov) {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return -1;
}
bool DriverStation::GetStickButton(uint32_t stick, uint32_t button)
{
if (stick < 0 || stick >= 6)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 0 and 5");
return false;
}
if (pov >= m_joystickPOVs[stick].count) {
if (pov >= kMaxJoystickPOVs) {
wpi_setWPIError(BadJoystickAxis);
}
else {
ReportJoystickUnpluggedError(
"WARNING: Joystick POV missing, check if all controllers are plugged "
"in\n");
}
return -1;
}
return m_joystickPOVs[stick].povs[pov];
std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
if (joysticks[stick] == nullptr || button >= joysticks[stick]->buttons().size())
{
return false;
}
return joysticks[stick]->buttons(button-1);
}
/**
* The state of the buttons on the joystick.
* 12 buttons (4 msb are unused) from the joystick.
*
* @param stick The joystick to read.
* @return The state of the buttons on the joystick.
*/
uint32_t DriverStation::GetStickButtons(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return 0;
}
short DriverStation::GetStickButtons(uint32_t stick)
{
if (stick < 0 || stick >= 6)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 0 and 5");
return false;
}
short btns = 0, btnid;
return m_joystickButtons[stick].buttons;
std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
msgs::FRCJoystickPtr joy = joysticks[stick];
for (btnid = 0; btnid < joy->buttons().size() && btnid < 12; btnid++)
{
if (joysticks[stick]->buttons(btnid))
{
btns |= (1 << btnid);
}
}
return btns;
}
// 5V divided by 10 bits
#define kDSAnalogInScaling ((float)(5.0 / 1023.0))
/**
* The state of one joystick button. Button indexes begin at 1.
* Get an analog voltage from the Driver Station.
* The analog values are returned as voltage values for the Driver Station analog inputs.
* These inputs are typically used for advanced operator interfaces consisting of potentiometers
* or resistor networks representing values on a rotary switch.
*
* @param stick The joystick to read.
* @param button The button index, beginning at 1.
* @return The state of the joystick button.
* @param channel The analog input channel on the driver station to read from. Valid range is 1 - 4.
* @return The analog voltage on the input.
*/
bool DriverStation::GetStickButton(uint32_t stick, uint8_t button) {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return false;
}
if (button > m_joystickButtons[stick].count) {
ReportJoystickUnpluggedError(
"WARNING: Joystick Button missing, check if all controllers are "
"plugged in\n");
return false;
}
if (button == 0) {
ReportJoystickUnpluggedError(
"ERROR: Button indexes begin at 1 in WPILib for C++ and Java");
return false;
}
return ((0x1 << (button - 1)) & m_joystickButtons[stick].buttons) != 0;
float DriverStation::GetAnalogIn(uint32_t channel)
{
wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetAnalogIn");
return 0.0;
}
/**
* Check if the DS has enabled the robot
* @return True if the robot is enabled and the DS is connected
* Get values from the digital inputs on the Driver Station.
* Return digital values from the Drivers Station. These values are typically used for buttons
* and switches on advanced operator interfaces.
* @param channel The digital input to get. Valid range is 1 - 8.
*/
bool DriverStation::IsEnabled() const {
HALControlWord controlWord;
memset(&controlWord, 0, sizeof(controlWord));
HALGetControlWord(&controlWord);
return controlWord.enabled && controlWord.dsAttached;
bool DriverStation::GetDigitalIn(uint32_t channel)
{
wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalIn");
return false;
}
/**
* Check if the robot is disabled
* @return True if the robot is explicitly disabled or the DS is not connected
* Set a value for the digital outputs on the Driver Station.
*
* Control digital outputs on the Drivers Station. These values are typically used for
* giving feedback on a custom operator station such as LEDs.
*
* @param channel The digital output to set. Valid range is 1 - 8.
* @param value The state to set the digital output.
*/
bool DriverStation::IsDisabled() const {
HALControlWord controlWord;
memset(&controlWord, 0, sizeof(controlWord));
HALGetControlWord(&controlWord);
return !(controlWord.enabled && controlWord.dsAttached);
void DriverStation::SetDigitalOut(uint32_t channel, bool value)
{
wpi_setWPIErrorWithContext(UnsupportedInSimulation, "SetDigitalOut");
}
/**
* Check if the DS is commanding autonomous mode
* @return True if the robot is being commanded to be in autonomous mode
* Get a value that was set for the digital outputs on the Driver Station.
* @param channel The digital ouput to monitor. Valid range is 1 through 8.
* @return A digital value being output on the Drivers Station.
*/
bool DriverStation::IsAutonomous() const {
HALControlWord controlWord;
memset(&controlWord, 0, sizeof(controlWord));
HALGetControlWord(&controlWord);
return controlWord.autonomous;
bool DriverStation::GetDigitalOut(uint32_t channel)
{
wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalOut");
return false;
}
/**
* Check if the DS is commanding teleop mode
* @return True if the robot is being commanded to be in teleop mode
*/
bool DriverStation::IsOperatorControl() const {
HALControlWord controlWord;
memset(&controlWord, 0, sizeof(controlWord));
HALGetControlWord(&controlWord);
return !(controlWord.autonomous || controlWord.test);
bool DriverStation::IsEnabled() const
{
std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
return state != nullptr ? state->enabled() : false;
}
/**
* Check if the DS is commanding test mode
* @return True if the robot is being commanded to be in test mode
*/
bool DriverStation::IsTest() const {
HALControlWord controlWord;
HALGetControlWord(&controlWord);
return controlWord.test;
bool DriverStation::IsDisabled() const
{
return !IsEnabled();
}
/**
* Check if the DS is attached
* @return True if the DS is connected to the robot
*/
bool DriverStation::IsDSAttached() const {
HALControlWord controlWord;
memset(&controlWord, 0, sizeof(controlWord));
HALGetControlWord(&controlWord);
return controlWord.dsAttached;
bool DriverStation::IsAutonomous() const
{
std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
return state != nullptr ?
state->state() == msgs::DriverStation_State_AUTO : false;
}
/**
* @return always true in simulation
*/
bool DriverStation::IsSysActive() const {
return true;
bool DriverStation::IsOperatorControl() const
{
return !(IsAutonomous() || IsTest());
}
/**
* @return always false in simulation
*/
bool DriverStation::IsSysBrownedOut() const {
return false;
}
/**
* Has a new control packet from the driver station arrived since the last time
* this function was called?
* Warning: If you call this function from more than one place at the same time,
* you will not get the get the intended behaviour.
* @return True if the control data has been updated since the last call.
*/
bool DriverStation::IsNewControlData() const {
return m_newControlData.tryTake() == false;
bool DriverStation::IsTest() const
{
std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
return state != nullptr ?
state->state() == msgs::DriverStation_State_TEST : false;
}
/**
* Is the driver station attached to a Field Management System?
* @return True if the robot is competing on a field being controlled by a Field
* Management System
* Note: This does not work with the Blue DS.
* @return True if the robot is competing on a field being controlled by a Field Management System
*/
bool DriverStation::IsFMSAttached() const {
HALControlWord controlWord;
HALGetControlWord(&controlWord);
return controlWord.fmsAttached;
bool DriverStation::IsFMSAttached() const
{
return false; // No FMS in simulation
}
/**
* Return the alliance that the driver station says it is on.
* This could return kRed or kBlue
* @return The Alliance enum (kRed, kBlue or kInvalid)
* @return The Alliance enum
*/
DriverStation::Alliance DriverStation::GetAlliance() const {
HALAllianceStationID allianceStationID;
HALGetAllianceStation(&allianceStationID);
switch (allianceStationID) {
case kHALAllianceStationID_red1:
case kHALAllianceStationID_red2:
case kHALAllianceStationID_red3:
return kRed;
case kHALAllianceStationID_blue1:
case kHALAllianceStationID_blue2:
case kHALAllianceStationID_blue3:
return kBlue;
default:
return kInvalid;
}
DriverStation::Alliance DriverStation::GetAlliance() const
{
// if (m_controlData->dsID_Alliance == 'R') return kRed;
// if (m_controlData->dsID_Alliance == 'B') return kBlue;
// wpi_assert(false);
return kInvalid; // TODO: Support alliance colors
}
/**
* Return the driver station location on the field
* This could return 1, 2, or 3
* @return The location of the driver station (1-3, 0 for invalid)
* @return The location of the driver station
*/
uint32_t DriverStation::GetLocation() const {
HALAllianceStationID allianceStationID;
HALGetAllianceStation(&allianceStationID);
switch (allianceStationID) {
case kHALAllianceStationID_red1:
case kHALAllianceStationID_blue1:
return 1;
case kHALAllianceStationID_red2:
case kHALAllianceStationID_blue2:
return 2;
case kHALAllianceStationID_red3:
case kHALAllianceStationID_blue3:
return 3;
default:
return 0;
}
uint32_t DriverStation::GetLocation() const
{
return -1; // TODO: Support locations
}
/**
* Wait until a new packet comes from the driver station
* This blocks on a semaphore, so the waiting is efficient.
* This is a good way to delay processing until there is new driver station data
* to act on
* This is a good way to delay processing until there is new driver station data to act on
*/
void DriverStation::WaitForData() {
std::unique_lock<priority_mutex> lock(m_waitForDataMutex);
m_waitForDataCond.wait(lock);
void DriverStation::WaitForData()
{
std::unique_lock<std::mutex> lock(m_waitForDataMutex);
m_waitForDataCond.wait(lock);
}
/**
* Return the approximate match time
* The FMS does not send an official match time to the robots, but does send an
* approximate match time.
* The value will count down the time remaining in the current period (auto or
* teleop).
* Warning: This is not an official time (so it cannot be used to dispute ref
* calls or guarantee that a function
* will trigger before the match ends)
* The Practice Match function of the DS approximates the behaviour seen on the
* field.
* @return Time remaining in current match period (auto or teleop)
* The FMS does not currently send the official match time to the robots
* This returns the time since the enable signal sent from the Driver Station
* At the beginning of autonomous, the time is reset to 0.0 seconds
* At the beginning of teleop, the time is reset to +15.0 seconds
* If the robot is disabled, this returns 0.0 seconds
* Warning: This is not an official time (so it cannot be used to argue with referees)
* @return Match time in seconds since the beginning of autonomous
*/
double DriverStation::GetMatchTime() const {
float matchTime;
HALGetMatchTime(&matchTime);
return (double)matchTime;
double DriverStation::GetMatchTime() const
{
if (m_approxMatchTimeOffset < 0.0)
return 0.0;
return Timer::GetFPGATimestamp() - m_approxMatchTimeOffset;
}
/**
* Report an error to the DriverStation messages window.
* The error is also printed to the program console.
*/
void DriverStation::ReportError(std::string error) {
std::cout << error << std::endl;
HALControlWord controlWord;
HALGetControlWord(&controlWord);
if (controlWord.dsAttached) {
HALSetErrorData(error.c_str(), error.size(), 0);
}
void DriverStation::ReportError(std::string error)
{
std::cout << error << std::endl;
}
/**
* Return the team number that the Driver Station is configured for
* @return The team number
*/
uint16_t DriverStation::GetTeamNumber() const
{
return 348;
}
void DriverStation::stateCallback(const msgs::ConstDriverStationPtr &msg)
{
{
std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
*state = *msg;
}
m_waitForDataCond.notify_all();
}
void DriverStation::joystickCallback(const msgs::ConstFRCJoystickPtr &msg,
int i)
{
std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
*(joysticks[i]) = *msg;
}
void DriverStation::joystickCallback0(const msgs::ConstFRCJoystickPtr &msg)
{
joystickCallback(msg, 0);
}
void DriverStation::joystickCallback1(const msgs::ConstFRCJoystickPtr &msg)
{
joystickCallback(msg, 1);
}
void DriverStation::joystickCallback2(const msgs::ConstFRCJoystickPtr &msg)
{
joystickCallback(msg, 2);
}
void DriverStation::joystickCallback3(const msgs::ConstFRCJoystickPtr &msg)
{
joystickCallback(msg, 3);
}
void DriverStation::joystickCallback4(const msgs::ConstFRCJoystickPtr &msg)
{
joystickCallback(msg, 4);
}
void DriverStation::joystickCallback5(const msgs::ConstFRCJoystickPtr &msg)
{
joystickCallback(msg, 5);
}

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();
}

2
wpilibc/simulation/src/RobotBase.cpp
View File

@@ -34,7 +34,7 @@ RobotBase &RobotBase::getInstance()
RobotBase::RobotBase() : m_ds(DriverStation::GetInstance())
{
RobotState::SetImplementation(DriverStation::GetInstance());
transport::SubscriberPtr time_pub = MainNode::Subscribe("time", &wpilib::internal::time_callback);
time_sub = MainNode::Subscribe("~/time", &wpilib::internal::time_callback);
}
/**

32
wpilibc/simulation/src/Utility.cpp
View File

@@ -16,7 +16,7 @@
#include <cstdio>
#include <cstdlib>
#include <cstring>
#if defined(UNIX)
#if not defined(_WIN32)
#include <execinfo.h>
#include <cxxabi.h>
#endif
@@ -58,29 +58,27 @@ static void wpi_handleTracing()
* This allows breakpoints to be set on an assert.
* The users don't call this, but instead use the wpi_assert macros in Utility.h.
*/
bool wpi_assert_impl(bool conditionValue, const std::string &conditionText,
const std::string &message, const std::string &fileName,
uint32_t lineNumber, const std::string &funcName) {
bool wpi_assert_impl(bool conditionValue, const char *conditionText,
const char *message, const char *fileName,
uint32_t lineNumber, const char *funcName) {
if (!conditionValue) {
// Error string buffer
std::stringstream error;
std::stringstream errorStream;
// If an error message was specified, include it
// Build error string
if (message.size()) {
error << "Assertion failed: \"" << message << "\", \"" << conditionText
<< "\" failed in " << funcName + "() in " << fileName << " at line "
<< lineNumber;
errorStream << "Assertion \"" << conditionText << "\" ";
errorStream << "on line " << lineNumber << " ";
errorStream << "of " << basename(fileName) << " ";
if (message[0] != '\0') {
errorStream << "failed: " << message << std::endl;
} else {
error << "Assertion failed: \"" << conditionText << "\" in " << funcName
<< "() in " << fileName << " at line " << lineNumber;
errorStream << "failed." << std::endl;
}
// Print to console and send to remote dashboard
std::cout << "\n\n>>>>" << error.str();
std::cout << "\n\n>>>>" << errorStream.str();
wpi_handleTracing();
}
return conditionValue;
}
@@ -167,7 +165,7 @@ uint32_t GetFPGATime()
}
//TODO: implement symbol demangling and backtrace on windows
#if defined(UNIX)
#if not defined(_WIN32)
/**
* Demangle a C++ symbol, used for printing stack traces.