Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Renamed folders for consistency, using sim/athena/shared schema (#27)

Browse Source 
Rename the following folders:
hal/lib/Athena -> hal/lib/athena
hal/lib/Desktop -> hal/lib/sim
hal/lib/Shared -> hal/lib/shared
wpilibc/Athena -> wpilibc/athena
wpilibc/simulation -> wpilibc/sim

Windows users may need to run gradlew clean after updating.
This commit is contained in:
Peter Mitrano
2016-05-22 17:55:51 -04:00
committed by Peter Johnson
parent 54092378e9
commit e71f454b9d
308 changed files with 14 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
wpilibc/sim/src/AnalogGyro.cpp Normal file
View File

@@ -0,0 +1,78 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "AnalogGyro.h"
#include "LiveWindow/LiveWindow.h"
#include "Timer.h"
#include "WPIErrors.h"
const uint32_t AnalogGyro::kOversampleBits = 10;
const uint32_t AnalogGyro::kAverageBits = 0;
const float AnalogGyro::kSamplesPerSecond = 50.0;
const float AnalogGyro::kCalibrationSampleTime = 5.0;
const float AnalogGyro::kDefaultVoltsPerDegreePerSecond = 0.007;
/**
* Initialize the gyro.
*
* Calibrate the gyro by running for a number of samples and computing the
* center value for this part. Then use the center value as the Accumulator
* center value for subsequent measurements. It's important to make sure that
* the robot is not moving while the centering calculations are in progress,
* this is typically done when the robot is first turned on while it's sitting
* at rest before the competition starts.
*/
void AnalogGyro::InitAnalogGyro(int channel) {
SetPIDSourceType(PIDSourceType::kDisplacement);
char buffer[50];
int n = sprintf(buffer, "analog/%d", channel);
impl = new SimGyro(buffer);
LiveWindow::GetInstance()->AddSensor("AnalogGyro", channel, this);
}
/**
* AnalogGyro constructor with only a channel.
*
* @param channel The analog channel the gyro is connected to.
*/
AnalogGyro::AnalogGyro(uint32_t channel) { InitAnalogGyro(channel); }
/**
* Reset the gyro.
*
* Resets the gyro to a heading of zero. This can be used if there is
* significant drift in the gyro and it needs to be recalibrated after it has
* been running.
*/
void AnalogGyro::Reset() { impl->Reset(); }
void AnalogGyro::Calibrate() { Reset(); }
/**
* Return the actual angle in degrees that the robot is currently facing.
*
* The angle is based on the current accumulator value corrected by the
* oversampling rate, the gyro type and the A/D calibration values. The angle
* is continuous, that is can go beyond 360 degrees. This make algorithms that
* wouldn't want to see a discontinuity in the gyro output as it sweeps past 0
* on the second time around.
*
* @return the current heading of the robot in degrees. This heading is based on
* integration of the returned rate from the gyro.
*/
float AnalogGyro::GetAngle() const { return impl->GetAngle(); }
/**
* Return the rate of rotation of the gyro
*
* The rate is based on the most recent reading of the gyro analog value
*
* @return the current rate in degrees per second
*/
double AnalogGyro::GetRate() const { return impl->GetVelocity(); }

83
wpilibc/sim/src/AnalogInput.cpp Normal file
View File

@@ -0,0 +1,83 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "AnalogInput.h"
#include "LiveWindow/LiveWindow.h"
#include "WPIErrors.h"
/**
* Construct an analog input.
*
* @param channel The channel number to represent.
*/
AnalogInput::AnalogInput(uint32_t channel) {
m_channel = channel;
char buffer[50];
int n = sprintf(buffer, "analog/%d", channel);
m_impl = new SimFloatInput(buffer);
LiveWindow::GetInstance()->AddSensor("AnalogInput", channel, this);
}
/**
* Get a scaled sample straight from this channel.
*
* The value is scaled to units of Volts using the calibrated scaling data from
* GetLSBWeight() and GetOffset().
*
* @return A scaled sample straight from this channel.
*/
float AnalogInput::GetVoltage() const { return m_impl->Get(); }
/**
* Get a scaled sample from the output of the oversample and average engine for
* this channel.
*
* The value is scaled to units of Volts using the calibrated scaling data from
* GetLSBWeight() and GetOffset(). Using oversampling will cause this value to
* be higher resolution, but it will update more slowly. Using averaging will
* cause this value to be more stable, but it will update more slowly.
*
* @return A scaled sample from the output of the oversample and average engine
* for this channel.
*/
float AnalogInput::GetAverageVoltage() const { return m_impl->Get(); }
/**
* Get the channel number.
*
* @return The channel number.
*/
uint32_t AnalogInput::GetChannel() const { return m_channel; }
/**
* Get the Average value for the PID Source base object.
*
* @return The average voltage.
*/
double AnalogInput::PIDGet() { return GetAverageVoltage(); }
void AnalogInput::UpdateTable() {
if (m_table != nullptr) {
m_table->PutNumber("Value", GetAverageVoltage());
}
}
void AnalogInput::StartLiveWindowMode() {}
void AnalogInput::StopLiveWindowMode() {}
std::string AnalogInput::GetSmartDashboardType() const {
return "Analog Input";
}
void AnalogInput::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> AnalogInput::GetTable() const { return m_table; }

84
wpilibc/sim/src/AnalogPotentiometer.cpp Normal file
View File

@@ -0,0 +1,84 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "AnalogPotentiometer.h"
/**
* Common initialization code called by all constructors.
*/
void AnalogPotentiometer::initPot(AnalogInput* input, double scale,
double offset) {
m_scale = scale;
m_offset = offset;
m_analog_input = input;
}
AnalogPotentiometer::AnalogPotentiometer(int channel, double scale,
double offset) {
m_init_analog_input = true;
initPot(new AnalogInput(channel), scale, offset);
}
AnalogPotentiometer::AnalogPotentiometer(AnalogInput* input, double scale,
double offset) {
m_init_analog_input = false;
initPot(input, scale, offset);
}
AnalogPotentiometer::AnalogPotentiometer(AnalogInput& input, double scale,
double offset) {
m_init_analog_input = false;
initPot(&input, scale, offset);
}
AnalogPotentiometer::~AnalogPotentiometer() {
if (m_init_analog_input) {
delete m_analog_input;
m_init_analog_input = false;
}
}
/**
* Get the current reading of the potentiomere.
*
* @return The current position of the potentiometer.
*/
double AnalogPotentiometer::Get() const {
return m_analog_input->GetVoltage() * m_scale + m_offset;
}
/**
* Implement the PIDSource interface.
*
* @return The current reading.
*/
double AnalogPotentiometer::PIDGet() { return Get(); }
/**
* @return the Smart Dashboard Type
*/
std::string AnalogPotentiometer::GetSmartDashboardType() const {
return "Analog Input";
}
/**
* Live Window code, only does anything if live window is activated.
*/
void AnalogPotentiometer::InitTable(std::shared_ptr<ITable> subtable) {
m_table = subtable;
UpdateTable();
}
void AnalogPotentiometer::UpdateTable() {
if (m_table != nullptr) {
m_table->PutNumber("Value", Get());
}
}
std::shared_ptr<ITable> AnalogPotentiometer::GetTable() const {
return m_table;
}

54
wpilibc/sim/src/DigitalInput.cpp Normal file
View File

@@ -0,0 +1,54 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "DigitalInput.h"
#include "WPIErrors.h"
/**
* Create an instance of a Digital Input class.
* Creates a digital input given a channel and uses the default module.
*
* @param channel The digital channel (1..14).
*/
DigitalInput::DigitalInput(uint32_t channel) {
char buf[64];
m_channel = channel;
int n = sprintf(buf, "dio/%d", channel);
m_impl = new SimDigitalInput(buf);
}
/**
* Get the value from a digital input channel.
* Retrieve the value of a single digital input channel from the FPGA.
*/
uint32_t DigitalInput::Get() const { return m_impl->Get(); }
/**
* @return The GPIO channel number that this object represents.
*/
uint32_t DigitalInput::GetChannel() const { return m_channel; }
void DigitalInput::UpdateTable() {
if (m_table != nullptr) {
m_table->PutBoolean("Value", Get());
}
}
void DigitalInput::StartLiveWindowMode() {}
void DigitalInput::StopLiveWindowMode() {}
std::string DigitalInput::GetSmartDashboardType() const {
return "DigitalInput";
}
void DigitalInput::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> DigitalInput::GetTable() const { return m_table; }

121
wpilibc/sim/src/DoubleSolenoid.cpp Normal file
View File

@@ -0,0 +1,121 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "DoubleSolenoid.h"
#include <string.h>
#include "LiveWindow/LiveWindow.h"
#include "WPIErrors.h"
/**
* Constructor.
*
* @param forwardChannel The forward channel on the module to control.
* @param reverseChannel The reverse channel on the module to control.
*/
DoubleSolenoid::DoubleSolenoid(uint32_t forwardChannel, uint32_t reverseChannel)
: DoubleSolenoid(1, forwardChannel, reverseChannel) {}
/**
* Constructor.
*
* @param moduleNumber The solenoid module (1 or 2).
* @param forwardChannel The forward channel on the module to control.
* @param reverseChannel The reverse channel on the module to control.
*/
DoubleSolenoid::DoubleSolenoid(uint8_t moduleNumber, uint32_t forwardChannel,
uint32_t reverseChannel) {
m_reversed = false;
if (reverseChannel < forwardChannel) { // Swap ports to get the right address
int channel = reverseChannel;
reverseChannel = forwardChannel;
forwardChannel = channel;
m_reversed = true;
}
char buffer[50];
int n = sprintf(buffer, "pneumatic/%d/%d/%d/%d", moduleNumber, forwardChannel,
moduleNumber, reverseChannel);
m_impl = new SimContinuousOutput(buffer);
LiveWindow::GetInstance()->AddActuator("DoubleSolenoid", moduleNumber,
forwardChannel, this);
}
DoubleSolenoid::~DoubleSolenoid() {
if (m_table != nullptr) m_table->RemoveTableListener(this);
}
/**
* Set the value of a solenoid.
*
* @param value Move the solenoid to forward, reverse, or don't move it.
*/
void DoubleSolenoid::Set(Value value) {
m_value = value;
switch (value) {
case kOff:
m_impl->Set(0);
break;
case kForward:
m_impl->Set(m_reversed ? -1 : 1);
break;
case kReverse:
m_impl->Set(m_reversed ? 1 : -1);
break;
}
}
/**
* Read the current value of the solenoid.
*
* @return The current value of the solenoid.
*/
DoubleSolenoid::Value DoubleSolenoid::Get() const { return m_value; }
void DoubleSolenoid::ValueChanged(ITable* source, llvm::StringRef key,
std::shared_ptr<nt::Value> value,
bool isNew) {
if (!value->IsString()) return;
Value lvalue = kOff;
if (value->GetString() == "Forward")
lvalue = kForward;
else if (value->GetString() == "Reverse")
lvalue = kReverse;
Set(lvalue);
}
void DoubleSolenoid::UpdateTable() {
if (m_table != nullptr) {
m_table->PutString(
"Value", (Get() == kForward ? "Forward"
: (Get() == kReverse ? "Reverse" : "Off")));
}
}
void DoubleSolenoid::StartLiveWindowMode() {
Set(kOff);
if (m_table != nullptr) {
m_table->AddTableListener("Value", this, true);
}
}
void DoubleSolenoid::StopLiveWindowMode() {
Set(kOff);
if (m_table != nullptr) {
m_table->RemoveTableListener(this);
}
}
std::string DoubleSolenoid::GetSmartDashboardType() const {
return "Double Solenoid";
}
void DoubleSolenoid::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> DoubleSolenoid::GetTable() const { return m_table; }

368
wpilibc/sim/src/DriverStation.cpp Normal file
View File

@@ -0,0 +1,368 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "DriverStation.h"
#include "Timer.h"
#include "simulation/MainNode.h"
//#include "MotorSafetyHelper.h"
#include <string.h>
#include "Log.hpp"
#include "Utility.h"
#include "WPIErrors.h"
#include "boost/mem_fn.hpp"
// set the logging level
TLogLevel dsLogLevel = logDEBUG;
#define DS_LOG(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 contructor.
*
* This is only called once the first time GetInstance() is called
*/
DriverStation::DriverStation() {
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();
}
/**
* Return a pointer to the singleton DriverStation.
*/
DriverStation& DriverStation::GetInstance() {
static DriverStation instance;
return instance;
}
/**
* Read the battery voltage. Hardcoded to 12 volts for Simulation.
*
* @return The battery voltage.
*/
float DriverStation::GetBatteryVoltage() const {
return 12.0; // 12 volts all the time!
}
/**
* Get the value of the axis on a joystick.
* This depends on the mapping of the joystick connected to the specified port.
*
* @param stick The joystick to read.
* @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 (axis < 0 || axis > (kJoystickAxes - 1)) {
wpi_setWPIError(BadJoystickAxis);
return 0.0;
}
if (stick < 0 || stick > 5) {
wpi_setWPIError(BadJoystickIndex);
return 0.0;
}
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);
}
/**
* The state of a specific button (1 - 12) on the joystick.
*
* This method only works in simulation, but is more efficient than
* GetStickButtons.
*
* @param stick The joystick to read.
* @param button The button number to check.
* @return If the button is pressed.
*/
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;
}
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.
*/
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;
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))
/**
* 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 channel The analog input channel on the driver station to read from.
* Valid range is 1 - 4.
* @return The analog voltage on the input.
*/
float DriverStation::GetAnalogIn(uint32_t channel) {
wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetAnalogIn");
return 0.0;
}
/**
* 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::GetDigitalIn(uint32_t channel) {
wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalIn");
return false;
}
/**
* 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.
*/
void DriverStation::SetDigitalOut(uint32_t channel, bool value) {
wpi_setWPIErrorWithContext(UnsupportedInSimulation, "SetDigitalOut");
}
/**
* 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::GetDigitalOut(uint32_t channel) {
wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalOut");
return false;
}
bool DriverStation::IsEnabled() const {
std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
return state != nullptr ? state->enabled() : false;
}
bool DriverStation::IsDisabled() const { return !IsEnabled(); }
bool DriverStation::IsAutonomous() const {
std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
return state != nullptr ? state->state() == msgs::DriverStation_State_AUTO
: false;
}
bool DriverStation::IsOperatorControl() const {
return !(IsAutonomous() || IsTest());
}
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?
* 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 {
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
*/
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
*/
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.
*/
void DriverStation::WaitForData() {
std::unique_lock<std::mutex> lock(m_waitForDataMutex);
m_waitForDataCond.wait(lock);
}
/**
* Return the approximate match time.
* 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 {
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;
}
/**
* Report a warning to the DriverStation messages window.
* The warning is also printed to the program console.
*/
void DriverStation::ReportWarning(std::string error) {
std::cout << error << std::endl;
}
/**
* Report an error to the DriverStation messages window.
* The error is also printed to the program console.
*/
void DriverStation::ReportError(bool is_error, int32_t code,
const std::string& error,
const std::string& location,
const std::string& stack) {
if (!location.empty())
std::cout << (is_error ? "Error" : "Warning") << " at " << location << ": ";
std::cout << error << std::endl;
if (!stack.empty()) std::cout << stack << 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);
}

384
wpilibc/sim/src/Encoder.cpp Normal file
View File

@@ -0,0 +1,384 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Encoder.h"
#include "LiveWindow/LiveWindow.h"
#include "Resource.h"
#include "WPIErrors.h"
/**
* Common initialization code for Encoders.
* This code allocates resources for Encoders and is common to all constructors.
*
* The counter will start counting immediately.
*
* @param reverseDirection If true, counts down instead of up (this is all
* relative)
* @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X
* decoding. If 4X is selected, then an encoder FPGA
* object is used and the returned counts will be 4x
* the encoder spec'd value since all rising and
* falling edges are counted. If 1X or 2X are selected
* then a counter object will be used and the returned
* value will either exactly match the spec'd count or
* be double (2x) the spec'd count.
*/
void Encoder::InitEncoder(int channelA, int channelB, bool reverseDirection,
EncodingType encodingType) {
m_table = nullptr;
this->channelA = channelA;
this->channelB = channelB;
m_encodingType = encodingType;
m_encodingScale = encodingType == k4X ? 4 : encodingType == k2X ? 2 : 1;
int32_t index = 0;
m_distancePerPulse = 1.0;
LiveWindow::GetInstance()->AddSensor("Encoder", channelA, this);
if (channelB < channelA) { // Swap ports
int channel = channelB;
channelB = channelA;
channelA = channel;
m_reverseDirection = !reverseDirection;
} else {
m_reverseDirection = reverseDirection;
}
char buffer[50];
int n = sprintf(buffer, "dio/%d/%d", channelA, channelB);
impl = new SimEncoder(buffer);
impl->Start();
}
/**
* Encoder constructor.
*
* Construct a Encoder given a and b channels.
*
* The counter will start counting immediately.
*
* @param aChannel The a channel digital input channel.
* @param bChannel The b channel digital input channel.
* @param reverseDirection If true, counts down instead of up (this is all
* relative)
* @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X
* decoding. If 4X is selected, then an encoder FPGA
* object is used and the returned counts will be 4x
* the encoder spec'd value since all rising and
* falling edges are counted. If 1X or 2X are selected
* then a counter object will be used and the returned
* value will either exactly match the spec'd count or
* be double (2x) the spec'd count.
*/
Encoder::Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection,
EncodingType encodingType) {
InitEncoder(aChannel, bChannel, reverseDirection, encodingType);
}
/**
* Encoder constructor.
*
* Construct a Encoder given a and b channels as digital inputs. This is used in
* the case where the digital inputs are shared. The Encoder class will not
* allocate the digital inputs and assume that they already are counted.
*
* The counter will start counting immediately.
*
* @param aSource The source that should be used for the a channel.
* @param bSource the source that should be used for the b channel.
* @param reverseDirection If true, counts down instead of up (this is all
* relative)
* @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X
* decoding. If 4X is selected, then an encoder FPGA
* object is used and the returned counts will be 4x
* the encoder spec'd value since all rising and
* falling edges are counted. If 1X or 2X are selected
* then a counter object will be used and the returned
* value will either exactly match the spec'd count or
* be double (2x) the spec'd count.
*/
/* TODO: [Not Supported] Encoder::Encoder(DigitalSource *aSource, DigitalSource
*bSource, bool reverseDirection, EncodingType encodingType) :
m_encoder(nullptr),
m_counter(nullptr)
{
m_aSource = aSource;
m_bSource = bSource;
m_allocatedASource = false;
m_allocatedBSource = false;
if (m_aSource == nullptr || m_bSource == nullptr)
wpi_setWPIError(NullParameter);
else
InitEncoder(reverseDirection, encodingType);
}*/
/**
* Encoder constructor.
*
* Construct a Encoder given a and b channels as digital inputs. This is used in
* the case where the digital inputs are shared. The Encoder class will not
* allocate the digital inputs and assume that they already are counted.
*
* The counter will start counting immediately.
*
* @param aSource The source that should be used for the a channel.
* @param bSource the source that should be used for the b channel.
* @param reverseDirection If true, counts down instead of up (this is all
* relative)
* @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X
* decoding. If 4X is selected, then an encoder FPGA
* object is used and the returned counts will be 4x
* the encoder spec'd value since all rising and
* falling edges are counted. If 1X or 2X are selected
* then a counter object will be used and the returned
* value will either exactly match the spec'd count or
* be double (2x) the spec'd count.
*/
/*// TODO: [Not Supported] Encoder::Encoder(DigitalSource &aSource,
DigitalSource &bSource, bool reverseDirection, EncodingType encodingType) :
m_encoder(nullptr),
m_counter(nullptr)
{
m_aSource = &aSource;
m_bSource = &bSource;
m_allocatedASource = false;
m_allocatedBSource = false;
InitEncoder(reverseDirection, encodingType);
}*/
/**
* Reset the Encoder distance to zero.
*
* Resets the current count to zero on the encoder.
*/
void Encoder::Reset() { impl->Reset(); }
/**
* Determine if the encoder is stopped.
*
* Using the MaxPeriod value, a boolean is returned that is true if the encoder
* is considered stopped and false if it is still moving. A stopped encoder is
* one where the most recent pulse width exceeds the MaxPeriod.
*
* @return True if the encoder is considered stopped.
*/
bool Encoder::GetStopped() const {
throw "Simulation doesn't currently support this method.";
}
/**
* The last direction the encoder value changed.
*
* @return The last direction the encoder value changed.
*/
bool Encoder::GetDirection() const {
throw "Simulation doesn't currently support this method.";
}
/**
* The scale needed to convert a raw counter value into a number of encoder
* pulses.
*/
double Encoder::DecodingScaleFactor() const {
switch (m_encodingType) {
case k1X:
return 1.0;
case k2X:
return 0.5;
case k4X:
return 0.25;
default:
return 0.0;
}
}
/**
* The encoding scale factor 1x, 2x, or 4x, per the requested encodingType.
*
* Used to divide raw edge counts down to spec'd counts.
*/
int32_t Encoder::GetEncodingScale() const { return m_encodingScale; }
/**
* Gets the raw value from the encoder.
*
* The raw value is the actual count unscaled by the 1x, 2x, or 4x scale
* factor.
*
* @return Current raw count from the encoder
*/
int32_t Encoder::GetRaw() const {
throw "Simulation doesn't currently support this method.";
}
/**
* Gets the current count.
*
* Returns the current count on the Encoder.
* This method compensates for the decoding type.
*
* @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale
* factor.
*/
int32_t Encoder::Get() const {
throw "Simulation doesn't currently support this method.";
}
/**
* Returns the period of the most recent pulse.
*
* Returns the period of the most recent Encoder pulse in seconds.
* This method compenstates for the decoding type.
*
* @deprecated Use GetRate() in favor of this method. This returns unscaled
* periods and GetRate() scales using value from
* SetDistancePerPulse().
*
* @return Period in seconds of the most recent pulse.
*/
double Encoder::GetPeriod() const {
throw "Simulation doesn't currently support this method.";
}
/**
* Sets the maximum period for stopped detection.
*
* Sets the value that represents the maximum period of the Encoder before it
* will assume that the attached device is stopped. This timeout allows users
* to determine if the wheels or other shaft has stopped rotating.
* This method compensates for the decoding type.
*
* @deprecated Use SetMinRate() in favor of this method. This takes unscaled
* periods and SetMinRate() scales using value from
* SetDistancePerPulse().
*
* @param maxPeriod The maximum time between rising and falling edges before the
* FPGA will report the device stopped. This is expressed in
* seconds.
*/
void Encoder::SetMaxPeriod(double maxPeriod) {
throw "Simulation doesn't currently support this method.";
}
/**
* Get the distance the robot has driven since the last reset.
*
* @return The distance driven since the last reset as scaled by the value from
* SetDistancePerPulse().
*/
double Encoder::GetDistance() const {
return m_distancePerPulse * impl->GetPosition();
}
/**
* Get the current rate of the encoder.
*
* Units are distance per second as scaled by the value from
* SetDistancePerPulse().
*
* @return The current rate of the encoder.
*/
double Encoder::GetRate() const {
return m_distancePerPulse * impl->GetVelocity();
}
/**
* Set the minimum rate of the device before the hardware reports it stopped.
*
* @param minRate The minimum rate. The units are in distance per second as
* scaled by the value from SetDistancePerPulse().
*/
void Encoder::SetMinRate(double minRate) {
throw "Simulation doesn't currently support this method.";
}
/**
* Set the distance per pulse for this encoder.
*
* This sets the multiplier used to determine the distance driven based on the
* count value from the encoder. Do not include the decoding type in this scale.
* The library already compensates for the decoding type. Set this value based
* on the encoder's rated Pulses per Revolution and factor in gearing reductions
* following the encoder shaft. This distance can be in any units you like,
* linear or angular.
*
* @param distancePerPulse The scale factor that will be used to convert pulses
* to useful units.
*/
void Encoder::SetDistancePerPulse(double distancePerPulse) {
if (m_reverseDirection) {
m_distancePerPulse = -distancePerPulse;
} else {
m_distancePerPulse = distancePerPulse;
}
}
/**
* Set the direction sensing for this encoder.
*
* This sets the direction sensing on the encoder so that it could count in the
* correct software direction regardless of the mounting.
*
* @param reverseDirection true if the encoder direction should be reversed
*/
void Encoder::SetReverseDirection(bool reverseDirection) {
throw "Simulation doesn't currently support this method.";
}
/**
* Set which parameter of the encoder you are using as a process control
* variable.
*
* @param pidSource An enum to select the parameter.
*/
void Encoder::SetPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
/**
* Implement the PIDSource interface.
*
* @return The current value of the selected source parameter.
*/
double Encoder::PIDGet() {
switch (m_pidSource) {
case PIDSourceType::kDisplacement:
return GetDistance();
case PIDSourceType::kRate:
return GetRate();
default:
return 0.0;
}
}
void Encoder::UpdateTable() {
if (m_table != nullptr) {
m_table->PutNumber("Speed", GetRate());
m_table->PutNumber("Distance", GetDistance());
m_table->PutNumber("Distance per Tick", m_reverseDirection
? -m_distancePerPulse
: m_distancePerPulse);
}
}
void Encoder::StartLiveWindowMode() {}
void Encoder::StopLiveWindowMode() {}
std::string Encoder::GetSmartDashboardType() const {
if (m_encodingType == k4X)
return "Quadrature Encoder";
else
return "Encoder";
}
void Encoder::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> Encoder::GetTable() const { return m_table; }

277
wpilibc/sim/src/IterativeRobot.cpp Normal file
View File

@@ -0,0 +1,277 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "IterativeRobot.h"
#include "DriverStation.h"
#include "LiveWindow/LiveWindow.h"
#include "SmartDashboard/SmartDashboard.h"
#include "networktables/NetworkTable.h"
// not sure what this is used for yet.
#ifdef _UNIX
#include <unistd.h>
#endif
const double IterativeRobot::kDefaultPeriod = 0;
/**
* Set the period for the periodic functions.
*
* @param period The period of the periodic function calls. 0.0 means sync to
* driver station control data.
*/
void IterativeRobot::SetPeriod(double period) {
if (period > 0.0) {
// Not syncing with the DS, so start the timer for the main loop
m_mainLoopTimer.Reset();
m_mainLoopTimer.Start();
} else {
// Syncing with the DS, don't need the timer
m_mainLoopTimer.Stop();
}
m_period = period;
}
/**
* Get the period for the periodic functions.
*
* Returns 0.0 if configured to syncronize with DS control data packets.
*
* @return Period of the periodic function calls
*/
double IterativeRobot::GetPeriod() { return m_period; }
/**
* Get the number of loops per second for the IterativeRobot.
*
* @return Frequency of the periodic function calls
*/
double IterativeRobot::GetLoopsPerSec() {
// If syncing to the driver station, we don't know the rate,
// so guess something close.
if (m_period <= 0.0) return 50.0;
return 1.0 / m_period;
}
/**
* Provide an alternate "main loop" via StartCompetition().
*
* This specific StartCompetition() implements "main loop" behavior like that of
* the FRC control system in 2008 and earlier, with a primary (slow) loop that
* is called periodically, and a "fast loop" (a.k.a. "spin loop") that is
* called as fast as possible with no delay between calls.
*/
void IterativeRobot::StartCompetition() {
LiveWindow* lw = LiveWindow::GetInstance();
// first and one-time initialization
SmartDashboard::init();
NetworkTable::GetTable("LiveWindow")
->GetSubTable("~STATUS~")
->PutBoolean("LW Enabled", false);
RobotInit();
// loop forever, calling the appropriate mode-dependent function
lw->SetEnabled(false);
while (true) {
// Call the appropriate function depending upon the current robot mode
if (IsDisabled()) {
// call DisabledInit() if we are now just entering disabled mode from
// either a different mode or from power-on
if (!m_disabledInitialized) {
lw->SetEnabled(false);
DisabledInit();
m_disabledInitialized = true;
// reset the initialization flags for the other modes
m_autonomousInitialized = false;
m_teleopInitialized = false;
m_testInitialized = false;
}
if (NextPeriodReady()) {
// TODO: HALNetworkCommunicationObserveUserProgramDisabled();
DisabledPeriodic();
}
} else if (IsAutonomous()) {
// call AutonomousInit() if we are now just entering autonomous mode from
// either a different mode or from power-on
if (!m_autonomousInitialized) {
lw->SetEnabled(false);
AutonomousInit();
m_autonomousInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_teleopInitialized = false;
m_testInitialized = false;
}
if (NextPeriodReady()) {
// TODO: HALNetworkCommunicationObserveUserProgramAutonomous();
AutonomousPeriodic();
}
} else if (IsTest()) {
// call TestInit() if we are now just entering test mode from
// either a different mode or from power-on
if (!m_testInitialized) {
lw->SetEnabled(true);
TestInit();
m_testInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_autonomousInitialized = false;
m_teleopInitialized = false;
}
if (NextPeriodReady()) {
// TODO: HALNetworkCommunicationObserveUserProgramTest();
TestPeriodic();
}
} else {
// call TeleopInit() if we are now just entering teleop mode from
// either a different mode or from power-on
if (!m_teleopInitialized) {
lw->SetEnabled(false);
TeleopInit();
m_teleopInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_autonomousInitialized = false;
m_testInitialized = false;
Scheduler::GetInstance()->SetEnabled(true);
}
if (NextPeriodReady()) {
// TODO: HALNetworkCommunicationObserveUserProgramTeleop();
TeleopPeriodic();
}
}
// wait for driver station data so the loop doesn't hog the CPU
m_ds.WaitForData();
}
}
/**
* Determine if the periodic functions should be called.
*
* If m_period > 0.0, call the periodic function every m_period as compared
* to Timer.Get(). If m_period == 0.0, call the periodic functions whenever
* a packet is received from the Driver Station, or about every 20ms.
*
* @todo Decide what this should do if it slips more than one cycle.
*/
bool IterativeRobot::NextPeriodReady() {
if (m_period > 0.0) {
return m_mainLoopTimer.HasPeriodPassed(m_period);
} else {
// XXX: BROKEN! return m_ds->IsNewControlData();
}
return true;
}
/**
* Robot-wide initialization code should go here.
*
* Users should override this method for default Robot-wide initialization which
* will be called when the robot is first powered on. It will be called
* exactly 1 time.
*/
void IterativeRobot::RobotInit() {
printf("Default %s() method... Overload me!\n", __FUNCTION__);
}
/**
* Initialization code for disabled mode should go here.
*
* Users should override this method for initialization code which will be
* called each time the robot enters disabled mode.
*/
void IterativeRobot::DisabledInit() {
printf("Default %s() method... Overload me!\n", __FUNCTION__);
}
/**
* Initialization code for autonomous mode should go here.
*
* Users should override this method for initialization code which will be
* called each time the robot enters autonomous mode.
*/
void IterativeRobot::AutonomousInit() {
printf("Default %s() method... Overload me!\n", __FUNCTION__);
}
/**
* Initialization code for teleop mode should go here.
*
* Users should override this method for initialization code which will be
* called each time the robot enters teleop mode.
*/
void IterativeRobot::TeleopInit() {
printf("Default %s() method... Overload me!\n", __FUNCTION__);
}
/**
* Initialization code for test mode should go here.
*
* Users should override this method for initialization code which will be
* called each time the robot enters test mode.
*/
void IterativeRobot::TestInit() {
printf("Default %s() method... Overload me!\n", __FUNCTION__);
}
/**
* Periodic code for disabled mode should go here.
*
* Users should override this method for code which will be called periodically
* at a regular rate while the robot is in disabled mode.
*/
void IterativeRobot::DisabledPeriodic() {
static bool firstRun = true;
if (firstRun) {
printf("Default %s() method... Overload me!\n", __FUNCTION__);
firstRun = false;
}
}
/**
* Periodic code for autonomous mode should go here.
*
* Users should override this method for code which will be called periodically
* at a regular rate while the robot is in autonomous mode.
*/
void IterativeRobot::AutonomousPeriodic() {
static bool firstRun = true;
if (firstRun) {
printf("Default %s() method... Overload me!\n", __FUNCTION__);
firstRun = false;
}
}
/**
* Periodic code for teleop mode should go here.
*
* Users should override this method for code which will be called periodically
* at a regular rate while the robot is in teleop mode.
*/
void IterativeRobot::TeleopPeriodic() {
static bool firstRun = true;
if (firstRun) {
printf("Default %s() method... Overload me!\n", __FUNCTION__);
firstRun = false;
}
}
/**
* Periodic code for test mode should go here.
*
* Users should override this method for code which will be called periodically
* at a regular rate while the robot is in test mode.
*/
void IterativeRobot::TestPeriodic() {
static bool firstRun = true;
if (firstRun) {
printf("Default %s() method... Overload me!\n", __FUNCTION__);
firstRun = false;
}
}

60
wpilibc/sim/src/Jaguar.cpp Normal file
View File

@@ -0,0 +1,60 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Jaguar.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "LiveWindow/LiveWindow.h"
/**
* @param channel The PWM channel that the Jaguar is attached to.
*/
Jaguar::Jaguar(uint32_t channel) : SafePWM(channel) {
/*
* Input profile defined by Luminary Micro.
*
* Full reverse ranges from 0.671325ms to 0.6972211ms
* Proportional reverse ranges from 0.6972211ms to 1.4482078ms
* Neutral ranges from 1.4482078ms to 1.5517922ms
* Proportional forward ranges from 1.5517922ms to 2.3027789ms
* Full forward ranges from 2.3027789ms to 2.328675ms
*/
SetBounds(2.31, 1.55, 1.507, 1.454, .697);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetRaw(m_centerPwm);
LiveWindow::GetInstance()->AddActuator("Jaguar", GetChannel(), this);
}
/**
* Set the PWM value.
*
* The PWM value is set using a range of -1.0 to 1.0, appropriately
* scaling the value for the FPGA.
*
* @param speed The speed value between -1.0 and 1.0 to set.
* @param syncGroup Unused interface.
*/
void Jaguar::Set(float speed, uint8_t syncGroup) { SetSpeed(speed); }
/**
* Get the recently set value of the PWM.
*
* @return The most recently set value for the PWM between -1.0 and 1.0.
*/
float Jaguar::Get() const { return GetSpeed(); }
/**
* Common interface for disabling a motor.
*/
void Jaguar::Disable() { SetRaw(kPwmDisabled); }
/**
* Write out the PID value as seen in the PIDOutput base object.
*
* @param output Write out the PWM value as was found in the PIDController
*/
void Jaguar::PIDWrite(float output) { Set(output); }

281
wpilibc/sim/src/Joystick.cpp Normal file
View File

@@ -0,0 +1,281 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Joystick.h"
#include <math.h>
#include <memory>
#include "DriverStation.h"
#include "WPIErrors.h"
const uint32_t Joystick::kDefaultXAxis;
const uint32_t Joystick::kDefaultYAxis;
const uint32_t Joystick::kDefaultZAxis;
const uint32_t Joystick::kDefaultTwistAxis;
const uint32_t Joystick::kDefaultThrottleAxis;
const uint32_t Joystick::kDefaultTriggerButton;
const uint32_t Joystick::kDefaultTopButton;
static Joystick* joysticks[DriverStation::kJoystickPorts];
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.
*/
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;
m_buttons[kTriggerButton] = kDefaultTriggerButton;
m_buttons[kTopButton] = kDefaultTopButton;
}
/**
* Version of the constructor to be called by sub-classes.
*
* This constructor allows the subclass to configure the number of constants
* for axes and buttons.
*
* @param port The port on the driver station that the joystick is
* plugged into.
* @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_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;
}
/**
* Get the X value of the joystick.
*
* This depends on the mapping of the joystick connected to the current port.
*/
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.
*/
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]); }
/**
* 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]); }
/**
* 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]);
}
/**
* Get the value of the axis.
*
* @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);
}
/**
* 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 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;
}
}
/**
* Read the state of the trigger on the joystick.
*
* 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.
* @return The state of the trigger.
*/
bool Joystick::GetTrigger(JoystickHand hand) const {
return GetRawButton(m_buttons[kTriggerButton]);
}
/**
* Read the state of the top button on the joystick.
*
* 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.
* @return The state of the top button.
*/
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;
}
/**
* 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 of each button. The appropriate button is returned as a boolean
* value.
*
* @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);
}
/**
* 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 0; // TODO
}
/**
* Get buttons based on an enumerated type.
*
* The button type will be looked up in the list of buttons and then read.
*
* @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;
}
}
/**
* 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) { return m_axes[axis]; }
/**
* Set the channel associated with a specified 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;
}
/**
* Get the magnitude of the direction vector formed by the joystick's
* current position relative to its origin.
*
* @return The magnitude of the direction vector
*/
float Joystick::GetMagnitude() const {
return sqrt(pow(GetX(), 2) + pow(GetY(), 2));
}
/**
* Get the direction of the vector formed by the joystick and its origin
* in radians.
*
* @return The direction of the vector in radians
*/
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 accessable PI
* constant in C++
*
* @return The direction of the vector in degrees
*/
float Joystick::GetDirectionDegrees() const {
return (180 / acos(-1)) * GetDirectionRadians();
}

145
wpilibc/sim/src/MotorSafetyHelper.cpp Normal file
View File

@@ -0,0 +1,145 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "MotorSafetyHelper.h"
#include "DriverStation.h"
#include "MotorSafety.h"
#include "Timer.h"
#include "WPIErrors.h"
#include <stdio.h>
#include <sstream>
std::set<MotorSafetyHelper*> MotorSafetyHelper::m_helperList;
priority_recursive_mutex MotorSafetyHelper::m_listMutex;
/**
* The constructor for a MotorSafetyHelper object.
*
* The helper object is constructed for every object that wants to implement the
* Motor Safety protocol. The helper object has the code to actually do the
* timing and call the motors Stop() method when the timeout expires. The motor
* object is expected to call the Feed() method whenever the motors value is
* updated.
*
* @param safeObject a pointer to the motor object implementing MotorSafety.
* This is used to call the Stop() method on the motor.
*/
MotorSafetyHelper::MotorSafetyHelper(MotorSafety* safeObject)
: m_safeObject(safeObject) {
m_enabled = false;
m_expiration = DEFAULT_SAFETY_EXPIRATION;
m_stopTime = Timer::GetFPGATimestamp();
std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
m_helperList.insert(this);
}
MotorSafetyHelper::~MotorSafetyHelper() {
std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
m_helperList.erase(this);
}
/**
* Feed the motor safety object.
*
* Resets the timer on this object that is used to do the timeouts.
*/
void MotorSafetyHelper::Feed() {
std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
}
/**
* Set the expiration time for the corresponding motor safety object.
*
* @param expirationTime The timeout value in seconds.
*/
void MotorSafetyHelper::SetExpiration(float expirationTime) {
std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
m_expiration = expirationTime;
}
/**
* Retrieve the timeout value for the corresponding motor safety object.
*
* @return the timeout value in seconds.
*/
float MotorSafetyHelper::GetExpiration() const {
std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
return m_expiration;
}
/**
* Determine if the motor is still operating or has timed out.
*
* @return a true value if the motor is still operating normally and hasn't
* timed out.
*/
bool MotorSafetyHelper::IsAlive() const {
std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
}
/**
* Check if this motor has exceeded its timeout.
*
* This method is called periodically to determine if this motor has exceeded
* its timeout value. If it has, the stop method is called, and the motor is
* shut down until its value is updated again.
*/
void MotorSafetyHelper::Check() {
DriverStation& ds = DriverStation::GetInstance();
if (!m_enabled || ds.IsDisabled() || ds.IsTest()) return;
std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
if (m_stopTime < Timer::GetFPGATimestamp()) {
std::ostringstream desc;
m_safeObject->GetDescription(desc);
desc << "... Output not updated often enough.";
wpi_setWPIErrorWithContext(Timeout, desc.str().c_str());
m_safeObject->StopMotor();
}
}
/**
* Enable/disable motor safety for this device.
*
* Turn on and off the motor safety option for this PWM object.
*
* @param enabled True if motor safety is enforced for this object
*/
void MotorSafetyHelper::SetSafetyEnabled(bool enabled) {
std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
m_enabled = enabled;
}
/**
* Return the state of the motor safety enabled flag.
*
* Return if the motor safety is currently enabled for this devicce.
*
* @return True if motor safety is enforced for this device
*/
bool MotorSafetyHelper::IsSafetyEnabled() const {
std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
return m_enabled;
}
/**
* Check the motors to see if any have timed out.
*
* This static method is called periodically to poll all the motors and stop
* any that have timed out.
*/
void MotorSafetyHelper::CheckMotors() {
std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
for (auto elem : m_helperList) {
elem->Check();
}
}

263
wpilibc/sim/src/Notifier.cpp Normal file
View File

@@ -0,0 +1,263 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Notifier.h"
#include "Timer.h"
#include "Utility.h"
#include "WPIErrors.h"
std::list<Notifier*> Notifier::timerQueue;
priority_recursive_mutex Notifier::queueMutex;
std::atomic<int> Notifier::refcount{0};
std::thread Notifier::m_task;
std::atomic<bool> Notifier::m_stopped(false);
/**
* Create a Notifier for timer event notification.
*
* @param handler The handler is called at the notification time which is set
* using StartSingle or StartPeriodic.
*/
Notifier::Notifier(TimerEventHandler handler) {
if (handler == nullptr)
wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
m_handler = handler;
m_periodic = false;
m_expirationTime = 0;
m_period = 0;
m_queued = false;
{
std::lock_guard<priority_recursive_mutex> sync(queueMutex);
// do the first time intialization of static variables
if (refcount.fetch_add(1) == 0) {
m_task = std::thread(Run);
}
}
}
/**
* Free the resources for a timer event.
*
* All resources will be freed and the timer event will be removed from the
* queue if necessary.
*/
Notifier::~Notifier() {
{
std::lock_guard<priority_recursive_mutex> sync(queueMutex);
DeleteFromQueue();
// Delete the static variables when the last one is going away
if (refcount.fetch_sub(1) == 1) {
m_stopped = true;
m_task.join();
}
}
// Acquire the semaphore; this makes certain that the handler is
// not being executed by the interrupt manager.
std::lock_guard<priority_mutex> lock(m_handlerMutex);
}
/**
* Update the alarm hardware to reflect the current first element in the queue.
*
* Compute the time the next alarm should occur based on the current time and
* the period for the first element in the timer queue.
*
* WARNING: this method does not do synchronization! It must be called from
* somewhere that is taking care of synchronizing access to the queue.
*/
void Notifier::UpdateAlarm() {}
/**
* ProcessQueue is called whenever there is a timer interrupt.
*
* We need to wake up and process the current top item in the timer queue as
* long as its scheduled time is after the current time. Then the item is
* removed or rescheduled (repetitive events) in the queue.
*/
void Notifier::ProcessQueue(uint32_t mask, void* params) {
Notifier* current;
while (true) // keep processing past events until no more
{
{
std::lock_guard<priority_recursive_mutex> sync(queueMutex);
double currentTime = GetClock();
if (timerQueue.empty()) {
break;
}
current = timerQueue.front();
if (current->m_expirationTime > currentTime) {
break; // no more timer events to process
}
// remove next entry before processing it
timerQueue.pop_front();
current->m_queued = false;
if (current->m_periodic) {
// if periodic, requeue the event
// compute when to put into queue
current->InsertInQueue(true);
} else {
// not periodic; removed from queue
current->m_queued = false;
}
// Take handler mutex while holding queue semaphore to make sure
// the handler will execute to completion in case we are being deleted.
current->m_handlerMutex.lock();
}
current->m_handler(); // call the event handler
current->m_handlerMutex.unlock();
}
// reschedule the first item in the queue
std::lock_guard<priority_recursive_mutex> sync(queueMutex);
UpdateAlarm();
}
/**
* Insert this Notifier into the timer queue in right place.
*
* WARNING: this method does not do synchronization! It must be called from
* somewhere that is taking care of synchronizing access to the queue.
*
* @param reschedule If false, the scheduled alarm is based on the curent time
* and UpdateAlarm method is called which will enable the
* alarm if necessary. If true, update the time by adding the
* period (no drift) when rescheduled periodic from
* ProcessQueue.
*
* This ensures that the public methods only update the queue after finishing
* inserting.
*/
void Notifier::InsertInQueue(bool reschedule) {
if (reschedule) {
m_expirationTime += m_period;
} else {
m_expirationTime = GetClock() + m_period;
}
// Attempt to insert new entry into queue
for (auto i = timerQueue.begin(); i != timerQueue.end(); i++) {
if ((*i)->m_expirationTime > m_expirationTime) {
timerQueue.insert(i, this);
m_queued = true;
}
}
/* If the new entry wasn't queued, either the queue was empty or the first
* element was greater than the new entry.
*/
if (!m_queued) {
timerQueue.push_front(this);
if (!reschedule) {
/* Since the first element changed, update alarm, unless we already
* plan to
*/
UpdateAlarm();
}
m_queued = true;
}
}
/**
* Delete this Notifier from the timer queue.
*
* WARNING: this method does not do synchronization! It must be called from
* somewhere that is taking care of synchronizing access to the queue.
*
* Remove this Notifier from the timer queue and adjust the next interrupt time
* to reflect the current top of the queue.
*/
void Notifier::DeleteFromQueue() {
if (m_queued) {
m_queued = false;
wpi_assert(!timerQueue.empty());
if (timerQueue.front() == this) {
// remove the first item in the list - update the alarm
timerQueue.pop_front();
UpdateAlarm();
} else {
timerQueue.remove(this);
}
}
}
/**
* Register for single event notification.
*
* A timer event is queued for a single event after the specified delay.
*
* @param delay Seconds to wait before the handler is called.
*/
void Notifier::StartSingle(double delay) {
std::lock_guard<priority_recursive_mutex> sync(queueMutex);
m_periodic = false;
m_period = delay;
DeleteFromQueue();
InsertInQueue(false);
}
/**
* Register for periodic event notification.
*
* A timer event is queued for periodic event notification. Each time the
* interrupt occurs, the event will be immediately requeued for the same time
* interval.
*
* @param period Period in seconds to call the handler starting one period after
* the call to this method.
*/
void Notifier::StartPeriodic(double period) {
std::lock_guard<priority_recursive_mutex> sync(queueMutex);
m_periodic = true;
m_period = period;
DeleteFromQueue();
InsertInQueue(false);
}
/**
* Stop timer events from occuring.
*
* Stop any repeating timer events from occuring. This will also remove any
* single notification events from the queue. If a timer-based call to the
* registered handler is in progress, this function will block until the
* handler call is complete.
*/
void Notifier::Stop() {
{
std::lock_guard<priority_recursive_mutex> sync(queueMutex);
DeleteFromQueue();
}
// Wait for a currently executing handler to complete before returning from
// Stop()
std::lock_guard<priority_mutex> sync(m_handlerMutex);
}
void Notifier::Run() {
while (!m_stopped) {
Notifier::ProcessQueue(0, nullptr);
bool isEmpty;
{
std::lock_guard<priority_recursive_mutex> sync(queueMutex);
isEmpty = timerQueue.empty();
}
if (!isEmpty) {
double expirationTime;
{
std::lock_guard<priority_recursive_mutex> sync(queueMutex);
expirationTime = timerQueue.front()->m_expirationTime;
}
Wait(expirationTime - GetClock());
} else {
Wait(0.05);
}
}
}

626
wpilibc/sim/src/PIDController.cpp Normal file
View File

@@ -0,0 +1,626 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "PIDController.h"
#include <math.h>
#include "Notifier.h"
#include "PIDOutput.h"
#include "PIDSource.h"
static const std::string kP = "p";
static const std::string kI = "i";
static const std::string kD = "d";
static const std::string kF = "f";
static const std::string kSetpoint = "setpoint";
static const std::string kEnabled = "enabled";
/**
* Allocate a PID object with the given constants for P, I, D.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
* @param period the loop time for doing calculations. This particularly effects
* calculations of the integral and differental terms. The
* default is 50ms.
*/
PIDController::PIDController(float Kp, float Ki, float Kd, PIDSource* source,
PIDOutput* output, float period) {
Initialize(Kp, Ki, Kd, 0.0f, source, output, period);
}
/**
* Allocate a PID object with the given constants for P, I, D.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
* @param period the loop time for doing calculations. This particularly effects
* calculations of the integral and differental terms. The
* default is 50ms.
*/
PIDController::PIDController(float Kp, float Ki, float Kd, float Kf,
PIDSource* source, PIDOutput* output,
float period) {
Initialize(Kp, Ki, Kd, Kf, source, output, period);
}
void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
PIDSource* source, PIDOutput* output,
float period) {
m_table = nullptr;
m_P = Kp;
m_I = Ki;
m_D = Kd;
m_F = Kf;
m_maximumOutput = 1.0;
m_minimumOutput = -1.0;
m_maximumInput = 0;
m_minimumInput = 0;
m_continuous = false;
m_enabled = false;
m_setpoint = 0;
m_prevError = 0;
m_totalError = 0;
m_tolerance = .05;
m_result = 0;
m_pidInput = source;
m_pidOutput = output;
m_period = period;
m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
m_controlLoop->StartPeriodic(m_period);
static int32_t instances = 0;
instances++;
m_toleranceType = kNoTolerance;
}
PIDController::~PIDController() {
if (m_table != nullptr) m_table->RemoveTableListener(this);
}
/**
* Read the input, calculate the output accordingly, and write to the output.
*
* This should only be called by the Notifier.
*/
void PIDController::Calculate() {
bool enabled;
PIDSource* pidInput;
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
if (m_pidInput == 0) return;
if (m_pidOutput == 0) return;
enabled = m_enabled;
pidInput = m_pidInput;
}
if (enabled) {
float input = pidInput->PIDGet();
float result;
PIDOutput* pidOutput;
{
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_error = m_setpoint - input;
if (m_continuous) {
if (fabs(m_error) > (m_maximumInput - m_minimumInput) / 2) {
if (m_error > 0) {
m_error = m_error - m_maximumInput + m_minimumInput;
} else {
m_error = m_error + m_maximumInput - m_minimumInput;
}
}
}
if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
if (m_P != 0) {
double potentialPGain = (m_totalError + m_error) * m_P;
if (potentialPGain < m_maximumOutput) {
if (potentialPGain > m_minimumOutput) {
m_totalError += m_error;
} else {
m_totalError = m_minimumOutput / m_P;
}
} else {
m_totalError = m_maximumOutput / m_P;
}
}
m_result = m_D * m_error + m_P * m_totalError + CalculateFeedForward();
} else {
if (m_I != 0) {
double potentialIGain = (m_totalError + m_error) * m_I;
if (potentialIGain < m_maximumOutput) {
if (potentialIGain > m_minimumOutput) {
m_totalError += m_error;
} else {
m_totalError = m_minimumOutput / m_I;
}
} else {
m_totalError = m_maximumOutput / m_I;
}
}
m_result = m_P * m_error + m_I * m_totalError +
m_D * (m_error - m_prevError) + CalculateFeedForward();
}
m_prevError = m_error;
if (m_result > m_maximumOutput)
m_result = m_maximumOutput;
else if (m_result < m_minimumOutput)
m_result = m_minimumOutput;
pidOutput = m_pidOutput;
result = m_result;
}
pidOutput->PIDWrite(result);
}
}
/**
* Calculate the feed forward term.
*
* Both of the provided feed forward calculations are velocity feed forwards.
* If a different feed forward calculation is desired, the user can override
* this function and provide his or her own. This function does no
* synchronization because the PIDController class only calls it in synchronized
* code, so be careful if calling it oneself.
*
* If a velocity PID controller is being used, the F term should be set to 1
* over the maximum setpoint for the output. If a position PID controller is
* being used, the F term should be set to 1 over the maximum speed for the
* output measured in setpoint units per this controller's update period (see
* the default period in this class's constructor).
*/
double PIDController::CalculateFeedForward() {
if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
return m_F * GetSetpoint();
} else {
double temp = m_F * GetDeltaSetpoint();
m_prevSetpoint = m_setpoint;
m_setpointTimer.Reset();
return temp;
}
}
/**
* Set the PID Controller gain parameters.
*
* Set the proportional, integral, and differential coefficients.
*
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
*/
void PIDController::SetPID(double p, double i, double d) {
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_P = p;
m_I = i;
m_D = d;
}
if (m_table != nullptr) {
m_table->PutNumber("p", m_P);
m_table->PutNumber("i", m_I);
m_table->PutNumber("d", m_D);
}
}
/**
* Set the PID Controller gain parameters.
*
* Set the proportional, integral, and differential coefficients.
*
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
* @param f Feed forward coefficient
*/
void PIDController::SetPID(double p, double i, double d, double f) {
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_P = p;
m_I = i;
m_D = d;
m_F = f;
}
if (m_table != nullptr) {
m_table->PutNumber("p", m_P);
m_table->PutNumber("i", m_I);
m_table->PutNumber("d", m_D);
m_table->PutNumber("f", m_F);
}
}
/**
* Get the Proportional coefficient.
*
* @return proportional coefficient
*/
double PIDController::GetP() const {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_P;
}
/**
* Get the Integral coefficient.
*
* @return integral coefficient
*/
double PIDController::GetI() const {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_I;
}
/**
* Get the Differential coefficient.
*
* @return differential coefficient
*/
double PIDController::GetD() const {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_D;
}
/**
* Get the Feed forward coefficient.
*
* @return Feed forward coefficient
*/
double PIDController::GetF() const {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_F;
}
/**
* Return the current PID result.
*
* This is always centered on zero and constrained the the max and min outs.
*
* @return the latest calculated output
*/
float PIDController::Get() const {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_result;
}
/**
* Set the PID controller to consider the input to be continuous.
*
* Rather then using the max and min in as constraints, it considers them to
* be the same point and automatically calculates the shortest route to
* the setpoint.
*
* @param continuous Set to true turns on continuous, false turns off continuous
*/
void PIDController::SetContinuous(bool continuous) {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_continuous = continuous;
}
/**
* Sets the maximum and minimum values expected from the input.
*
* @param minimumInput the minimum value expected from the input
* @param maximumInput the maximum value expected from the output
*/
void PIDController::SetInputRange(float minimumInput, float maximumInput) {
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
}
SetSetpoint(m_setpoint);
}
/**
* Sets the minimum and maximum values to write.
*
* @param minimumOutput the minimum value to write to the output
* @param maximumOutput the maximum value to write to the output
*/
void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
}
/**
* Set the setpoint for the PIDController.
*
* @param setpoint the desired setpoint
*/
void PIDController::SetSetpoint(float setpoint) {
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
if (m_maximumInput > m_minimumInput) {
if (setpoint > m_maximumInput)
m_setpoint = m_maximumInput;
else if (setpoint < m_minimumInput)
m_setpoint = m_minimumInput;
else
m_setpoint = setpoint;
} else {
m_setpoint = setpoint;
}
}
if (m_table != nullptr) {
m_table->PutNumber("setpoint", m_setpoint);
}
}
/**
* Returns the current setpoint of the PIDController.
*
* @return the current setpoint
*/
double PIDController::GetSetpoint() const {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_setpoint;
}
/**
* Returns the change in setpoint over time of the PIDController.
*
* @return the change in setpoint over time
*/
double PIDController::GetDeltaSetpoint() const {
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
}
/**
* Returns the current difference of the input from the setpoint.
*
* @return the current error
*/
float PIDController::GetError() const {
double pidInput;
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
pidInput = m_pidInput->PIDGet();
}
return GetSetpoint() - pidInput;
}
/**
* Sets what type of input the PID controller will use.
*/
void PIDController::SetPIDSourceType(PIDSourceType pidSource) {
m_pidInput->SetPIDSourceType(pidSource);
}
/**
* Returns the type of input the PID controller is using.
*
* @return the PID controller input type
*/
PIDSourceType PIDController::GetPIDSourceType() const {
return m_pidInput->GetPIDSourceType();
}
/**
* Returns the current average of the error over the past few iterations.
*
* You can specify the number of iterations to average with SetToleranceBuffer()
* (defaults to 1). This is the same value that is used for OnTarget().
*
* @return the average error
*/
float PIDController::GetAvgError() const {
float avgError = 0;
{
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
// Don't divide by zero.
if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
}
return avgError;
}
/**
* Set the percentage error which is considered tolerable for use with
* OnTarget.
*
* @param percent percentage error which is tolerable
*/
void PIDController::SetTolerance(float percent) {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
/**
* Set the percentage error which is considered tolerable for use with
* OnTarget.
*
* @param percent percentage error which is tolerable
*/
void PIDController::SetPercentTolerance(float percent) {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
/**
* Set the absolute error which is considered tolerable for use with
* OnTarget.
*
* @param absTolerance absolute error which is tolerable
*/
void PIDController::SetAbsoluteTolerance(float absTolerance) {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_toleranceType = kAbsoluteTolerance;
m_tolerance = absTolerance;
}
/**
* Set the number of previous error samples to average for tolerancing.
*
* When determining whether a mechanism is on target, the user may want to use a
* rolling average of previous measurements instead of a precise position or
* velocity. This is useful for noisy sensors which return a few erroneous
* measurements when the mechanism is on target. However, the mechanism will
* not register as on target for at least the specified bufLength cycles.
* @param bufLength Number of previous cycles to average. Defaults to 1.
*/
void PIDController::SetToleranceBuffer(unsigned bufLength) {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_bufLength = bufLength;
// Cut the buffer down to size if needed.
while (m_buf.size() > bufLength) {
m_bufTotal -= m_buf.front();
m_buf.pop();
}
}
/**
* Return true if the error is within the percentage of the total input range,
* determined by SetTolerance.
*
* This asssumes that the maximum and minimum input were set using SetInput.
* Currently this just reports on target as the actual value passes through the
* setpoint. Ideally it should be based on being within the tolerance for some
* period of time.
*/
bool PIDController::OnTarget() const {
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
if (m_buf.size() == 0) return false;
double error = GetError();
switch (m_toleranceType) {
case kPercentTolerance:
return fabs(error) <
m_tolerance / 100 * (m_maximumInput - m_minimumInput);
break;
case kAbsoluteTolerance:
return fabs(error) < m_tolerance;
break;
case kNoTolerance: // TODO: this case needs an error
return false;
}
return false;
}
/**
* Begin running the PIDController.
*/
void PIDController::Enable() {
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_enabled = true;
}
if (m_table != nullptr) {
m_table->PutBoolean("enabled", true);
}
}
/**
* Stop running the PIDController, this sets the output to zero before stopping.
*/
void PIDController::Disable() {
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_pidOutput->PIDWrite(0);
m_enabled = false;
}
if (m_table != nullptr) {
m_table->PutBoolean("enabled", false);
}
}
/**
* Return true if PIDController is enabled.
*/
bool PIDController::IsEnabled() const {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_enabled;
}
/**
* Reset the previous error,, the integral term, and disable the controller.
*/
void PIDController::Reset() {
Disable();
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_prevError = 0;
m_totalError = 0;
m_result = 0;
}
std::string PIDController::GetSmartDashboardType() const {
return "PIDController";
}
void PIDController::InitTable(std::shared_ptr<ITable> table) {
if (m_table != nullptr) m_table->RemoveTableListener(this);
m_table = table;
if (m_table != nullptr) {
m_table->PutNumber(kP, GetP());
m_table->PutNumber(kI, GetI());
m_table->PutNumber(kD, GetD());
m_table->PutNumber(kF, GetF());
m_table->PutNumber(kSetpoint, GetSetpoint());
m_table->PutBoolean(kEnabled, IsEnabled());
m_table->AddTableListener(this, false);
}
}
std::shared_ptr<ITable> PIDController::GetTable() const { return m_table; }
void PIDController::ValueChanged(ITable* source, llvm::StringRef key,
std::shared_ptr<nt::Value> value, bool isNew) {
if (key == kP || key == kI || key == kD || key == kF) {
if (m_P != m_table->GetNumber(kP, 0.0) ||
m_I != m_table->GetNumber(kI, 0.0) ||
m_D != m_table->GetNumber(kD, 0.0) ||
m_F != m_table->GetNumber(kF, 0.0)) {
SetPID(m_table->GetNumber(kP, 0.0), m_table->GetNumber(kI, 0.0),
m_table->GetNumber(kD, 0.0), m_table->GetNumber(kF, 0.0));
}
} else if (key == kSetpoint && value->IsDouble() &&
m_setpoint != value->GetDouble()) {
SetSetpoint(value->GetDouble());
} else if (key == kEnabled && value->IsBoolean() &&
m_enabled != value->GetBoolean()) {
if (value->GetBoolean()) {
Enable();
} else {
Disable();
}
}
}
void PIDController::UpdateTable() {}
void PIDController::StartLiveWindowMode() { Disable(); }
void PIDController::StopLiveWindowMode() {}

240
wpilibc/sim/src/PWM.cpp Normal file
View File

@@ -0,0 +1,240 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "PWM.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "Utility.h"
#include "WPIErrors.h"
const float PWM::kDefaultPwmPeriod = 5.05;
const float PWM::kDefaultPwmCenter = 1.5;
const int32_t PWM::kDefaultPwmStepsDown = 1000;
const int32_t PWM::kPwmDisabled = 0;
/**
* Allocate a PWM given a channel number.
*
* Checks channel value range and allocates the appropriate channel.
* The allocation is only done to help users ensure that they don't double
* assign channels.
*
* @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP
* port
*/
PWM::PWM(uint32_t channel) {
char buf[64];
if (!CheckPWMChannel(channel)) {
snprintf(buf, 64, "PWM Channel %d", channel);
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
return;
}
sprintf(buf, "pwm/%d", channel);
impl = new SimContinuousOutput(buf);
m_channel = channel;
m_eliminateDeadband = false;
m_centerPwm = kPwmDisabled; // In simulation, the same thing.
}
PWM::~PWM() {
if (m_table != nullptr) m_table->RemoveTableListener(this);
}
/**
* Optionally eliminate the deadband from a speed controller.
*
* @param eliminateDeadband If true, set the motor curve on the Jaguar to
* eliminate the deadband in the middle of the range.
* Otherwise, keep the full range without modifying
* any values.
*/
void PWM::EnableDeadbandElimination(bool eliminateDeadband) {
m_eliminateDeadband = eliminateDeadband;
}
/**
* Set the bounds on the PWM values.
*
* This sets the bounds on the PWM values for a particular each type of
* controller. The values determine the upper and lower speeds as well as the
* deadband bracket.
*
* @param max The Minimum pwm value
* @param deadbandMax The high end of the deadband range
* @param center The center speed (off)
* @param deadbandMin The low end of the deadband range
* @param min The minimum pwm value
*/
void PWM::SetBounds(int32_t max, int32_t deadbandMax, int32_t center,
int32_t deadbandMin, int32_t min) {
// Nothing to do in simulation.
}
/**
* Set the bounds on the PWM pulse widths.
*
* This sets the bounds on the PWM values for a particular type of controller.
* The values determine the upper and lower speeds as well as the deadband
* bracket.
*
* @param max The max PWM pulse width in ms
* @param deadbandMax The high end of the deadband range pulse width in ms
* @param center The center (off) pulse width in ms
* @param deadbandMin The low end of the deadband pulse width in ms
* @param min The minimum pulse width in ms
*/
void PWM::SetBounds(double max, double deadbandMax, double center,
double deadbandMin, double min) {
// Nothing to do in simulation.
}
/**
* Set the PWM value based on a position.
*
* This is intended to be used by servos.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinNegativePwm() called.
*
* @param pos The position to set the servo between 0.0 and 1.0.
*/
void PWM::SetPosition(float pos) {
if (pos < 0.0) {
pos = 0.0;
} else if (pos > 1.0) {
pos = 1.0;
}
impl->Set(pos);
}
/**
* Get the PWM value in terms of a position.
*
* This is intended to be used by servos.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinNegativePwm() called.
*
* @return The position the servo is set to between 0.0 and 1.0.
*/
float PWM::GetPosition() const {
float value = impl->Get();
if (value < 0.0) {
return 0.0;
} else if (value > 1.0) {
return 1.0;
} else {
return value;
}
}
/**
* Set the PWM value based on a speed.
*
* This is intended to be used by speed controllers.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinPositivePwm() called.
* @pre SetCenterPwm() called.
* @pre SetMaxNegativePwm() called.
* @pre SetMinNegativePwm() called.
*
* @param speed The speed to set the speed controller between -1.0 and 1.0.
*/
void PWM::SetSpeed(float speed) {
// clamp speed to be in the range 1.0 >= speed >= -1.0
if (speed < -1.0) {
speed = -1.0;
} else if (speed > 1.0) {
speed = 1.0;
}
impl->Set(speed);
}
/**
* Get the PWM value in terms of speed.
*
* This is intended to be used by speed controllers.
*
* @pre SetMaxPositivePwm() called.
* @pre SetMinPositivePwm() called.
* @pre SetMaxNegativePwm() called.
* @pre SetMinNegativePwm() called.
*
* @return The most recently set speed between -1.0 and 1.0.
*/
float PWM::GetSpeed() const {
float value = impl->Get();
if (value > 1.0) {
return 1.0;
} else if (value < -1.0) {
return -1.0;
} else {
return value;
}
}
/**
* Set the PWM value directly to the hardware.
*
* Write a raw value to a PWM channel.
*
* @param value Raw PWM value.
*/
void PWM::SetRaw(unsigned short value) {
wpi_assert(value == kPwmDisabled);
impl->Set(0);
}
/**
* Slow down the PWM signal for old devices.
*
* @param mult The period multiplier to apply to this channel
*/
void PWM::SetPeriodMultiplier(PeriodMultiplier mult) {
// Do nothing in simulation.
}
void PWM::ValueChanged(ITable* source, llvm::StringRef key,
std::shared_ptr<nt::Value> value, bool isNew) {
if (!value->IsDouble()) return;
SetSpeed(value->GetDouble());
}
void PWM::UpdateTable() {
if (m_table != nullptr) {
m_table->PutNumber("Value", GetSpeed());
}
}
void PWM::StartLiveWindowMode() {
SetSpeed(0);
if (m_table != nullptr) {
m_table->AddTableListener("Value", this, true);
}
}
void PWM::StopLiveWindowMode() {
SetSpeed(0);
if (m_table != nullptr) {
m_table->RemoveTableListener(this);
}
}
std::string PWM::GetSmartDashboardType() const { return "Speed Controller"; }
void PWM::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> PWM::GetTable() const { return m_table; }

242
wpilibc/sim/src/Relay.cpp Normal file
View File

@@ -0,0 +1,242 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Relay.h"
#include "MotorSafetyHelper.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "LiveWindow/LiveWindow.h"
#include "WPIErrors.h"
/**
* Relay constructor given a channel.
*
* This code initializes the relay and reserves all resources that need to be
* locked. Initially the relay is set to both lines at 0v.
*
* @param channel The channel number (0-3).
* @param direction The direction that the Relay object will control.
*/
Relay::Relay(uint32_t channel, Relay::Direction direction)
: m_channel(channel), m_direction(direction) {
char buf[64];
if (!SensorBase::CheckRelayChannel(m_channel)) {
snprintf(buf, 64, "Relay Channel %d", m_channel);
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
return;
}
m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
m_safetyHelper->SetSafetyEnabled(false);
sprintf(buf, "relay/%d", m_channel);
impl = new SimContinuousOutput(buf); // TODO: Allow two different relays
// (targetting the different halves of a
// relay) to be combined to control one
// motor.
LiveWindow::GetInstance()->AddActuator("Relay", 1, m_channel, this);
go_pos = go_neg = false;
}
/**
* Free the resource associated with a relay.
*
* The relay channels are set to free and the relay output is turned off.
*/
Relay::~Relay() {
impl->Set(0);
if (m_table != nullptr) m_table->RemoveTableListener(this);
}
/**
* Set the relay state.
*
* Valid values depend on which directions of the relay are controlled by the
* object.
*
* When set to kBothDirections, the relay can be any of the four states:
* 0v-0v, 0v-12v, 12v-0v, 12v-12v
*
* When set to kForwardOnly or kReverseOnly, you can specify the constant for
* the direction or you can simply specify kOff and kOn. Using only kOff and
* kOn is recommended.
*
* @param value The state to set the relay.
*/
void Relay::Set(Relay::Value value) {
switch (value) {
case kOff:
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
go_pos = false;
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
go_neg = false;
}
break;
case kOn:
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
go_pos = true;
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
go_neg = true;
}
break;
case kForward:
if (m_direction == kReverseOnly) {
wpi_setWPIError(IncompatibleMode);
break;
}
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
go_pos = true;
}
if (m_direction == kBothDirections) {
go_neg = false;
}
break;
case kReverse:
if (m_direction == kForwardOnly) {
wpi_setWPIError(IncompatibleMode);
break;
}
if (m_direction == kBothDirections) {
go_pos = false;
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
go_neg = true;
}
break;
}
impl->Set((go_pos ? 1 : 0) + (go_neg ? -1 : 0));
}
/**
* Get the Relay State
*
* Gets the current state of the relay.
*
* When set to kForwardOnly or kReverseOnly, value is returned as kOn/kOff not
* kForward/kReverse (per the recommendation in Set).
*
* @return The current state of the relay as a Relay::Value
*/
Relay::Value Relay::Get() const {
// TODO: Don't assume that the go_pos and go_neg fields are correct?
if ((go_pos || m_direction == kReverseOnly) &&
(go_neg || m_direction == kForwardOnly)) {
return kOn;
} else if (go_pos) {
return kForward;
} else if (go_neg) {
return kReverse;
} else {
return kOff;
}
}
uint32_t Relay::GetChannel() const { return m_channel; }
/**
* Set the expiration time for the Relay object.
*
* @param timeout The timeout (in seconds) for this relay object
*/
void Relay::SetExpiration(float timeout) {
m_safetyHelper->SetExpiration(timeout);
}
/**
* Return the expiration time for the relay object.
*
* @return The expiration time value.
*/
float Relay::GetExpiration() const { return m_safetyHelper->GetExpiration(); }
/**
* Check if the relay object is currently alive or stopped due to a timeout.
*
* @return a bool value that is true if the motor has NOT timed out and should
* still be running.
*/
bool Relay::IsAlive() const { return m_safetyHelper->IsAlive(); }
/**
* Stop the motor associated with this PWM object.
*
* This is called by the MotorSafetyHelper object when it has a timeout for this
* relay and needs to stop it from running.
*/
void Relay::StopMotor() { Set(kOff); }
/**
* Enable/disable motor safety for this device
*
* Turn on and off the motor safety option for this relay object.
*
* @param enabled True if motor safety is enforced for this object
*/
void Relay::SetSafetyEnabled(bool enabled) {
m_safetyHelper->SetSafetyEnabled(enabled);
}
/**
* Check if motor safety is enabled for this object.
*
* @return True if motor safety is enforced for this object
*/
bool Relay::IsSafetyEnabled() const {
return m_safetyHelper->IsSafetyEnabled();
}
void Relay::GetDescription(std::ostringstream& desc) const {
desc << "Relay " << GetChannel();
}
void Relay::ValueChanged(ITable* source, llvm::StringRef key,
std::shared_ptr<nt::Value> value, bool isNew) {
if (!value->IsString()) return;
if (value->GetString() == "Off")
Set(kOff);
else if (value->GetString() == "Forward")
Set(kForward);
else if (value->GetString() == "Reverse")
Set(kReverse);
}
void Relay::UpdateTable() {
if (m_table != nullptr) {
if (Get() == kOn) {
m_table->PutString("Value", "On");
} else if (Get() == kForward) {
m_table->PutString("Value", "Forward");
} else if (Get() == kReverse) {
m_table->PutString("Value", "Reverse");
} else {
m_table->PutString("Value", "Off");
}
}
}
void Relay::StartLiveWindowMode() {
if (m_table != nullptr) {
m_table->AddTableListener("Value", this, true);
}
}
void Relay::StopLiveWindowMode() {
if (m_table != nullptr) {
m_table->RemoveTableListener(this);
}
}
std::string Relay::GetSmartDashboardType() const { return "Relay"; }
void Relay::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> Relay::GetTable() const { return m_table; }

89
wpilibc/sim/src/RobotBase.cpp Normal file
View File

@@ -0,0 +1,89 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "RobotBase.h"
#include "RobotState.h"
#include "Utility.h"
#include <cstring>
RobotBase* RobotBase::m_instance = nullptr;
void RobotBase::setInstance(RobotBase* robot) {
wpi_assert(m_instance == nullptr);
m_instance = robot;
}
RobotBase& RobotBase::getInstance() { return *m_instance; }
/**
* Constructor for a generic robot program.
*
* User code should be placed in the constuctor that runs before the Autonomous
* or Operator Control period starts. The constructor will run to completion
* before Autonomous is entered.
*
* This must be used to ensure that the communications code starts. In the
* future it would be nice to put this code into it's own task that loads on
* boot so ensure that it runs.
*/
RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
RobotState::SetImplementation(DriverStation::GetInstance());
time_sub = MainNode::Subscribe("~/time", &wpilib::internal::time_callback);
}
/**
* Determine if the Robot is currently enabled.
*
* @return True if the Robot is currently enabled by the field controls.
*/
bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); }
/**
* Determine if the Robot is currently disabled.
*
* @return True if the Robot is currently disabled by the field controls.
*/
bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); }
/**
* Determine if the robot is currently in Autnomous mode.
*
* @return True if the robot is currently operating Autonomously as determined
* by the field controls.
*/
bool RobotBase::IsAutonomous() const { return m_ds.IsAutonomous(); }
/**
* Determine if the robot is currently in Operator Control mode.
*
* @return True if the robot is currently operating in Tele-Op mode as
* determined by the field controls.
*/
bool RobotBase::IsOperatorControl() const { return m_ds.IsOperatorControl(); }
/**
* Determine if the robot is currently in Test mode.
*
* @return True if the robot is currently running tests as determined by the
* field controls.
*/
bool RobotBase::IsTest() const { return m_ds.IsTest(); }
/**
* This class exists for the sole purpose of getting its destructor called when
* the module unloads.
*
* Before the module is done unloading, we need to delete the RobotBase derived
* singleton. This should delete the other remaining singletons that were
* registered. This should also stop all tasks that are using the Task class.
*/
class RobotDeleter {
public:
~RobotDeleter() { delete &RobotBase::getInstance(); }
};
static RobotDeleter g_robotDeleter;

750
wpilibc/sim/src/RobotDrive.cpp Normal file
View File

@@ -0,0 +1,750 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "RobotDrive.h"
//#include "CANJaguar.h"
#include <math.h>
#include "GenericHID.h"
#include "Joystick.h"
#include "Talon.h"
#include "Utility.h"
#include "WPIErrors.h"
#undef max
#include <algorithm>
const int32_t RobotDrive::kMaxNumberOfMotors;
static auto make_shared_nodelete(SpeedController* ptr) {
return std::shared_ptr<SpeedController>(ptr, NullDeleter<SpeedController>());
}
/*
* Driving functions.
*
* These functions provide an interface to multiple motors that is used for C
* programming.
* The Drive(speed, direction) function is the main part of the set that makes
* it easy to set speeds and direction independently in one call.
*/
/**
* Common function to initialize all the robot drive constructors.
* Create a motor safety object (the real reason for the common code) and
* initialize all the motor assignments. The default timeout is set for the
* robot drive.
*/
void RobotDrive::InitRobotDrive() {
// FIXME: m_safetyHelper = new MotorSafetyHelper(this);
// FIXME: m_safetyHelper->SetSafetyEnabled(true);
}
/**
* Constructor for RobotDrive with 2 motors specified with channel numbers.
*
* Set up parameters for a two wheel drive system where the
* left and right motor pwm channels are specified in the call.
* This call assumes Talons for controlling the motors.
*
* @param leftMotorChannel The PWM channel number that drives the left motor.
* @param rightMotorChannel The PWM channel number that drives the right motor.
*/
RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel) {
InitRobotDrive();
m_rearLeftMotor = std::make_shared<Talon>(leftMotorChannel);
m_rearRightMotor = std::make_shared<Talon>(rightMotorChannel);
SetLeftRightMotorOutputs(0.0, 0.0);
m_deleteSpeedControllers = true;
}
/**
* Constructor for RobotDrive with 4 motors specified with channel numbers.
*
* Set up parameters for a four wheel drive system where all four motor
* pwm channels are specified in the call.
* This call assumes Talons for controlling the motors.
*
* @param frontLeftMotor Front left motor channel number
* @param rearLeftMotor Rear Left motor channel number
* @param frontRightMotor Front right motor channel number
* @param rearRightMotor Rear Right motor channel number
*/
RobotDrive::RobotDrive(uint32_t frontLeftMotor, uint32_t rearLeftMotor,
uint32_t frontRightMotor, uint32_t rearRightMotor) {
InitRobotDrive();
m_rearLeftMotor = std::make_shared<Talon>(rearLeftMotor);
m_rearRightMotor = std::make_shared<Talon>(rearRightMotor);
m_frontLeftMotor = std::make_shared<Talon>(frontLeftMotor);
m_frontRightMotor = std::make_shared<Talon>(frontRightMotor);
SetLeftRightMotorOutputs(0.0, 0.0);
m_deleteSpeedControllers = true;
}
/**
* Constructor for RobotDrive with 2 motors specified as SpeedController
* objects.
*
* The SpeedController version of the constructor enables programs to use the
* RobotDrive classes with subclasses of the SpeedController objects, for
* example, versions with ramping or reshaping of the curve to suit motor bias
* or deadband elimination.
*
* @param leftMotor The left SpeedController object used to drive the robot.
* @param rightMotor the right SpeedController object used to drive the robot.
*/
RobotDrive::RobotDrive(SpeedController* leftMotor,
SpeedController* rightMotor) {
InitRobotDrive();
if (leftMotor == nullptr || rightMotor == nullptr) {
wpi_setWPIError(NullParameter);
m_rearLeftMotor = m_rearRightMotor = nullptr;
return;
}
m_rearLeftMotor = make_shared_nodelete(leftMotor);
m_rearRightMotor = make_shared_nodelete(rightMotor);
}
// TODO: Change to rvalue references & move syntax.
RobotDrive::RobotDrive(SpeedController& leftMotor,
SpeedController& rightMotor) {
InitRobotDrive();
m_rearLeftMotor = make_shared_nodelete(&leftMotor);
m_rearRightMotor = make_shared_nodelete(&rightMotor);
}
RobotDrive::RobotDrive(std::shared_ptr<SpeedController> leftMotor,
std::shared_ptr<SpeedController> rightMotor) {
InitRobotDrive();
if (leftMotor == nullptr || rightMotor == nullptr) {
wpi_setWPIError(NullParameter);
m_rearLeftMotor = m_rearRightMotor = nullptr;
return;
}
m_rearLeftMotor = leftMotor;
m_rearRightMotor = rightMotor;
}
/**
* Constructor for RobotDrive with 4 motors specified as SpeedController
* objects.
*
* Speed controller input version of RobotDrive (see previous comments).
*
* @param rearLeftMotor The back left SpeedController object used to drive the
* robot.
* @param frontLeftMotor The front left SpeedController object used to drive
* the robot
* @param rearRightMotor The back right SpeedController object used to drive
* the robot.
* @param frontRightMotor The front right SpeedController object used to drive
* the robot.
*/
RobotDrive::RobotDrive(SpeedController* frontLeftMotor,
SpeedController* rearLeftMotor,
SpeedController* frontRightMotor,
SpeedController* rearRightMotor) {
InitRobotDrive();
if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
frontRightMotor == nullptr || rearRightMotor == nullptr) {
wpi_setWPIError(NullParameter);
return;
}
m_frontLeftMotor = make_shared_nodelete(frontLeftMotor);
m_rearLeftMotor = make_shared_nodelete(rearLeftMotor);
m_frontRightMotor = make_shared_nodelete(frontRightMotor);
m_rearRightMotor = make_shared_nodelete(rearRightMotor);
}
RobotDrive::RobotDrive(SpeedController& frontLeftMotor,
SpeedController& rearLeftMotor,
SpeedController& frontRightMotor,
SpeedController& rearRightMotor) {
InitRobotDrive();
m_frontLeftMotor = make_shared_nodelete(&frontLeftMotor);
m_rearLeftMotor = make_shared_nodelete(&rearLeftMotor);
m_frontRightMotor = make_shared_nodelete(&frontRightMotor);
m_rearRightMotor = make_shared_nodelete(&rearRightMotor);
}
RobotDrive::RobotDrive(std::shared_ptr<SpeedController> frontLeftMotor,
std::shared_ptr<SpeedController> rearLeftMotor,
std::shared_ptr<SpeedController> frontRightMotor,
std::shared_ptr<SpeedController> rearRightMotor) {
InitRobotDrive();
if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
frontRightMotor == nullptr || rearRightMotor == nullptr) {
wpi_setWPIError(NullParameter);
return;
}
m_frontLeftMotor = frontLeftMotor;
m_rearLeftMotor = rearLeftMotor;
m_frontRightMotor = frontRightMotor;
m_rearRightMotor = rearRightMotor;
}
/**
* Drive the motors at "outputMagnitude" and "curve".
* Both outputMagnitude and curve are -1.0 to +1.0 values, where 0.0 represents
* stopped and not turning. curve < 0 will turn left and curve > 0 will turn
* right.
*
* The algorithm for steering provides a constant turn radius for any normal
* speed range, both forward and backward. Increasing m_sensitivity causes
* sharper turns for fixed values of curve.
*
* This function will most likely be used in an autonomous routine.
*
* @param outputMagnitude The speed setting for the outside wheel in a turn,
* forward or backwards, +1 to -1.
* @param curve The rate of turn, constant for different forward
* speeds. Set curve < 0 for left turn or curve > 0 for
* right turn. Set curve = e^(-r/w) to get a turn radius
* r for wheelbase w of your robot. Conversely, turn
* radius r = -ln(curve)*w for a given value of curve and
* wheelbase w.
*/
void RobotDrive::Drive(float outputMagnitude, float curve) {
float leftOutput, rightOutput;
static bool reported = false;
if (!reported) {
reported = true;
}
if (curve < 0) {
float value = log(-curve);
float ratio = (value - m_sensitivity) / (value + m_sensitivity);
if (ratio == 0) ratio = .0000000001;
leftOutput = outputMagnitude / ratio;
rightOutput = outputMagnitude;
} else if (curve > 0) {
float value = log(curve);
float ratio = (value - m_sensitivity) / (value + m_sensitivity);
if (ratio == 0) ratio = .0000000001;
leftOutput = outputMagnitude;
rightOutput = outputMagnitude / ratio;
} else {
leftOutput = outputMagnitude;
rightOutput = outputMagnitude;
}
SetLeftRightMotorOutputs(leftOutput, rightOutput);
}
/**
* Provide tank steering using the stored robot configuration.
*
* Drive the robot using two joystick inputs. The Y-axis will be selected from
* each Joystick object.
*
* @param leftStick The joystick to control the left side of the robot.
* @param rightStick The joystick to control the right side of the robot.
*/
void RobotDrive::TankDrive(GenericHID* leftStick, GenericHID* rightStick,
bool squaredInputs) {
if (leftStick == nullptr || rightStick == nullptr) {
wpi_setWPIError(NullParameter);
return;
}
TankDrive(leftStick->GetY(), rightStick->GetY(), squaredInputs);
}
void RobotDrive::TankDrive(GenericHID& leftStick, GenericHID& rightStick,
bool squaredInputs) {
TankDrive(leftStick.GetY(), rightStick.GetY(), squaredInputs);
}
/**
* Provide tank steering using the stored robot configuration.
*
* This function lets you pick the axis to be used on each Joystick object for
* the left and right sides of the robot.
*
* @param leftStick The Joystick object to use for the left side of the robot.
* @param leftAxis The axis to select on the left side Joystick object.
* @param rightStick The Joystick object to use for the right side of the robot.
* @param rightAxis The axis to select on the right side Joystick object.
*/
void RobotDrive::TankDrive(GenericHID* leftStick, uint32_t leftAxis,
GenericHID* rightStick, uint32_t rightAxis,
bool squaredInputs) {
if (leftStick == nullptr || rightStick == nullptr) {
wpi_setWPIError(NullParameter);
return;
}
TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis),
squaredInputs);
}
void RobotDrive::TankDrive(GenericHID& leftStick, uint32_t leftAxis,
GenericHID& rightStick, uint32_t rightAxis,
bool squaredInputs) {
TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis),
squaredInputs);
}
/**
* Provide tank steering using the stored robot configuration.
*
* This function lets you directly provide joystick values from any source.
*
* @param leftValue The value of the left stick.
* @param rightValue The value of the right stick.
*/
void RobotDrive::TankDrive(float leftValue, float rightValue,
bool squaredInputs) {
static bool reported = false;
if (!reported) {
reported = true;
}
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
leftValue = Limit(leftValue);
rightValue = Limit(rightValue);
if (squaredInputs) {
if (leftValue >= 0.0) {
leftValue = (leftValue * leftValue);
} else {
leftValue = -(leftValue * leftValue);
}
if (rightValue >= 0.0) {
rightValue = (rightValue * rightValue);
} else {
rightValue = -(rightValue * rightValue);
}
}
SetLeftRightMotorOutputs(leftValue, rightValue);
}
/**
* Arcade drive implements single stick driving.
*
* Given a single Joystick, the class assumes the Y axis for the move value and
* the X axis for the rotate value.
* (Should add more information here regarding the way that arcade drive works.)
*
* @param stick The joystick to use for Arcade single-stick driving.
* The Y-axis will be selected for forwards/backwards and
* the X-axis will be selected for rotation rate.
* @param squaredInputs If true, the sensitivity will be increased for small
* values
*/
void RobotDrive::ArcadeDrive(GenericHID* stick, bool squaredInputs) {
// simply call the full-featured ArcadeDrive with the appropriate values
ArcadeDrive(stick->GetY(), stick->GetX(), squaredInputs);
}
/**
* Arcade drive implements single stick driving.
*
* Given a single Joystick, the class assumes the Y axis for the move value and
* the X axis for the rotate value.
* (Should add more information here regarding the way that arcade drive works.)
*
* @param stick The joystick to use for Arcade single-stick driving.
* The Y-axis will be selected for forwards/backwards and
* the X-axis will be selected for rotation rate.
* @param squaredInputs If true, the sensitivity will be increased for small
* values
*/
void RobotDrive::ArcadeDrive(GenericHID& stick, bool squaredInputs) {
// simply call the full-featured ArcadeDrive with the appropriate values
ArcadeDrive(stick.GetY(), stick.GetX(), squaredInputs);
}
/**
* Arcade drive implements single stick driving.
*
* Given two joystick instances and two axis, compute the values to send to
* either two or four motors.
*
* @param moveStick The Joystick object that represents the forward/backward
* direction
* @param moveAxis The axis on the moveStick object to use for
* forwards/backwards (typically Y_AXIS)
* @param rotateStick The Joystick object that represents the rotation value
* @param rotateAxis The axis on the rotation object to use for the rotate
* right/left (typically X_AXIS)
* @param squaredInputs Setting this parameter to true increases the sensitivity
* at lower speeds
*/
void RobotDrive::ArcadeDrive(GenericHID* moveStick, uint32_t moveAxis,
GenericHID* rotateStick, uint32_t rotateAxis,
bool squaredInputs) {
float moveValue = moveStick->GetRawAxis(moveAxis);
float rotateValue = rotateStick->GetRawAxis(rotateAxis);
ArcadeDrive(moveValue, rotateValue, squaredInputs);
}
/**
* Arcade drive implements single stick driving.
*
* Given two joystick instances and two axis, compute the values to send to
* either two or four motors.
*
* @param moveStick The Joystick object that represents the forward/backward
* direction
* @param moveAxis The axis on the moveStick object to use for
* forwards/backwards (typically Y_AXIS)
* @param rotateStick The Joystick object that represents the rotation value
* @param rotateAxis The axis on the rotation object to use for the rotate
* right/left (typically X_AXIS)
* @param squaredInputs Setting this parameter to true increases the sensitivity
* at lower speeds
*/
void RobotDrive::ArcadeDrive(GenericHID& moveStick, uint32_t moveAxis,
GenericHID& rotateStick, uint32_t rotateAxis,
bool squaredInputs) {
float moveValue = moveStick.GetRawAxis(moveAxis);
float rotateValue = rotateStick.GetRawAxis(rotateAxis);
ArcadeDrive(moveValue, rotateValue, squaredInputs);
}
/**
* Arcade drive implements single stick driving.
*
* This function lets you directly provide joystick values from any source.
*
* @param moveValue The value to use for fowards/backwards
* @param rotateValue The value to use for the rotate right/left
* @param squaredInputs If set, increases the sensitivity at low speeds
*/
void RobotDrive::ArcadeDrive(float moveValue, float rotateValue,
bool squaredInputs) {
static bool reported = false;
if (!reported) {
reported = true;
}
// local variables to hold the computed PWM values for the motors
float leftMotorOutput;
float rightMotorOutput;
moveValue = Limit(moveValue);
rotateValue = Limit(rotateValue);
if (squaredInputs) {
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
if (moveValue >= 0.0) {
moveValue = (moveValue * moveValue);
} else {
moveValue = -(moveValue * moveValue);
}
if (rotateValue >= 0.0) {
rotateValue = (rotateValue * rotateValue);
} else {
rotateValue = -(rotateValue * rotateValue);
}
}
if (moveValue > 0.0) {
if (rotateValue > 0.0) {
leftMotorOutput = moveValue - rotateValue;
rightMotorOutput = std::max(moveValue, rotateValue);
} else {
leftMotorOutput = std::max(moveValue, -rotateValue);
rightMotorOutput = moveValue + rotateValue;
}
} else {
if (rotateValue > 0.0) {
leftMotorOutput = -std::max(-moveValue, rotateValue);
rightMotorOutput = moveValue + rotateValue;
} else {
leftMotorOutput = moveValue - rotateValue;
rightMotorOutput = -std::max(-moveValue, -rotateValue);
}
}
SetLeftRightMotorOutputs(leftMotorOutput, rightMotorOutput);
}
/**
* Drive method for Mecanum wheeled robots.
*
* A method for driving with Mecanum wheeled robots. There are 4 wheels
* on the robot, arranged so that the front and back wheels are toed in 45
* degrees. When looking at the wheels from the top, the roller axles should
* form an X across the robot.
*
* This is designed to be directly driven by joystick axes.
*
* @param x The speed that the robot should drive in the X direction.
* [-1.0..1.0]
* @param y The speed that the robot should drive in the Y direction.
* This input is inverted to match the forward == -1.0 that
* joysticks produce. [-1.0..1.0]
* @param rotation The rate of rotation for the robot that is completely
* independent of the translation. [-1.0..1.0]
* @param gyroAngle The current angle reading from the gyro. Use this to
* implement field-oriented controls.
*/
void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation,
float gyroAngle) {
static bool reported = false;
if (!reported) {
reported = true;
}
double xIn = x;
double yIn = y;
// Negate y for the joystick.
yIn = -yIn;
// Compenstate for gyro angle.
RotateVector(xIn, yIn, gyroAngle);
double wheelSpeeds[kMaxNumberOfMotors];
wheelSpeeds[kFrontLeftMotor] = xIn + yIn + rotation;
wheelSpeeds[kFrontRightMotor] = -xIn + yIn - rotation;
wheelSpeeds[kRearLeftMotor] = -xIn + yIn + rotation;
wheelSpeeds[kRearRightMotor] = xIn + yIn - rotation;
Normalize(wheelSpeeds);
uint8_t syncGroup = 0x80;
m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] *
m_invertedMotors[kFrontLeftMotor] * m_maxOutput,
syncGroup);
m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] *
m_invertedMotors[kFrontRightMotor] * m_maxOutput,
syncGroup);
m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] *
m_invertedMotors[kRearLeftMotor] * m_maxOutput,
syncGroup);
m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] *
m_invertedMotors[kRearRightMotor] * m_maxOutput,
syncGroup);
// CANJaguar::UpdateSyncGroup(syncGroup);
// FIXME: m_safetyHelper->Feed();
}
/**
* Drive method for Mecanum wheeled robots.
*
* A method for driving with Mecanum wheeled robots. There are 4 wheels
* on the robot, arranged so that the front and back wheels are toed in 45
* degrees. When looking at the wheels from the top, the roller axles should
* form an X across the robot.
*
* @param magnitude The speed that the robot should drive in a given direction.
* [-1.0..1.0]
* @param direction The direction the robot should drive in degrees. The
* direction and maginitute are independent of the rotation
* rate.
* @param rotation The rate of rotation for the robot that is completely
* independent of the magnitute or direction. [-1.0..1.0]
*/
void RobotDrive::MecanumDrive_Polar(float magnitude, float direction,
float rotation) {
static bool reported = false;
if (!reported) {
reported = true;
}
// Normalized for full power along the Cartesian axes.
magnitude = Limit(magnitude) * sqrt(2.0);
// The rollers are at 45 degree angles.
double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
double cosD = cos(dirInRad);
double sinD = sin(dirInRad);
double wheelSpeeds[kMaxNumberOfMotors];
wheelSpeeds[kFrontLeftMotor] = sinD * magnitude + rotation;
wheelSpeeds[kFrontRightMotor] = cosD * magnitude - rotation;
wheelSpeeds[kRearLeftMotor] = cosD * magnitude + rotation;
wheelSpeeds[kRearRightMotor] = sinD * magnitude - rotation;
Normalize(wheelSpeeds);
uint8_t syncGroup = 0x80;
m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] *
m_invertedMotors[kFrontLeftMotor] * m_maxOutput,
syncGroup);
m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] *
m_invertedMotors[kFrontRightMotor] * m_maxOutput,
syncGroup);
m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] *
m_invertedMotors[kRearLeftMotor] * m_maxOutput,
syncGroup);
m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] *
m_invertedMotors[kRearRightMotor] * m_maxOutput,
syncGroup);
// CANJaguar::UpdateSyncGroup(syncGroup);
// FIXME: m_safetyHelper->Feed();
}
/**
* Holonomic Drive method for Mecanum wheeled robots.
*
* This is an alias to MecanumDrive_Polar() for backward compatability
*
* @param magnitude The speed that the robot should drive in a given direction.
* [-1.0..1.0]
* @param direction The direction the robot should drive. The direction and
* magnitude are independent of the rotation rate.
* @param rotation The rate of rotation for the robot that is completely
* independent of the magnitude or direction. [-1.0..1.0]
*/
void RobotDrive::HolonomicDrive(float magnitude, float direction,
float rotation) {
MecanumDrive_Polar(magnitude, direction, rotation);
}
/**
* Set the speed of the right and left motors.
*
* This is used once an appropriate drive setup function is called such as
* TwoWheelDrive(). The motors are set to "leftOutput" and "rightOutput"
* and includes flipping the direction of one side for opposing motors.
*
* @param leftOutput The speed to send to the left side of the robot.
* @param rightOutput The speed to send to the right side of the robot.
*/
void RobotDrive::SetLeftRightMotorOutputs(float leftOutput, float rightOutput) {
wpi_assert(m_rearLeftMotor != nullptr && m_rearRightMotor != nullptr);
uint8_t syncGroup = 0x80;
if (m_frontLeftMotor != nullptr)
m_frontLeftMotor->Set(
Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput,
syncGroup);
m_rearLeftMotor->Set(
Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput,
syncGroup);
if (m_frontRightMotor != nullptr)
m_frontRightMotor->Set(
-Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput,
syncGroup);
m_rearRightMotor->Set(
-Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput,
syncGroup);
// CANJaguar::UpdateSyncGroup(syncGroup);
// FIXME: m_safetyHelper->Feed();
}
/**
* Limit motor values to the -1.0 to +1.0 range.
*/
float RobotDrive::Limit(float num) {
if (num > 1.0) {
return 1.0;
}
if (num < -1.0) {
return -1.0;
}
return num;
}
/**
* Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
*/
void RobotDrive::Normalize(double* wheelSpeeds) {
double maxMagnitude = fabs(wheelSpeeds[0]);
int32_t i;
for (i = 1; i < kMaxNumberOfMotors; i++) {
double temp = fabs(wheelSpeeds[i]);
if (maxMagnitude < temp) maxMagnitude = temp;
}
if (maxMagnitude > 1.0) {
for (i = 0; i < kMaxNumberOfMotors; i++) {
wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
}
}
}
/**
* Rotate a vector in Cartesian space.
*/
void RobotDrive::RotateVector(double& x, double& y, double angle) {
double cosA = cos(angle * (3.14159 / 180.0));
double sinA = sin(angle * (3.14159 / 180.0));
double xOut = x * cosA - y * sinA;
double yOut = x * sinA + y * cosA;
x = xOut;
y = yOut;
}
/**
* Invert a motor direction.
*
* This is used when a motor should run in the opposite direction as the drive
* code would normally run it. Motors that are direct drive would be inverted,
* the Drive code assumes that the motors are geared with one reversal.
*
* @param motor The motor index to invert.
* @param isInverted True if the motor should be inverted when operated.
*/
void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted) {
if (motor < 0 || motor > 3) {
wpi_setWPIError(InvalidMotorIndex);
return;
}
m_invertedMotors[motor] = isInverted ? -1 : 1;
}
/**
* Set the turning sensitivity.
*
* This only impacts the Drive() entry-point.
*
* @param sensitivity Effectively sets the turning sensitivity (or turn radius
* for a given value)
*/
void RobotDrive::SetSensitivity(float sensitivity) {
m_sensitivity = sensitivity;
}
/**
* Configure the scaling factor for using RobotDrive with motor controllers in a
* mode other than PercentVbus.
*
* @param maxOutput Multiplied with the output percentage computed by the drive
* functions.
*/
void RobotDrive::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; }
void RobotDrive::SetExpiration(float timeout) {
// FIXME: m_safetyHelper->SetExpiration(timeout);
}
float RobotDrive::GetExpiration() const {
return -1; // FIXME: return m_safetyHelper->GetExpiration();
}
bool RobotDrive::IsAlive() const {
return true; // FIXME: m_safetyHelper->IsAlive();
}
bool RobotDrive::IsSafetyEnabled() const {
return false; // FIXME: return m_safetyHelper->IsSafetyEnabled();
}
void RobotDrive::SetSafetyEnabled(bool enabled) {
// FIXME: m_safetyHelper->SetSafetyEnabled(enabled);
}
void RobotDrive::GetDescription(std::ostringstream& desc) const {
desc << "RobotDrive";
}
void RobotDrive::StopMotor() {
if (m_frontLeftMotor != nullptr) m_frontLeftMotor->Disable();
if (m_frontRightMotor != nullptr) m_frontRightMotor->Disable();
if (m_rearLeftMotor != nullptr) m_rearLeftMotor->Disable();
if (m_rearRightMotor != nullptr) m_rearRightMotor->Disable();
}

89
wpilibc/sim/src/SafePWM.cpp Normal file
View File

@@ -0,0 +1,89 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "SafePWM.h"
#include <memory>
#include <sstream>
/**
* Constructor for a SafePWM object taking a channel number.
*
* @param channel The PWM channel number (0..19).
*/
SafePWM::SafePWM(uint32_t channel) : PWM(channel) {
m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
m_safetyHelper->SetSafetyEnabled(false);
}
/**
* Set the expiration time for the PWM object.
*
* @param timeout The timeout (in seconds) for this motor object
*/
void SafePWM::SetExpiration(float timeout) {
m_safetyHelper->SetExpiration(timeout);
}
/**
* Return the expiration time for the PWM object.
*
* @returns The expiration time value.
*/
float SafePWM::GetExpiration() const { return m_safetyHelper->GetExpiration(); }
/**
* Check if the PWM object is currently alive or stopped due to a timeout.
*
* @return a bool value that is true if the motor has NOT timed out and should
* still be running.
*/
bool SafePWM::IsAlive() const { return m_safetyHelper->IsAlive(); }
/**
* Stop the motor associated with this PWM object.
*
* This is called by the MotorSafetyHelper object when it has a timeout for this
* PWM and needs to stop it from running.
*/
void SafePWM::StopMotor() { SetRaw(kPwmDisabled); }
/**
* Enable/disable motor safety for this device.
*
* Turn on and off the motor safety option for this PWM object.
*
* @param enabled True if motor safety is enforced for this object
*/
void SafePWM::SetSafetyEnabled(bool enabled) {
m_safetyHelper->SetSafetyEnabled(enabled);
}
/**
* Check if motor safety is enabled for this object.
*
* @returns True if motor safety is enforced for this object
*/
bool SafePWM::IsSafetyEnabled() const {
return m_safetyHelper->IsSafetyEnabled();
}
void SafePWM::GetDescription(std::ostringstream& desc) const {
desc << "PWM " << GetChannel();
}
/**
* Feed the MotorSafety timer when setting the speed.
*
* This method is called by the subclass motor whenever it updates its speed,
* thereby reseting the timeout value.
*
* @param speed Value to pass to the PWM class
*/
void SafePWM::SetSpeed(float speed) {
PWM::SetSpeed(speed);
m_safetyHelper->Feed();
}

144
wpilibc/sim/src/SampleRobot.cpp Normal file
View File

@@ -0,0 +1,144 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "SampleRobot.h"
#include <stdio.h>
#include "LiveWindow/LiveWindow.h"
#include "SmartDashboard/SmartDashboard.h"
#include "Timer.h"
#include "networktables/NetworkTable.h"
#if defined(_UNIX)
#include <unistd.h>
#elif defined(_WIN32)
#include <windows.h>
void sleep(unsigned milliseconds) { Sleep(milliseconds); }
#endif
SampleRobot::SampleRobot() : m_robotMainOverridden(true) {}
/**
* Robot-wide initialization code should go here.
*
* Programmers should override this method for default Robot-wide initialization
* which will be called each time the robot enters the disabled state.
*/
void SampleRobot::RobotInit() {
printf("Default %s() method... Override me!\n", __FUNCTION__);
}
/**
* Disabled should go here.
*
* Programmers should override this method to run code that should run while the
* field is disabled.
*/
void SampleRobot::Disabled() {
printf("Default %s() method... Override me!\n", __FUNCTION__);
}
/**
* Autonomous should go here.
*
* Programmers should override this method to run code that should run while the
* field is in the autonomous period. This will be called once each time the
* robot enters the autonomous state.
*/
void SampleRobot::Autonomous() {
printf("Default %s() method... Override me!\n", __FUNCTION__);
}
/**
* Operator control (tele-operated) code should go here.
*
* Programmers should override this method to run code that should run while the
* field is in the Operator Control (tele-operated) period. This is called once
* each time the robot enters the teleop state.
*/
void SampleRobot::OperatorControl() {
printf("Default %s() method... Override me!\n", __FUNCTION__);
}
/**
* Test program should go here.
*
* Programmers should override this method to run code that executes while the
* robot is in test mode. This will be called once whenever the robot enters
* test mode.
*/
void SampleRobot::Test() {
printf("Default %s() method... Override me!\n", __FUNCTION__);
}
/**
* Robot main program for free-form programs.
*
* This should be overridden by user subclasses if the intent is to not use the
* Autonomous() and OperatorControl() methods. In that case, the program is
* responsible for sensing when to run the autonomous and operator control
* functions in their program.
*
* This method will be called immediately after the constructor is called. If it
* has not been overridden by a user subclass (i.e. the default version runs),
* then the Autonomous() and OperatorControl() methods will be called.
*/
void SampleRobot::RobotMain() { m_robotMainOverridden = false; }
/**
* Start a competition.
*
* This code needs to track the order of the field starting to ensure that
* everything happens in the right order. Repeatedly run the correct method,
* either Autonomous or OperatorControl or Test when the robot is enabled.
* After running the correct method, wait for some state to change, either the
* other mode starts or the robot is disabled. Then go back and wait for the
* robot to be enabled again.
*/
void SampleRobot::StartCompetition() {
LiveWindow* lw = LiveWindow::GetInstance();
SmartDashboard::init();
NetworkTable::GetTable("LiveWindow")
->GetSubTable("~STATUS~")
->PutBoolean("LW Enabled", false);
RobotMain();
if (!m_robotMainOverridden) {
// first and one-time initialization
lw->SetEnabled(false);
RobotInit();
while (true) {
if (IsDisabled()) {
m_ds.InDisabled(true);
Disabled();
m_ds.InDisabled(false);
while (IsDisabled()) sleep(1); // m_ds.WaitForData();
} else if (IsAutonomous()) {
m_ds.InAutonomous(true);
Autonomous();
m_ds.InAutonomous(false);
while (IsAutonomous() && IsEnabled()) sleep(1); // m_ds.WaitForData();
} else if (IsTest()) {
lw->SetEnabled(true);
m_ds.InTest(true);
Test();
m_ds.InTest(false);
while (IsTest() && IsEnabled()) sleep(1); // m_ds.WaitForData();
lw->SetEnabled(false);
} else {
m_ds.InOperatorControl(true);
OperatorControl();
m_ds.InOperatorControl(false);
while (IsOperatorControl() && IsEnabled())
sleep(1); // m_ds.WaitForData();
}
}
}
}

150
wpilibc/sim/src/SensorBase.cpp Normal file
View File

@@ -0,0 +1,150 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "SensorBase.h"
#include "WPIErrors.h"
const uint32_t SensorBase::kDigitalChannels;
const uint32_t SensorBase::kAnalogInputs;
const uint32_t SensorBase::kSolenoidChannels;
const uint32_t SensorBase::kSolenoidModules;
const uint32_t SensorBase::kPwmChannels;
const uint32_t SensorBase::kRelayChannels;
const uint32_t SensorBase::kPDPChannels;
const uint32_t SensorBase::kChassisSlots;
SensorBase* SensorBase::m_singletonList = nullptr;
/**
* Creates an instance of the sensor base and gets an FPGA handle
*/
SensorBase::SensorBase() {}
/**
* Add sensor to the singleton list.
*
* Add this sensor to the list of singletons that need to be deleted when
* the robot program exits. Each of the sensors on this list are singletons,
* that is they aren't allocated directly with new, but instead are allocated
* by the static GetInstance method. As a result, they are never deleted when
* the program exits. Consequently these sensors may still be holding onto
* resources and need to have their destructors called at the end of the
* program.
*/
void SensorBase::AddToSingletonList() {
m_nextSingleton = m_singletonList;
m_singletonList = this;
}
/**
* Delete all the singleton classes on the list.
*
* All the classes that were allocated as singletons need to be deleted so
* their resources can be freed.
*/
void SensorBase::DeleteSingletons() {
for (SensorBase* next = m_singletonList; next != nullptr;) {
SensorBase* tmp = next;
next = next->m_nextSingleton;
delete tmp;
}
m_singletonList = nullptr;
}
/**
* Check that the solenoid module number is valid.
*
* @return Solenoid module is valid and present
*/
bool SensorBase::CheckSolenoidModule(uint8_t moduleNumber) {
return 1 <= moduleNumber && moduleNumber <= 2; // TODO: Fix for Athena
}
/**
* Check that the digital channel number is valid.
*
* Verify that the channel number is one of the legal channel numbers. Channel
* numbers are 1-based.
*
* @return Digital channel is valid
*/
bool SensorBase::CheckDigitalChannel(uint32_t channel) {
if (channel > 0 && channel <= kDigitalChannels) return true;
return false;
}
/**
* Check that the digital channel number is valid.
*
* Verify that the channel number is one of the legal channel numbers. Channel
* numbers are 1-based.
*
* @return Relay channel is valid
*/
bool SensorBase::CheckRelayChannel(uint32_t channel) {
if (channel > 0 && channel <= kRelayChannels) return true;
return false;
}
/**
* Check that the digital channel number is valid.
*
* Verify that the channel number is one of the legal channel numbers. Channel
* numbers are 1-based.
*
* @return PWM channel is valid
*/
bool SensorBase::CheckPWMChannel(uint32_t channel) {
if (channel > 0 && channel <= kPwmChannels) return true;
return false;
}
/**
* Check that the analog input number is valid.
*
* Verify that the analog input number is one of the legal channel numbers.
* Channel numbers are 1-based.
*
* @return Analog channel is valid
*/
bool SensorBase::CheckAnalogInput(uint32_t channel) {
if (channel > 0 && channel <= kAnalogInputs) return true;
return false;
}
/**
* Check that the analog output number is valid.
*
* Verify that the analog output number is one of the legal channel numbers.
* Channel numbers are 1-based.
*
* @return Analog channel is valid
*/
bool SensorBase::CheckAnalogOutput(uint32_t channel) {
if (channel > 0 && channel <= kAnalogOutputs) return true;
return false;
}
/**
* Verify that the solenoid channel number is within limits.
*
* @return Solenoid channel is valid
*/
bool SensorBase::CheckSolenoidChannel(uint32_t channel) {
if (channel > 0 && channel <= kSolenoidChannels) return true;
return false;
}
/**
* Verify that the power distribution channel number is within limits.
*
* @return PDP channel is valid
*/
bool SensorBase::CheckPDPChannel(uint32_t channel) {
if (channel > 0 && channel <= kPDPChannels) return true;
return false;
}

89
wpilibc/sim/src/Solenoid.cpp Normal file
View File

@@ -0,0 +1,89 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Solenoid.h"
#include "LiveWindow/LiveWindow.h"
#include "WPIErrors.h"
#include "simulation/simTime.h"
/**
* Constructor.
*
* @param channel The channel on the solenoid module to control (1..8).
*/
Solenoid::Solenoid(uint32_t channel) : Solenoid(1, channel) {}
/**
* Constructor.
*
* @param moduleNumber The solenoid module (1 or 2).
* @param channel The channel on the solenoid module to control (1..8).
*/
Solenoid::Solenoid(uint8_t moduleNumber, uint32_t channel) {
char buffer[50];
int n = sprintf(buffer, "pneumatic/%d/%d", moduleNumber, channel);
m_impl = new SimContinuousOutput(buffer);
LiveWindow::GetInstance()->AddActuator("Solenoid", moduleNumber, channel,
this);
}
Solenoid::~Solenoid() {
if (m_table != nullptr) m_table->RemoveTableListener(this);
}
/**
* Set the value of a solenoid.
*
* @param on Turn the solenoid output off or on.
*/
void Solenoid::Set(bool on) {
m_on = on;
m_impl->Set(on ? 1 : -1);
}
/**
* Read the current value of the solenoid.
*
* @return The current value of the solenoid.
*/
bool Solenoid::Get() const { return m_on; }
void Solenoid::ValueChanged(ITable* source, llvm::StringRef key,
std::shared_ptr<nt::Value> value, bool isNew) {
if (!value->IsBoolean()) return;
Set(value->GetBoolean());
}
void Solenoid::UpdateTable() {
if (m_table != nullptr) {
m_table->PutBoolean("Value", Get());
}
}
void Solenoid::StartLiveWindowMode() {
Set(false);
if (m_table != nullptr) {
m_table->AddTableListener("Value", this, true);
}
}
void Solenoid::StopLiveWindowMode() {
Set(false);
if (m_table != nullptr) {
m_table->RemoveTableListener(this);
}
}
std::string Solenoid::GetSmartDashboardType() const { return "Solenoid"; }
void Solenoid::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> Solenoid::GetTable() const { return m_table; }

65
wpilibc/sim/src/Talon.cpp Normal file
View File

@@ -0,0 +1,65 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Talon.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "LiveWindow/LiveWindow.h"
/**
* @param channel The PWM channel that the Talon is attached to.
*/
Talon::Talon(uint32_t channel) : SafePWM(channel) {
/* Note that the Talon uses the following bounds for PWM values. These values
* should work reasonably well for most controllers, but if users experience
* issues such as asymmetric behavior around the deadband or inability to
* saturate the controller in either direction, calibration is recommended.
* The calibration procedure can be found in the Talon User Manual available
* from CTRE.
*
* - 211 = full "forward"
* - 133 = the "high end" of the deadband range
* - 129 = center of the deadband range (off)
* - 125 = the "low end" of the deadband range
* - 49 = full "reverse"
*/
SetBounds(2.037, 1.539, 1.513, 1.487, .989);
SetPeriodMultiplier(kPeriodMultiplier_2X);
SetRaw(m_centerPwm);
LiveWindow::GetInstance()->AddActuator("Talon", GetChannel(), this);
}
/**
* Set the PWM value.
*
* The PWM value is set using a range of -1.0 to 1.0, appropriately
* scaling the value for the FPGA.
*
* @param speed The speed value between -1.0 and 1.0 to set.
* @param syncGroup Unused interface.
*/
void Talon::Set(float speed, uint8_t syncGroup) { SetSpeed(speed); }
/**
* Get the recently set value of the PWM.
*
* @return The most recently set value for the PWM between -1.0 and 1.0.
*/
float Talon::Get() const { return GetSpeed(); }
/**
* Common interface for disabling a motor.
*/
void Talon::Disable() { SetRaw(kPwmDisabled); }
/**
* Write out the PID value as seen in the PIDOutput base object.
*
* @param output Write out the PWM value as was found in the PIDController
*/
void Talon::PIDWrite(float output) { Set(output); }

194
wpilibc/sim/src/Timer.cpp Normal file
View File

@@ -0,0 +1,194 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Timer.h"
#include <time.h>
#include "Utility.h"
#include "simulation/simTime.h"
// Internal stuff
#include "simulation/MainNode.h"
#include "simulation/SimFloatInput.h"
namespace wpilib {
namespace internal {
double simTime = 0;
std::condition_variable time_wait;
std::mutex time_wait_mutex;
void time_callback(const msgs::ConstFloat64Ptr& msg) {
simTime = msg->data();
time_wait.notify_all();
}
}
}
/**
* Pause the task for a specified time.
*
* Pause the execution of the program for a specified period of time given in
* seconds. Motors will continue to run at their last assigned values, and
* sensors will continue to update. Only the task containing the wait will
* pause until the wait time is expired.
*
* @param seconds Length of time to pause, in seconds.
*/
void Wait(double seconds) {
if (seconds < 0.0) return;
double start = wpilib::internal::simTime;
std::unique_lock<std::mutex> lock(wpilib::internal::time_wait_mutex);
while ((wpilib::internal::simTime - start) < seconds) {
wpilib::internal::time_wait.wait(lock);
}
}
/*
* Return the FPGA system clock time in seconds.
*
* This is deprecated and just forwards to Timer::GetFPGATimestamp().
*
* @returns Robot running time in seconds.
*/
double GetClock() { return Timer::GetFPGATimestamp(); }
/**
* @brief Gives real-time clock system time with nanosecond resolution
* @return The time, just in case you want the robot to start autonomous at 8pm
* on Saturday (except in simulation).
*/
double GetTime() {
return Timer::GetFPGATimestamp(); // The epoch starts when Gazebo starts
}
// for compatibility with msvc12--see C2864
const double Timer::kRolloverTime = (1ll << 32) / 1e6;
/**
* Create a new timer object.
*
* Create a new timer object and reset the time to zero. The timer is initially
* not running and must be started.
*/
Timer::Timer() : m_startTime(0.0), m_accumulatedTime(0.0), m_running(false) {
// Creates a semaphore to control access to critical regions.
// Initially 'open'
Reset();
}
/**
* Get the current time from the timer.
*
* If the clock is running it is derived from the current system clock the
* start time stored in the timer class. If the clock is not running, then
* return the time when it was last stopped.
*
* @return unsigned Current time value for this timer in seconds
*/
double Timer::Get() const {
double result;
double currentTime = GetFPGATimestamp();
std::lock_guard<priority_mutex> sync(m_mutex);
if (m_running) {
// This math won't work if the timer rolled over (71 minutes after boot).
// TODO: Check for it and compensate.
result = (currentTime - m_startTime) + m_accumulatedTime;
} else {
result = m_accumulatedTime;
}
return result;
}
/**
* Reset the timer by setting the time to 0.
*
* Make the timer startTime the current time so new requests will be relative to
* now.
*/
void Timer::Reset() {
std::lock_guard<priority_mutex> sync(m_mutex);
m_accumulatedTime = 0;
m_startTime = GetFPGATimestamp();
}
/**
* Start the timer running.
*
* Just set the running flag to true indicating that all time requests should be
* relative to the system clock.
*/
void Timer::Start() {
std::lock_guard<priority_mutex> sync(m_mutex);
if (!m_running) {
m_startTime = GetFPGATimestamp();
m_running = true;
}
}
/**
* Stop the timer.
*
* This computes the time as of now and clears the running flag, causing all
* subsequent time requests to be read from the accumulated time rather than
* looking at the system clock.
*/
void Timer::Stop() {
double temp = Get();
std::lock_guard<priority_mutex> sync(m_mutex);
if (m_running) {
m_accumulatedTime = temp;
m_running = false;
}
}
/**
* Check if the period specified has passed and if it has, advance the start
* time by that period. This is useful to decide if it's time to do periodic
* work without drifting later by the time it took to get around to checking.
*
* @param period The period to check for (in seconds).
* @return If the period has passed.
*/
bool Timer::HasPeriodPassed(double period) {
if (Get() > period) {
std::lock_guard<priority_mutex> sync(m_mutex);
// Advance the start time by the period.
// Don't set it to the current time... we want to avoid drift.
m_startTime += period;
return true;
}
return false;
}
/*
* Return the FPGA system clock time in seconds.
*
* Return the time from the FPGA hardware clock in seconds since the FPGA
* started. Rolls over after 71 minutes.
*
* @return Robot running time in seconds.
*/
double Timer::GetFPGATimestamp() {
// FPGA returns the timestamp in microseconds
// Call the helper GetFPGATime() in Utility.cpp
return wpilib::internal::simTime;
}
/*
* Not in a match.
*/
double Timer::GetMatchTime() { return Timer::GetFPGATimestamp(); }
// Internal function that reads the PPC timestamp counter.
extern "C" {
uint32_t niTimestamp32(void);
uint64_t niTimestamp64(void);
}

212
wpilibc/sim/src/Utility.cpp Normal file
View File

@@ -0,0 +1,212 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Utility.h"
#include <stdio.h>
#include <string.h>
#include "Timer.h"
#include "simulation/simTime.h"
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <sstream>
#if not defined(_WIN32)
#include <cxxabi.h>
#include <execinfo.h>
#endif
static bool stackTraceEnabled = false;
static bool suspendOnAssertEnabled = false;
/**
* Enable Stack trace after asserts.
*/
void wpi_stackTraceOnAssertEnable(bool enabled) { stackTraceEnabled = enabled; }
/**
* Enable suspend on asssert.
*
* If enabled, the user task will be suspended whenever an assert fails. This
* will allow the user to attach to the task with the debugger and examine
* variables around the failure.
*/
void wpi_suspendOnAssertEnabled(bool enabled) {
suspendOnAssertEnabled = enabled;
}
static void wpi_handleTracing() {
// if (stackTraceEnabled)
// {
// printf("\n-----------<Stack Trace>----------------\n");
// printCurrentStackTrace();
// }
printf("\n");
}
/**
* Assert implementation.
* 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 char* conditionText,
const char* message, const char* fileName,
uint32_t lineNumber, const char* funcName) {
if (!conditionValue) {
std::stringstream errorStream;
errorStream << "Assertion \"" << conditionText << "\" ";
errorStream << "on line " << lineNumber << " ";
errorStream << "of " << basename(fileName) << " ";
if (message[0] != '\0') {
errorStream << "failed: " << message << std::endl;
} else {
errorStream << "failed." << std::endl;
}
// Print to console and send to remote dashboard
std::cout << "\n\n>>>>" << errorStream.str();
wpi_handleTracing();
}
return conditionValue;
}
/**
* Common error routines for wpi_assertEqual_impl and wpi_assertNotEqual_impl
* This should not be called directly; it should only be used by
* wpi_assertEqual_impl and wpi_assertNotEqual_impl.
*/
void wpi_assertEqual_common_impl(int valueA, int valueB,
const std::string& equalityType,
const std::string& message,
const std::string& fileName,
uint32_t lineNumber,
const std::string& funcName) {
// Error string buffer
std::stringstream error;
// If an error message was specified, include it
// Build error string
if (message.size() > 0) {
error << "Assertion failed: \"" << message << "\", \"" << valueA << "\" "
<< equalityType << " \"" << valueB << "\" in " << funcName << "() in "
<< fileName << " at line " << lineNumber << "\n";
} else {
error << "Assertion failed: \"" << valueA << "\" " << equalityType << " \""
<< valueB << "\" in " << funcName << "() in " << fileName
<< " at line " << lineNumber << "\n";
}
// Print to console and send to remote dashboard
std::cout << "\n\n>>>>" << error.str();
wpi_handleTracing();
}
/**
* Assert equal implementation.
* This determines whether the two given integers are equal. If not,
* the value of each is printed along with an optional message string.
* The users don't call this, but instead use the wpi_assertEqual macros in
* Utility.h.
*/
bool wpi_assertEqual_impl(int valueA, int valueB, const std::string& message,
const std::string& fileName, uint32_t lineNumber,
const std::string& funcName) {
if (!(valueA == valueB)) {
wpi_assertEqual_common_impl(valueA, valueB, "!=", message, fileName,
lineNumber, funcName);
}
return valueA == valueB;
}
/**
* Assert not equal implementation.
* This determines whether the two given integers are equal. If so,
* the value of each is printed along with an optional message string.
* The users don't call this, but instead use the wpi_assertNotEqual macros in
* Utility.h.
*/
bool wpi_assertNotEqual_impl(int valueA, int valueB, const std::string& message,
const std::string& fileName, uint32_t lineNumber,
const std::string& funcName) {
if (!(valueA != valueB)) {
wpi_assertEqual_common_impl(valueA, valueB, "==", message, fileName,
lineNumber, funcName);
}
return valueA != valueB;
}
/**
* Read the microsecond-resolution timer on the FPGA.
*
* @return The current time in microseconds according to the FPGA (since FPGA
* reset).
*/
uint64_t GetFPGATime() { return wpilib::internal::simTime * 1e6; }
// TODO: implement symbol demangling and backtrace on windows
#if not defined(_WIN32)
/**
* Demangle a C++ symbol, used for printing stack traces.
*/
static std::string demangle(char const* mangledSymbol) {
char buffer[256];
size_t length;
int status;
if (sscanf(mangledSymbol, "%*[^(]%*[^_]%255[^)+]", buffer)) {
char* symbol = abi::__cxa_demangle(buffer, nullptr, &length, &status);
if (status == 0) {
return symbol;
} else {
// If the symbol couldn't be demangled, it's probably a C function,
// so just return it as-is.
return buffer;
}
}
// If everything else failed, just return the mangled symbol
return mangledSymbol;
}
/**
* Get a stack trace, ignoring the first "offset" symbols.
*/
std::string GetStackTrace(uint32_t offset) {
void* stackTrace[128];
int stackSize = backtrace(stackTrace, 128);
char** mangledSymbols = backtrace_symbols(stackTrace, stackSize);
std::stringstream trace;
for (int i = offset; i < stackSize; i++) {
// Only print recursive functions once in a row.
if (i == 0 || stackTrace[i] != stackTrace[i - 1]) {
trace << "\tat " << demangle(mangledSymbols[i]) << std::endl;
}
}
free(mangledSymbols);
return trace.str();
}
#else
static std::string demangle(char const* mangledSymbol) {
return "no demangling on windows";
}
std::string GetStackTrace(uint32_t offset) {
return "no stack trace on windows";
}
#endif

67
wpilibc/sim/src/Victor.cpp Normal file
View File

@@ -0,0 +1,67 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Victor.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "LiveWindow/LiveWindow.h"
/**
* @param channel The PWM channel that the Victor is attached to.
*/
Victor::Victor(uint32_t channel) : SafePWM(channel) {
/* Note that the Victor uses the following bounds for PWM values. These values
* were determined empirically and optimized for the Victor 888. These values
* should work reasonably well for Victor 884 controllers as well but if users
* experience issues such as asymmetric behavior around the deadband or
* inability to saturate the controller in either direction, calibration is
* recommended. The calibration procedure can be found in the Victor 884 User
* Manual available from IFI.
*
* - 206 = full "forward"
* - 131 = the "high end" of the deadband range
* - 128 = center of the deadband range (off)
* - 125 = the "low end" of the deadband range
* - 56 = full "reverse"
*/
SetBounds(2.027, 1.525, 1.507, 1.49, 1.026);
SetPeriodMultiplier(kPeriodMultiplier_2X);
SetRaw(m_centerPwm);
LiveWindow::GetInstance()->AddActuator("Victor", GetChannel(), this);
}
/**
* Set the PWM value.
*
* The PWM value is set using a range of -1.0 to 1.0, appropriately
* scaling the value for the FPGA.
*
* @param speed The speed value between -1.0 and 1.0 to set.
* @param syncGroup Unused interface.
*/
void Victor::Set(float speed, uint8_t syncGroup) { SetSpeed(speed); }
/**
* Get the recently set value of the PWM.
*
* @return The most recently set value for the PWM between -1.0 and 1.0.
*/
float Victor::Get() const { return GetSpeed(); }
/**
* Common interface for disabling a motor.
*/
void Victor::Disable() { SetRaw(kPwmDisabled); }
/**
* Write out the PID value as seen in the PIDOutput base object.
*
* @param output Write out the PWM value as was found in the PIDController
*/
void Victor::PIDWrite(float output) { Set(output); }

22
wpilibc/sim/src/simulation/SimContinuousOutput.cpp Normal file
View File

@@ -0,0 +1,22 @@
/*
* SimContinuousOutput.cpp
*
* Created on: May 28, 2014
* Author: alex
*/
#include "simulation/SimContinuousOutput.h"
#include "simulation/MainNode.h"
SimContinuousOutput::SimContinuousOutput(std::string topic) {
pub = MainNode::Advertise<msgs::Float64>("~/simulator/" + topic);
std::cout << "Initialized ~/simulator/" + topic << std::endl;
}
void SimContinuousOutput::Set(float speed) {
msgs::Float64 msg;
msg.set_data(speed);
pub->Publish(msg);
}
float SimContinuousOutput::Get() { return speed; }

21
wpilibc/sim/src/simulation/SimDigitalInput.cpp Normal file
View File

@@ -0,0 +1,21 @@
/*
* SimDigitalInput.cpp
*
* Created on: May 28, 2014
* Author: alex
*/
#include "simulation/SimDigitalInput.h"
#include "simulation/MainNode.h"
SimDigitalInput::SimDigitalInput(std::string topic) {
sub = MainNode::Subscribe("~/simulator/" + topic, &SimDigitalInput::callback,
this);
std::cout << "Initialized ~/simulator/" + topic << std::endl;
}
bool SimDigitalInput::Get() { return value; }
void SimDigitalInput::callback(const msgs::ConstBoolPtr& msg) {
value = msg->data();
}

52
wpilibc/sim/src/simulation/SimEncoder.cpp Normal file
View File

@@ -0,0 +1,52 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "simulation/SimEncoder.h"
#include "simulation/MainNode.h"
SimEncoder::SimEncoder(std::string topic) {
commandPub =
MainNode::Advertise<msgs::GzString>("~/simulator/" + topic + "/control");
posSub = MainNode::Subscribe("~/simulator/" + topic + "/position",
&SimEncoder::positionCallback, this);
velSub = MainNode::Subscribe("~/simulator/" + topic + "/velocity",
&SimEncoder::velocityCallback, this);
if (commandPub->WaitForConnection(
gazebo::common::Time(5.0))) { // Wait up to five seconds.
std::cout << "Initialized ~/simulator/" + topic << std::endl;
} else {
std::cerr << "Failed to initialize ~/simulator/" + topic +
": does the encoder exist?"
<< std::endl;
}
}
void SimEncoder::Reset() { sendCommand("reset"); }
void SimEncoder::Start() { sendCommand("start"); }
void SimEncoder::Stop() { sendCommand("stop"); }
double SimEncoder::GetPosition() { return position; }
double SimEncoder::GetVelocity() { return velocity; }
void SimEncoder::sendCommand(std::string cmd) {
msgs::GzString msg;
msg.set_data(cmd);
commandPub->Publish(msg);
}
void SimEncoder::positionCallback(const msgs::ConstFloat64Ptr& msg) {
position = msg->data();
}
void SimEncoder::velocityCallback(const msgs::ConstFloat64Ptr& msg) {
velocity = msg->data();
}

21
wpilibc/sim/src/simulation/SimFloatInput.cpp Normal file
View File

@@ -0,0 +1,21 @@
/*
* SimFloatInput.cpp
*
* Created on: May 28, 2014
* Author: alex
*/
#include "simulation/SimFloatInput.h"
#include "simulation/MainNode.h"
SimFloatInput::SimFloatInput(std::string topic) {
sub = MainNode::Subscribe("~/simulator/" + topic, &SimFloatInput::callback,
this);
std::cout << "Initialized ~/simulator/" + topic << std::endl;
}
double SimFloatInput::Get() { return value; }
void SimFloatInput::callback(const msgs::ConstFloat64Ptr& msg) {
value = msg->data();
}

48
wpilibc/sim/src/simulation/SimGyro.cpp Normal file
View File

@@ -0,0 +1,48 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2016. 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 the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "simulation/SimGyro.h"
#include "simulation/MainNode.h"
SimGyro::SimGyro(std::string topic) {
commandPub =
MainNode::Advertise<msgs::GzString>("~/simulator/" + topic + "/control");
posSub = MainNode::Subscribe("~/simulator/" + topic + "/position",
&SimGyro::positionCallback, this);
velSub = MainNode::Subscribe("~/simulator/" + topic + "/velocity",
&SimGyro::velocityCallback, this);
if (commandPub->WaitForConnection(
gazebo::common::Time(5.0))) { // Wait up to five seconds.
std::cout << "Initialized ~/simulator/" + topic << std::endl;
} else {
std::cerr << "Failed to initialize ~/simulator/" + topic +
": does the gyro exist?"
<< std::endl;
}
}
void SimGyro::Reset() { sendCommand("reset"); }
double SimGyro::GetAngle() { return position; }
double SimGyro::GetVelocity() { return velocity; }
void SimGyro::sendCommand(std::string cmd) {
msgs::GzString msg;
msg.set_data(cmd);
commandPub->Publish(msg);
}
void SimGyro::positionCallback(const msgs::ConstFloat64Ptr& msg) {
position = msg->data();
}
void SimGyro::velocityCallback(const msgs::ConstFloat64Ptr& msg) {
velocity = msg->data();
}