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

Add format script which invokes clang-format on the C++ source code (#41)

Browse Source 
On Windows machines, clang-format.exe must be in the PATH environment variable.
This commit is contained in:
Tyler Veness
2016-05-20 17:30:37 -07:00
committed by Peter Johnson
parent 68690643d2
commit e14e45da76
383 changed files with 13787 additions and 13198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
wpilibc/simulation/src/AnalogGyro.cpp
View File

@@ -6,9 +6,9 @@
/*----------------------------------------------------------------------------*/
#include "AnalogGyro.h"
#include "LiveWindow/LiveWindow.h"
#include "Timer.h"
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"
const uint32_t AnalogGyro::kOversampleBits = 10;
const uint32_t AnalogGyro::kAverageBits = 0;
@@ -18,63 +18,55 @@ 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.
*
* 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);
void AnalogGyro::InitAnalogGyro(int channel) {
SetPIDSourceType(PIDSourceType::kDisplacement);
char buffer[50];
int n = sprintf(buffer, "analog/%d", channel);
impl = new SimGyro(buffer);
char buffer[50];
int n = sprintf(buffer, "analog/%d", channel);
impl = new SimGyro(buffer);
LiveWindow::GetInstance()->AddSensor("AnalogGyro", channel, this);
LiveWindow::GetInstance()->AddSensor("AnalogGyro", channel, this);
}
/**
* AnalogGyro constructor with only a channel..
* AnalogGyro constructor with only a channel.
*
* @param channel The analog channel the gyro is connected to.
*/
AnalogGyro::AnalogGyro(uint32_t channel)
{
InitAnalogGyro(channel);
}
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.
*
* 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::Reset() { impl->Reset(); }
void AnalogGyro::Calibrate(){
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.
* 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.
* @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();
}
float AnalogGyro::GetAngle() const { return impl->GetAngle(); }
/**
* Return the rate of rotation of the gyro
@@ -83,7 +75,4 @@ float AnalogGyro::GetAngle() const
*
* @return the current rate in degrees per second
*/
double AnalogGyro::GetRate() const
{
return impl->GetVelocity();
}
double AnalogGyro::GetRate() const { return impl->GetVelocity(); }

84
wpilibc/simulation/src/AnalogInput.cpp
View File

@@ -6,88 +6,78 @@
/*----------------------------------------------------------------------------*/
#include "AnalogInput.h"
#include "WPIErrors.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);
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);
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().
*
* 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();
}
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.
* 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();
}
float AnalogInput::GetAverageVoltage() const { return m_impl->Get(); }
/**
* Get the channel number.
*
* @return The channel number.
*/
uint32_t AnalogInput::GetChannel() const
{
return m_channel;
}
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();
}
double AnalogInput::PIDGet() { return GetAverageVoltage(); }
void AnalogInput::UpdateTable() {
if (m_table != nullptr) {
m_table->PutNumber("Value", GetAverageVoltage());
}
if (m_table != nullptr) {
m_table->PutNumber("Value", GetAverageVoltage());
}
}
void AnalogInput::StartLiveWindowMode() {
}
void AnalogInput::StartLiveWindowMode() {}
void AnalogInput::StopLiveWindowMode() {
}
void AnalogInput::StopLiveWindowMode() {}
std::string AnalogInput::GetSmartDashboardType() const {
return "Analog Input";
return "Analog Input";
}
void AnalogInput::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> AnalogInput::GetTable() const {
return m_table;
}
std::shared_ptr<ITable> AnalogInput::GetTable() const { return m_table; }

53
wpilibc/simulation/src/AnalogPotentiometer.cpp
View File

@@ -10,29 +10,33 @@
/**
* 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;
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(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(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){
if (m_init_analog_input) {
delete m_analog_input;
m_init_analog_input = false;
}
@@ -44,7 +48,7 @@ AnalogPotentiometer::~AnalogPotentiometer() {
* @return The current position of the potentiometer.
*/
double AnalogPotentiometer::Get() const {
return m_analog_input->GetVoltage() * m_scale + m_offset;
return m_analog_input->GetVoltage() * m_scale + m_offset;
}
/**
@@ -52,32 +56,29 @@ double AnalogPotentiometer::Get() const {
*
* @return The current reading.
*/
double AnalogPotentiometer::PIDGet() {
return Get();
}
double AnalogPotentiometer::PIDGet() { return Get(); }
/**
* @return the Smart Dashboard Type
*/
std::string AnalogPotentiometer::GetSmartDashboardType() const {
return "Analog Input";
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();
m_table = subtable;
UpdateTable();
}
void AnalogPotentiometer::UpdateTable() {
if (m_table != nullptr) {
m_table->PutNumber("Value", Get());
}
if (m_table != nullptr) {
m_table->PutNumber("Value", Get());
}
}
std::shared_ptr<ITable> AnalogPotentiometer::GetTable() const {
return m_table;
return m_table;
}

47
wpilibc/simulation/src/DigitalInput.cpp
View File

@@ -14,54 +14,41 @@
*
* @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);
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();
}
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;
}
uint32_t DigitalInput::GetChannel() const { return m_channel; }
void DigitalInput::UpdateTable() {
if (m_table != nullptr) {
m_table->PutBoolean("Value", Get());
}
if (m_table != nullptr) {
m_table->PutBoolean("Value", Get());
}
}
void DigitalInput::StartLiveWindowMode() {
void DigitalInput::StartLiveWindowMode() {}
}
void DigitalInput::StopLiveWindowMode() {
}
void DigitalInput::StopLiveWindowMode() {}
std::string DigitalInput::GetSmartDashboardType() const {
return "DigitalInput";
return "DigitalInput";
}
void DigitalInput::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> DigitalInput::GetTable() const {
return m_table;
}
std::shared_ptr<ITable> DigitalInput::GetTable() const { return m_table; }

119
wpilibc/simulation/src/DoubleSolenoid.cpp
View File

@@ -6,9 +6,9 @@
/*----------------------------------------------------------------------------*/
#include "DoubleSolenoid.h"
#include "WPIErrors.h"
#include <string.h>
#include "LiveWindow/LiveWindow.h"
#include "WPIErrors.h"
/**
* Constructor.
@@ -17,35 +17,35 @@
* @param reverseChannel The reverse channel on the module to control.
*/
DoubleSolenoid::DoubleSolenoid(uint32_t forwardChannel, uint32_t reverseChannel)
: DoubleSolenoid(1, forwardChannel, reverseChannel) {}
: DoubleSolenoid(1, forwardChannel, reverseChannel) {}
/**
* Constructor.
*
* @param moduleNumber The solenoid module (1 or 2).
* @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(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);
if (m_table != nullptr) m_table->RemoveTableListener(this);
}
/**
@@ -53,21 +53,19 @@ DoubleSolenoid::~DoubleSolenoid() {
*
* @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;
}
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;
}
}
/**
@@ -75,52 +73,49 @@ void DoubleSolenoid::Set(Value value)
*
* @return The current value of the solenoid.
*/
DoubleSolenoid::Value DoubleSolenoid::Get() const
{
return m_value;
}
DoubleSolenoid::Value DoubleSolenoid::Get() const { return m_value; }
void DoubleSolenoid::ValueChanged(ITable *source, llvm::StringRef key,
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);
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")));
}
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);
}
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);
}
Set(kOff);
if (m_table != nullptr) {
m_table->RemoveTableListener(this);
}
}
std::string DoubleSolenoid::GetSmartDashboardType() const {
return "Double Solenoid";
return "Double Solenoid";
}
void DoubleSolenoid::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> DoubleSolenoid::GetTable() const {
return m_table;
}
std::shared_ptr<ITable> DoubleSolenoid::GetTable() const { return m_table; }

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

@@ -9,18 +9,20 @@
#include "Timer.h"
#include "simulation/MainNode.h"
//#include "MotorSafetyHelper.h"
#include "Utility.h"
#include "WPIErrors.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)
#define DS_LOG(level) \
if (level > dsLogLevel) \
; \
else \
Log().Get(level)
const uint32_t DriverStation::kBatteryChannel;
const uint32_t DriverStation::kJoystickPorts;
@@ -34,39 +36,38 @@ uint8_t DriverStation::m_updateNumber = 0;
* 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);
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();
AddToSingletonList();
}
/**
* Return a pointer to the singleton DriverStation.
*/
DriverStation& DriverStation::GetInstance()
{
static DriverStation instance;
return instance;
DriverStation& DriverStation::GetInstance() {
static DriverStation instance;
return instance;
}
/**
@@ -74,9 +75,8 @@ DriverStation& DriverStation::GetInstance()
*
* @return The battery voltage.
*/
float DriverStation::GetBatteryVoltage() const
{
return 12.0; // 12 volts all the time!
float DriverStation::GetBatteryVoltage() const {
return 12.0; // 12 volts all the time!
}
/**
@@ -84,80 +84,75 @@ float DriverStation::GetBatteryVoltage() const
* 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.
* @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;
}
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);
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.
* 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;
}
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);
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;
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;
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
@@ -165,163 +160,153 @@ short DriverStation::GetStickButtons(uint32_t stick)
/**
* 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.
* 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;
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.
*
* 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;
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.
* 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.
* @param value The state to set the digital output.
*/
void DriverStation::SetDigitalOut(uint32_t channel, bool value)
{
wpi_setWPIErrorWithContext(UnsupportedInSimulation, "SetDigitalOut");
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::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::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::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::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::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;
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
* @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
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
* 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
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 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
uint32_t DriverStation::GetLocation() const {
return -1; // TODO: Support locations
}
/**
* Wait until a new packet comes from the driver station
* Wait until a new packet comes from the driver station.
* This blocks on a semaphore, so the waiting is efficient.
* This is a good way to delay processing until there is new driver station data to act on
* This is a good way to delay processing until there is new driver station data
* to act on.
*/
void DriverStation::WaitForData()
{
std::unique_lock<std::mutex> lock(m_waitForDataMutex);
m_waitForDataCond.wait(lock);
void DriverStation::WaitForData() {
std::unique_lock<std::mutex> lock(m_waitForDataMutex);
m_waitForDataCond.wait(lock);
}
/**
* Return the approximate match time
* 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)
* 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;
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;
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;
void DriverStation::ReportWarning(std::string error) {
std::cout << error << std::endl;
}
/**
@@ -331,66 +316,53 @@ void DriverStation::ReportWarning(std::string error)
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;
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 that the Driver Station is configured for.
* @return The team number
*/
uint16_t DriverStation::GetTeamNumber() const
{
return 348;
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::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::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::joystickCallback0(const msgs::ConstFRCJoystickPtr &msg)
{
joystickCallback(msg, 0);
void DriverStation::joystickCallback1(const msgs::ConstFRCJoystickPtr& msg) {
joystickCallback(msg, 1);
}
void DriverStation::joystickCallback1(const msgs::ConstFRCJoystickPtr &msg)
{
joystickCallback(msg, 1);
void DriverStation::joystickCallback2(const msgs::ConstFRCJoystickPtr& msg) {
joystickCallback(msg, 2);
}
void DriverStation::joystickCallback2(const msgs::ConstFRCJoystickPtr &msg)
{
joystickCallback(msg, 2);
void DriverStation::joystickCallback3(const msgs::ConstFRCJoystickPtr& msg) {
joystickCallback(msg, 3);
}
void DriverStation::joystickCallback3(const msgs::ConstFRCJoystickPtr &msg)
{
joystickCallback(msg, 3);
void DriverStation::joystickCallback4(const msgs::ConstFRCJoystickPtr& msg) {
joystickCallback(msg, 4);
}
void DriverStation::joystickCallback4(const msgs::ConstFRCJoystickPtr &msg)
{
joystickCallback(msg, 4);
}
void DriverStation::joystickCallback5(const msgs::ConstFRCJoystickPtr &msg)
{
joystickCallback(msg, 5);
void DriverStation::joystickCallback5(const msgs::ConstFRCJoystickPtr& msg) {
joystickCallback(msg, 5);
}

417
wpilibc/simulation/src/Encoder.cpp
View File

@@ -6,9 +6,9 @@
/*----------------------------------------------------------------------------*/
#include "Encoder.h"
#include "LiveWindow/LiveWindow.h"
#include "Resource.h"
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"
/**
* Common initialization code for Encoders.
@@ -16,302 +16,327 @@
*
* 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.
* @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;
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;
int32_t index = 0;
m_distancePerPulse = 1.0;
LiveWindow::GetInstance()->AddSensor("Encoder", channelA, this);
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();
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 represents the orientation of the encoder and inverts the output values
* if necessary so forward represents positive values.
* @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.
* @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::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.
*
* 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 represents the orientation of the encoder and inverts the output values
* if necessary so forward represents positive values.
* @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.
* @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)
/* 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);
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.
*
* 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 represents the orientation of the encoder and inverts the output values
* if necessary so forward represents positive values.
* @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.
* @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)
/*// 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);
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();
}
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.
*
* 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.";
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.";
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.
* 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;
}
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.";
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.
* @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.";
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().
* @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.";
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.
*
* 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().
* @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.
* @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.";
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().
* @return The distance driven since the last reset as scaled by the value from
* SetDistancePerPulse().
*/
double Encoder::GetDistance() const
{
return m_distancePerPulse * impl->GetPosition();
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().
*
* 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();
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().
* @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.";
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.
* 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;
}
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.
*
* 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.";
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.
* 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;
void Encoder::SetPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
/**
@@ -319,47 +344,41 @@ void Encoder::SetPIDSourceType(PIDSourceType pidSource)
*
* @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;
}
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);
}
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::StartLiveWindowMode() {}
}
void Encoder::StopLiveWindowMode() {
}
void Encoder::StopLiveWindowMode() {}
std::string Encoder::GetSmartDashboardType() const {
if (m_encodingType == k4X)
return "Quadrature Encoder";
else
return "Encoder";
if (m_encodingType == k4X)
return "Quadrature Encoder";
else
return "Encoder";
}
void Encoder::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> Encoder::GetTable() const {
return m_table;
}
std::shared_ptr<ITable> Encoder::GetTable() const { return m_table; }

375
wpilibc/simulation/src/IterativeRobot.cpp
View File

@@ -8,13 +8,13 @@
#include "IterativeRobot.h"
#include "DriverStation.h"
#include "SmartDashboard/SmartDashboard.h"
#include "LiveWindow/LiveWindow.h"
#include "SmartDashboard/SmartDashboard.h"
#include "networktables/NetworkTable.h"
//not sure what this is used for yet.
// not sure what this is used for yet.
#ifdef _UNIX
#include <unistd.h>
#include <unistd.h>
#endif
const double IterativeRobot::kDefaultPeriod = 0;
@@ -22,152 +22,132 @@ 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.
* @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;
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;
}
double IterativeRobot::GetPeriod() { return m_period; }
/**
* Get the number of loops per second for the IterativeRobot
* 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;
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
* 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();
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();
}
// 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();
}
}
/**
@@ -180,135 +160,118 @@ void IterativeRobot::StartCompetition()
* @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;
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.
* 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__);
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.
* 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__);
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.
* 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__);
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.
* 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__);
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.
* 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__);
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.
* 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;
}
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.
* 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;
}
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.
* 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;
}
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.
* 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;
}
void IterativeRobot::TestPeriodic() {
static bool firstRun = true;
if (firstRun) {
printf("Default %s() method... Overload me!\n", __FUNCTION__);
firstRun = false;
}
}

52
wpilibc/simulation/src/Jaguar.cpp
View File

@@ -5,7 +5,6 @@
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Jaguar.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "LiveWindow/LiveWindow.h"
@@ -13,22 +12,21 @@
/**
* @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);
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);
LiveWindow::GetInstance()->AddActuator("Jaguar", GetChannel(), this);
}
/**
@@ -37,38 +35,26 @@ Jaguar::Jaguar(uint32_t channel) : SafePWM(channel)
* 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 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);
}
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();
}
float Jaguar::Get() const { return GetSpeed(); }
/**
* Common interface for disabling a motor.
*/
void Jaguar::Disable()
{
SetRaw(kPwmDisabled);
}
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);
}
void Jaguar::PIDWrite(float output) { Set(output); }

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

@@ -6,10 +6,10 @@
/*----------------------------------------------------------------------------*/
#include "Joystick.h"
#include "DriverStation.h"
#include "WPIErrors.h"
#include <math.h>
#include <memory>
#include "DriverStation.h"
#include "WPIErrors.h"
const uint32_t Joystick::kDefaultXAxis;
const uint32_t Joystick::kDefaultYAxis;
@@ -18,26 +18,26 @@ 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 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;
: 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;
m_buttons[kTriggerButton] = kDefaultTriggerButton;
m_buttons[kTopButton] = kDefaultTopButton;
}
/**
@@ -46,80 +46,73 @@ Joystick::Joystick(uint32_t port)
* 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 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;
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);
m_axes = std::make_unique<uint32_t[]>(numAxisTypes);
m_buttons = std::make_unique<uint32_t[]>(numButtonTypes);
}
Joystick * Joystick::GetStickForPort(uint32_t port)
{
Joystick *stick = joysticks[port];
if (stick == nullptr)
{
stick = new Joystick(port);
joysticks[port] = stick;
}
return stick;
Joystick* Joystick::GetStickForPort(uint32_t port) {
Joystick* stick = joysticks[port];
if (stick == nullptr) {
stick = new Joystick(port);
joysticks[port] = stick;
}
return stick;
}
/**
* Get the X value of the joystick.
*
* This depends on the mapping of the joystick connected to the current port.
*/
float Joystick::GetX(JoystickHand hand) const
{
return GetRawAxis(m_axes[kXAxis]);
float Joystick::GetX(JoystickHand hand) const {
return GetRawAxis(m_axes[kXAxis]);
}
/**
* Get the Y value of the joystick.
*
* This depends on the mapping of the joystick connected to the current port.
*/
float Joystick::GetY(JoystickHand hand) const
{
return GetRawAxis(m_axes[kYAxis]);
float Joystick::GetY(JoystickHand hand) const {
return GetRawAxis(m_axes[kYAxis]);
}
/**
* Get the Z value of the current joystick.
*
* This depends on the mapping of the joystick connected to the current port.
*/
float Joystick::GetZ() const
{
return GetRawAxis(m_axes[kZAxis]);
}
float Joystick::GetZ() const { return GetRawAxis(m_axes[kZAxis]); }
/**
* Get the twist value of the current joystick.
*
* This depends on the mapping of the joystick connected to the current port.
*/
float Joystick::GetTwist() const
{
return GetRawAxis(m_axes[kTwistAxis]);
}
float Joystick::GetTwist() const { return GetRawAxis(m_axes[kTwistAxis]); }
/**
* Get the throttle value of the current joystick.
*
* This depends on the mapping of the joystick connected to the current port.
*/
float Joystick::GetThrottle() const
{
return GetRawAxis(m_axes[kThrottleAxis]);
float Joystick::GetThrottle() const {
return GetRawAxis(m_axes[kThrottleAxis]);
}
/**
@@ -128,33 +121,36 @@ float Joystick::GetThrottle() const
* @param axis The axis to read [1-6].
* @return The value of the axis.
*/
float Joystick::GetRawAxis(uint32_t axis) const
{
return m_ds.GetStickAxis(m_port, axis);
float Joystick::GetRawAxis(uint32_t axis) const {
return m_ds.GetStickAxis(m_port, axis);
}
/**
* For the current joystick, return the axis determined by the argument.
*
* This is for cases where the joystick axis is returned programatically, otherwise one of the
* previous functions would be preferable (for example GetX()).
* This is for cases where the joystick axis is returned programatically,
* otherwise one of the previous functions would be preferable (for example
* GetX()).
*
* @param axis The axis to read.
* @return The value of the axis.
*/
float Joystick::GetAxis(AxisType axis) const
{
switch(axis)
{
case kXAxis: return this->GetX();
case kYAxis: return this->GetY();
case kZAxis: return this->GetZ();
case kTwistAxis: return this->GetTwist();
case kThrottleAxis: return this->GetThrottle();
default:
wpi_setWPIError(BadJoystickAxis);
return 0.0;
}
float Joystick::GetAxis(AxisType axis) const {
switch (axis) {
case kXAxis:
return this->GetX();
case kYAxis:
return this->GetY();
case kZAxis:
return this->GetZ();
case kTwistAxis:
return this->GetTwist();
case kThrottleAxis:
return this->GetThrottle();
default:
wpi_setWPIError(BadJoystickAxis);
return 0.0;
}
}
/**
@@ -162,12 +158,12 @@ float Joystick::GetAxis(AxisType axis) const
*
* Look up which button has been assigned to the trigger and read its state.
*
* @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface.
* @param hand This parameter is ignored for the Joystick class and is only here
* to complete the GenericHID interface.
* @return The state of the trigger.
*/
bool Joystick::GetTrigger(JoystickHand hand) const
{
return GetRawButton(m_buttons[kTriggerButton]);
bool Joystick::GetTrigger(JoystickHand hand) const {
return GetRawButton(m_buttons[kTriggerButton]);
}
/**
@@ -175,45 +171,45 @@ bool Joystick::GetTrigger(JoystickHand hand) const
*
* Look up which button has been assigned to the top and read its state.
*
* @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface.
* @param hand This parameter is ignored for the Joystick class and is only here
* to complete the GenericHID interface.
* @return The state of the top button.
*/
bool Joystick::GetTop(JoystickHand hand) const
{
return GetRawButton(m_buttons[kTopButton]);
bool Joystick::GetTop(JoystickHand hand) const {
return GetRawButton(m_buttons[kTopButton]);
}
/**
* This is not supported for the Joystick.
*
* This method is only here to complete the GenericHID interface.
*/
bool Joystick::GetBumper(JoystickHand hand) const
{
// Joysticks don't have bumpers.
return false;
bool Joystick::GetBumper(JoystickHand hand) const {
// Joysticks don't have bumpers.
return false;
}
/**
* Get the button value 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.
* 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);
*/
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.
*/
* 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
return 0; // TODO
}
/**
@@ -224,15 +220,15 @@ int Joystick::GetPOV(uint32_t pov) const {
* @param button The type of button to read.
* @return The state of the button.
*/
bool Joystick::GetButton(ButtonType button) const
{
switch (button)
{
case kTriggerButton: return GetTrigger();
case kTopButton: return GetTop();
default:
return false;
}
bool Joystick::GetButton(ButtonType button) const {
switch (button) {
case kTriggerButton:
return GetTrigger();
case kTopButton:
return GetTop();
default:
return false;
}
}
/**
@@ -241,10 +237,7 @@ bool Joystick::GetButton(ButtonType button) const
* @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];
}
uint32_t Joystick::GetAxisChannel(AxisType axis) { return m_axes[axis]; }
/**
* Set the channel associated with a specified axis.
@@ -252,40 +245,37 @@ uint32_t Joystick::GetAxisChannel(AxisType axis)
* @param axis The axis to set the channel for.
* @param channel The channel to set the axis to.
*/
void Joystick::SetAxisChannel(AxisType axis, uint32_t channel)
{
m_axes[axis] = channel;
void Joystick::SetAxisChannel(AxisType axis, uint32_t channel) {
m_axes[axis] = channel;
}
/**
* Get the magnitude of the direction vector formed by the joystick's
* current position relative to its origin
* 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) );
return sqrt(pow(GetX(), 2) + pow(GetY(), 2));
}
/**
* Get the direction of the vector formed by the joystick and its origin
* in radians
* in radians.
*
* @return The direction of the vector in radians
*/
float Joystick::GetDirectionRadians() const {
return atan2(GetX(), -GetY());
}
float Joystick::GetDirectionRadians() const { return atan2(GetX(), -GetY()); }
/**
* Get the direction of the vector formed by the joystick and its origin
* in degrees
* in degrees.
*
* uses acos(-1) to represent Pi due to absence of readily accessable Pi
* uses acos(-1) to represent Pi due to absence of readily accessable PI
* constant in C++
*
* @return The direction of the vector in degrees
*/
float Joystick::GetDirectionDegrees() const {
return (180/acos(-1))*GetDirectionRadians();
return (180 / acos(-1)) * GetDirectionRadians();
}

53
wpilibc/simulation/src/MotorSafetyHelper.cpp
View File

@@ -20,18 +20,17 @@ 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.
* 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.
* This is used to call the Stop() method on the motor.
*/
MotorSafetyHelper::MotorSafetyHelper(MotorSafety *safeObject)
MotorSafetyHelper::MotorSafetyHelper(MotorSafety* safeObject)
: m_safeObject(safeObject) {
m_enabled = false;
m_expiration = DEFAULT_SAFETY_EXPIRATION;
@@ -48,6 +47,7 @@ MotorSafetyHelper::~MotorSafetyHelper() {
/**
* Feed the motor safety object.
*
* Resets the timer on this object that is used to do the timeouts.
*/
void MotorSafetyHelper::Feed() {
@@ -57,6 +57,7 @@ void MotorSafetyHelper::Feed() {
/**
* Set the expiration time for the corresponding motor safety object.
*
* @param expirationTime The timeout value in seconds.
*/
void MotorSafetyHelper::SetExpiration(float expirationTime) {
@@ -66,6 +67,7 @@ void MotorSafetyHelper::SetExpiration(float expirationTime) {
/**
* Retrieve the timeout value for the corresponding motor safety object.
*
* @return the timeout value in seconds.
*/
float MotorSafetyHelper::GetExpiration() const {
@@ -75,8 +77,9 @@ float MotorSafetyHelper::GetExpiration() const {
/**
* 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.
* timed out.
*/
bool MotorSafetyHelper::IsAlive() const {
std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
@@ -85,30 +88,30 @@ bool MotorSafetyHelper::IsAlive() const {
/**
* 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.
* 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;
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.";
desc << "... Output not updated often enough.";
wpi_setWPIErrorWithContext(Timeout, desc.str().c_str());
m_safeObject->StopMotor();
}
}
/**
* Enable/disable motor safety for this device
* 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) {
@@ -117,8 +120,10 @@ void MotorSafetyHelper::SetSafetyEnabled(bool enabled) {
}
/**
* Return the state of the motor safety enabled flag
* 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 {
@@ -128,9 +133,9 @@ bool MotorSafetyHelper::IsSafetyEnabled() const {
/**
* 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.
*
* 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);

375
wpilibc/simulation/src/Notifier.cpp
View File

@@ -18,255 +18,246 @@ 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.
* 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);
}
}
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();
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)
{
// Delete the static variables when the last one is going away
if (refcount.fetch_sub(1) == 1) {
m_stopped = true;
m_task.join();
}
}
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);
// 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.
*
* 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()
{
}
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.
*
* 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();
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();
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_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();
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.
*
* 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;
}
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;
}
}
// 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 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();
}
if (!reschedule) {
/* Since the first element changed, update alarm, unless we already
* plan to
*/
UpdateAlarm();
}
m_queued = true;
}
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.
*
* 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);
}
}
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);
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.
*
* 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);
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.
*
* 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::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);
}
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);
}
}
}

687
wpilibc/simulation/src/PIDController.cpp
View File

@@ -6,10 +6,10 @@
/*----------------------------------------------------------------------------*/
#include "PIDController.h"
#include "Notifier.h"
#include "PIDSource.h"
#include "PIDOutput.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";
@@ -18,184 +18,172 @@ 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
* 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.
* @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);
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
* 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.
* @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);
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;
PIDSource* source, PIDOutput* output,
float period) {
m_table = nullptr;
m_P = Kp;
m_I = Ki;
m_D = Kd;
m_F = Kf;
m_P = Kp;
m_I = Ki;
m_D = Kd;
m_F = Kf;
m_maximumOutput = 1.0;
m_minimumOutput = -1.0;
m_maximumOutput = 1.0;
m_minimumOutput = -1.0;
m_maximumInput = 0;
m_minimumInput = 0;
m_maximumInput = 0;
m_minimumInput = 0;
m_continuous = false;
m_enabled = false;
m_setpoint = 0;
m_continuous = false;
m_enabled = false;
m_setpoint = 0;
m_prevError = 0;
m_totalError = 0;
m_tolerance = .05;
m_prevError = 0;
m_totalError = 0;
m_tolerance = .05;
m_result = 0;
m_result = 0;
m_pidInput = source;
m_pidOutput = output;
m_period = period;
m_pidInput = source;
m_pidOutput = output;
m_period = period;
m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
m_controlLoop->StartPeriodic(m_period);
m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
m_controlLoop->StartPeriodic(m_period);
static int32_t instances = 0;
instances++;
static int32_t instances = 0;
instances++;
m_toleranceType = kNoTolerance;
m_toleranceType = kNoTolerance;
}
PIDController::~PIDController() {
if (m_table != nullptr) m_table->RemoveTableListener(this);
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;
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;
}
{
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;
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;
}
}
}
{
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();
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 {
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;
}
}
} else {
m_totalError = m_maximumOutput / m_P;
}
}
m_result = m_P * m_error + m_I * m_totalError +
m_D * (m_error - m_prevError) + CalculateFeedForward();
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;
}
m_prevError = m_error;
} else {
m_totalError = m_maximumOutput / m_I;
}
}
if (m_result > m_maximumOutput) m_result = m_maximumOutput;
else if (m_result < m_minimumOutput) m_result = m_minimumOutput;
m_result = m_P * m_error + m_I * m_totalError +
m_D * (m_error - m_prevError) + CalculateFeedForward();
}
m_prevError = m_error;
pidOutput = m_pidOutput;
result = m_result;
}
if (m_result > m_maximumOutput)
m_result = m_maximumOutput;
else if (m_result < m_minimumOutput)
m_result = m_minimumOutput;
pidOutput->PIDWrite(result);
}
pidOutput = m_pidOutput;
result = m_result;
}
pidOutput->PIDWrite(result);
}
}
/**
* Calculate the feed forward term
* 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
* 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.
*
@@ -208,8 +196,7 @@ void PIDController::Calculate()
double PIDController::CalculateFeedForward() {
if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
return m_F * GetSetpoint();
}
else {
} else {
double temp = m_F * GetDeltaSetpoint();
m_prevSetpoint = m_setpoint;
m_setpointTimer.Reset();
@@ -219,115 +206,119 @@ double PIDController::CalculateFeedForward() {
/**
* 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;
}
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);
}
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;
}
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);
}
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
* Get the Proportional coefficient.
*
* @return proportional coefficient
*/
double PIDController::GetP() const
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_P;
double PIDController::GetP() const {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_P;
}
/**
* Get the Integral coefficient
* Get the Integral coefficient.
*
* @return integral coefficient
*/
double PIDController::GetI() const
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_I;
double PIDController::GetI() const {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_I;
}
/**
* Get the Differential coefficient
* Get the Differential coefficient.
*
* @return differential coefficient
*/
double PIDController::GetD() const
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_D;
double PIDController::GetD() const {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_D;
}
/**
* Get the Feed forward coefficient
* 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;
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 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;
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.
* 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;
void PIDController::SetContinuous(bool continuous) {
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_continuous = continuous;
}
/**
@@ -336,15 +327,14 @@ void PIDController::SetContinuous(bool continuous)
* @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;
}
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);
SetSetpoint(m_setpoint);
}
/**
@@ -353,85 +343,82 @@ void PIDController::SetInputRange(float minimumInput, float maximumInput)
* @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;
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
* 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);
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_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);
}
if (m_table != nullptr) {
m_table->PutNumber("setpoint", m_setpoint);
}
}
/**
* Returns the current setpoint of the PIDController
* 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;
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
* 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();
double PIDController::GetDeltaSetpoint() const {
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
}
/**
* Retruns the current difference of the input from the setpoint
* 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;
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
* 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
* Returns the type of input the PID controller is using.
*
* @return the PID controller input type
*/
PIDSourceType PIDController::GetPIDSourceType() const {
@@ -440,8 +427,10 @@ PIDSourceType PIDController::GetPIDSourceType() const {
/**
* 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 {
@@ -454,45 +443,46 @@ float PIDController::GetAvgError() const {
return avgError;
}
/*
/**
* Set the percentage error which is considered tolerable for use with
* OnTarget.
* @param percentage error which is tolerable
*
* @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;
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 percentage error which is tolerable
*
* @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;
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 percentage error which is tolerable
*
* @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;
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
/**
* 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
@@ -510,106 +500,101 @@ void PIDController::SetToleranceBuffer(unsigned bufLength) {
}
}
/*
/**
* 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.
* 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;
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
* Begin running the PIDController.
*/
void PIDController::Enable()
{
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_enabled = true;
}
void PIDController::Enable() {
{
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_enabled = true;
}
if (m_table != nullptr) {
m_table->PutBoolean("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;
}
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);
}
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;
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();
void PIDController::Reset() {
Disable();
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_prevError = 0;
m_totalError = 0;
m_result = 0;
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";
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);
}
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;
}
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) {
@@ -634,14 +619,8 @@ void PIDController::ValueChanged(ITable* source, llvm::StringRef key,
}
}
void PIDController::UpdateTable() {
void PIDController::UpdateTable() {}
}
void PIDController::StartLiveWindowMode() { Disable(); }
void PIDController::StartLiveWindowMode() {
Disable();
}
void PIDController::StopLiveWindowMode() {
}
void PIDController::StopLiveWindowMode() {}

216
wpilibc/simulation/src/PWM.cpp
View File

@@ -22,72 +22,77 @@ const int32_t PWM::kPwmDisabled = 0;
* 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
* port
*/
PWM::PWM(uint32_t channel)
{
char buf[64];
PWM::PWM(uint32_t channel) {
char buf[64];
if (!CheckPWMChannel(channel))
{
snprintf(buf, 64, "PWM Channel %d", channel);
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
return;
}
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;
sprintf(buf, "pwm/%d", channel);
impl = new SimContinuousOutput(buf);
m_channel = channel;
m_eliminateDeadband = false;
m_centerPwm = kPwmDisabled; // In simulation, the same thing.
m_centerPwm = kPwmDisabled; // In simulation, the same thing.
}
PWM::~PWM() {
if (m_table != nullptr) m_table->RemoveTableListener(this);
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.
*
* @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;
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
*
* 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 center The center speed (off)
* @param deadbandMin The low end of the deadband range
* @param min The minimum pwm value
* @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.
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
*
* 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 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
* @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.
void PWM::SetBounds(double max, double deadbandMax, double center,
double deadbandMin, double min) {
// Nothing to do in simulation.
}
/**
@@ -100,18 +105,14 @@ void PWM::SetBounds(double max, double deadbandMax, double center, double deadba
*
* @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;
}
void PWM::SetPosition(float pos) {
if (pos < 0.0) {
pos = 0.0;
} else if (pos > 1.0) {
pos = 1.0;
}
impl->Set(pos);
impl->Set(pos);
}
/**
@@ -124,21 +125,15 @@ void PWM::SetPosition(float pos)
*
* @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;
}
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;
}
}
/**
@@ -154,19 +149,15 @@ float PWM::GetPosition() const
*
* @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;
}
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);
impl->Set(speed);
}
/**
@@ -181,21 +172,15 @@ void PWM::SetSpeed(float speed)
*
* @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;
}
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;
}
}
/**
@@ -205,10 +190,9 @@ float PWM::GetSpeed() const
*
* @param value Raw PWM value.
*/
void PWM::SetRaw(unsigned short value)
{
wpi_assert(value == kPwmDisabled);
impl->Set(0);
void PWM::SetRaw(unsigned short value) {
wpi_assert(value == kPwmDisabled);
impl->Set(0);
}
/**
@@ -216,12 +200,10 @@ void PWM::SetRaw(unsigned short value)
*
* @param mult The period multiplier to apply to this channel
*/
void PWM::SetPeriodMultiplier(PeriodMultiplier mult)
{
// Do nothing in simulation.
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;
@@ -229,34 +211,30 @@ void PWM::ValueChanged(ITable* source, llvm::StringRef key,
}
void PWM::UpdateTable() {
if (m_table != nullptr) {
m_table->PutNumber("Value", GetSpeed());
}
if (m_table != nullptr) {
m_table->PutNumber("Value", GetSpeed());
}
}
void PWM::StartLiveWindowMode() {
SetSpeed(0);
if (m_table != nullptr) {
m_table->AddTableListener("Value", this, true);
}
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);
}
SetSpeed(0);
if (m_table != nullptr) {
m_table->RemoveTableListener(this);
}
}
std::string PWM::GetSmartDashboardType() const {
return "Speed Controller";
}
std::string PWM::GetSmartDashboardType() const { return "Speed Controller"; }
void PWM::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> PWM::GetTable() const {
return m_table;
}
std::shared_ptr<ITable> PWM::GetTable() const { return m_table; }

256
wpilibc/simulation/src/Relay.cpp
View File

@@ -9,118 +9,108 @@
#include "MotorSafetyHelper.h"
//#include "NetworkCommunication/UsageReporting.h"
#include "WPIErrors.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 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_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);
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;
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);
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.
* 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.
* 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));
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));
}
/**
@@ -129,29 +119,29 @@ void Relay::Set(Relay::Value value)
* 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)
* 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;
}
// 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;
}
uint32_t Relay::GetChannel() const { return m_channel; }
/**
* Set the expiration time for the Relay object
* Set the expiration time for the Relay object.
*
* @param timeout The timeout (in seconds) for this relay object
*/
void Relay::SetExpiration(float timeout) {
@@ -160,19 +150,22 @@ void Relay::SetExpiration(float 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.
* @returns a bool value that is true if the motor has NOT timed out and should
* still be running.
*
* @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.
*/
@@ -180,7 +173,9 @@ 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) {
@@ -188,8 +183,9 @@ void Relay::SetSafetyEnabled(bool enabled) {
}
/**
* Check if motor safety is enabled for this object
* @returns True if motor safety is enforced for this object
* 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();
@@ -202,49 +198,45 @@ void Relay::GetDescription(std::ostringstream& desc) const {
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);
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");
}
}
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);
}
if (m_table != nullptr) {
m_table->AddTableListener("Value", this, true);
}
}
void Relay::StopLiveWindowMode() {
if(m_table != nullptr){
m_table->RemoveTableListener(this);
}
if (m_table != nullptr) {
m_table->RemoveTableListener(this);
}
}
std::string Relay::GetSmartDashboardType() const {
return "Relay";
}
std::string Relay::GetSmartDashboardType() const { return "Relay"; }
void Relay::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> Relay::GetTable() const {
return m_table;
}
std::shared_ptr<ITable> Relay::GetTable() const { return m_table; }

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

@@ -13,88 +13,77 @@
RobotBase* RobotBase::m_instance = nullptr;
void RobotBase::setInstance(RobotBase* robot)
{
wpi_assert(m_instance == nullptr);
m_instance = robot;
void RobotBase::setInstance(RobotBase* robot) {
wpi_assert(m_instance == nullptr);
m_instance = robot;
}
RobotBase &RobotBase::getInstance()
{
return *m_instance;
}
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.
* 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);
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();
}
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();
}
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.
*
* @return True if the robot is currently operating Autonomously as determined
* by the field controls.
*/
bool RobotBase::IsAutonomous() const
{
return m_ds.IsAutonomous();
}
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.
*
* @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();
}
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.
*
* @return True if the robot is currently running tests as determined by the
* field controls.
*/
bool RobotBase::IsTest() const
{
return m_ds.IsTest();
}
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.
* 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();
}
class RobotDeleter {
public:
~RobotDeleter() { delete &RobotBase::getInstance(); }
};
static RobotDeleter g_robotDeleter;

980
wpilibc/simulation/src/RobotDrive.cpp
View File

File diff suppressed because it is too large Load Diff

81
wpilibc/simulation/src/SafePWM.cpp
View File

@@ -6,89 +6,84 @@
/*----------------------------------------------------------------------------*/
#include "SafePWM.h"
#include <sstream>
#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);
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
/**
* 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);
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();
}
float SafePWM::GetExpiration() const { return m_safetyHelper->GetExpiration(); }
/**
* Check if the PWM object is currently alive or stopped due to a timeout.
* @returns a bool value that is true if the motor has NOT timed out and should still
* be running.
*
* @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();
}
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.
*
* 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);
}
void SafePWM::StopMotor() { SetRaw(kPwmDisabled); }
/**
* Enable/disable motor safety for this device
* 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);
void SafePWM::SetSafetyEnabled(bool enabled) {
m_safetyHelper->SetSafetyEnabled(enabled);
}
/**
* Check if motor safety is enabled for this object
* 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();
bool SafePWM::IsSafetyEnabled() const {
return m_safetyHelper->IsSafetyEnabled();
}
void SafePWM::GetDescription(std::ostringstream& desc) const
{
desc << "PWM " << GetChannel();
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.
*
* 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();
void SafePWM::SetSpeed(float speed) {
PWM::SetSpeed(speed);
m_safetyHelper->Feed();
}

184
wpilibc/simulation/src/SampleRobot.cpp
View File

@@ -8,151 +8,137 @@
#include "SampleRobot.h"
#include <stdio.h>
#include "Timer.h"
#include "SmartDashboard/SmartDashboard.h"
#include "LiveWindow/LiveWindow.h"
#include "SmartDashboard/SmartDashboard.h"
#include "Timer.h"
#include "networktables/NetworkTable.h"
#if defined(_UNIX)
#include <unistd.h>
#include <unistd.h>
#elif defined(_WIN32)
#include <windows.h>
void sleep(unsigned milliseconds)
{
Sleep(milliseconds);
}
#include <windows.h>
void sleep(unsigned milliseconds) { Sleep(milliseconds); }
#endif
SampleRobot::SampleRobot()
: m_robotMainOverridden (true)
{
}
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.
* 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__);
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.
*
* 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__);
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.
*
* 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__);
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.
*
* 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__);
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
*
* 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__);
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 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.
* 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;
}
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
*
* 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();
void SampleRobot::StartCompetition() {
LiveWindow* lw = LiveWindow::GetInstance();
SmartDashboard::init();
NetworkTable::GetTable("LiveWindow")->GetSubTable("~STATUS~")->PutBoolean("LW Enabled", false);
SmartDashboard::init();
NetworkTable::GetTable("LiveWindow")
->GetSubTable("~STATUS~")
->PutBoolean("LW Enabled", false);
RobotMain();
RobotMain();
if (!m_robotMainOverridden)
{
// first and one-time initialization
lw->SetEnabled(false);
RobotInit();
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();
}
}
}
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();
}
}
}
}

122
wpilibc/simulation/src/SensorBase.cpp
View File

@@ -17,44 +17,42 @@ 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;
SensorBase* SensorBase::m_singletonList = nullptr;
/**
* Creates an instance of the sensor base and gets an FPGA handle
*/
SensorBase::SensorBase()
{
}
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.
* resources and need to have their destructors called at the end of the
* program.
*/
void SensorBase::AddToSingletonList()
{
m_nextSingleton = m_singletonList;
m_singletonList = this;
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;
void SensorBase::DeleteSingletons() {
for (SensorBase* next = m_singletonList; next != nullptr;) {
SensorBase* tmp = next;
next = next->m_nextSingleton;
delete tmp;
}
m_singletonList = nullptr;
}
/**
@@ -62,91 +60,83 @@ void SensorBase::DeleteSingletons()
*
* @return Solenoid module is valid and present
*/
bool SensorBase::CheckSolenoidModule(uint8_t moduleNumber)
{
return 1 <= moduleNumber && moduleNumber <= 2; // TODO: Fix for Athena
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.
*
* 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;
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.
*
* 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;
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.
*
* 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;
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.
*
* 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;
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.
*
* 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;
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;
bool SensorBase::CheckSolenoidChannel(uint32_t channel) {
if (channel > 0 && channel <= kSolenoidChannels) return true;
return false;
}
/**
@@ -154,9 +144,7 @@ bool SensorBase::CheckSolenoidChannel(uint32_t channel)
*
* @return PDP channel is valid
*/
bool SensorBase::CheckPDPChannel(uint32_t channel)
{
if (channel > 0 && channel <= kPDPChannels)
return true;
return false;
bool SensorBase::CheckPDPChannel(uint32_t channel) {
if (channel > 0 && channel <= kPDPChannels) return true;
return false;
}

70
wpilibc/simulation/src/Solenoid.cpp
View File

@@ -6,8 +6,8 @@
/*----------------------------------------------------------------------------*/
#include "Solenoid.h"
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"
#include "WPIErrors.h"
#include "simulation/simTime.h"
/**
@@ -21,20 +21,19 @@ 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).
* @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(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);
if (m_table != nullptr) m_table->RemoveTableListener(this);
}
/**
@@ -42,10 +41,9 @@ Solenoid::~Solenoid() {
*
* @param on Turn the solenoid output off or on.
*/
void Solenoid::Set(bool on)
{
m_on = on;
m_impl->Set(on ? 1 : -1);
void Solenoid::Set(bool on) {
m_on = on;
m_impl->Set(on ? 1 : -1);
}
/**
@@ -53,47 +51,39 @@ void Solenoid::Set(bool on)
*
* @return The current value of the solenoid.
*/
bool Solenoid::Get() const
{
return m_on;
}
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());
Set(value->GetBoolean());
}
void Solenoid::UpdateTable() {
if (m_table != nullptr) {
m_table->PutBoolean("Value", Get());
}
if (m_table != nullptr) {
m_table->PutBoolean("Value", Get());
}
}
void Solenoid::StartLiveWindowMode() {
Set(false);
if (m_table != nullptr) {
m_table->AddTableListener("Value", this, true);
}
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);
}
Set(false);
if (m_table != nullptr) {
m_table->RemoveTableListener(this);
}
}
std::string Solenoid::GetSmartDashboardType() const {
return "Solenoid";
}
std::string Solenoid::GetSmartDashboardType() const { return "Solenoid"; }
void Solenoid::InitTable(std::shared_ptr<ITable> subTable) {
m_table = subTable;
UpdateTable();
m_table = subTable;
UpdateTable();
}
std::shared_ptr<ITable> Solenoid::GetTable() const {
return m_table;
}
std::shared_ptr<ITable> Solenoid::GetTable() const { return m_table; }

25
wpilibc/simulation/src/Talon.cpp
View File

@@ -13,8 +13,7 @@
/**
* @param channel The PWM channel that the Talon is attached to.
*/
Talon::Talon(uint32_t channel) : SafePWM(channel)
{
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
@@ -41,38 +40,26 @@ Talon::Talon(uint32_t channel) : SafePWM(channel)
* 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 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);
}
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();
}
float Talon::Get() const { return GetSpeed(); }
/**
* Common interface for disabling a motor.
*/
void Talon::Disable()
{
SetRaw(kPwmDisabled);
}
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);
}
void Talon::PIDWrite(float output) { Set(output); }

209
wpilibc/simulation/src/Timer.cpp
View File

@@ -9,152 +9,144 @@
#include <time.h>
#include "simulation/simTime.h"
#include "Utility.h"
#include "simulation/simTime.h"
// Internal stuff
#include "simulation/SimFloatInput.h"
#include "simulation/MainNode.h"
namespace wpilib { namespace internal {
double simTime = 0;
std::condition_variable time_wait;
std::mutex time_wait_mutex;
#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();
}
}}
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.
* 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;
void Wait(double seconds) {
if (seconds < 0.0) return;
double start = wpilib::internal::simTime;
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);
}
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();
}
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
* @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
// 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.
* 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();
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.
* 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();
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;
}
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;
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
* 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();
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;
}
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();
void Timer::Stop() {
double temp = Get();
std::lock_guard<priority_mutex> sync(m_mutex);
if (m_running)
{
m_accumulatedTime = temp;
m_running = false;
}
std::lock_guard<priority_mutex> sync(m_mutex);
if (m_running) {
m_accumulatedTime = temp;
m_running = false;
}
}
/**
@@ -165,45 +157,38 @@ void Timer::Stop()
* @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;
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.
* @returns Robot running time in seconds.
* 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;
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();
}
double Timer::GetMatchTime() { return Timer::GetFPGATimestamp(); }
// Internal function that reads the PPC timestamp counter.
extern "C"
{
uint32_t niTimestamp32(void);
uint64_t niTimestamp64(void);
extern "C" {
uint32_t niTimestamp32(void);
uint64_t niTimestamp64(void);
}

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

@@ -5,26 +5,21 @@
/* the project. */
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Utility.h"
#include "Timer.h"
#include "simulation/simTime.h"
#include <stdio.h>
#include <string.h>
#include "Timer.h"
#include "simulation/simTime.h"
#include <iostream>
#include <sstream>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <sstream>
#if not defined(_WIN32)
#include <execinfo.h>
#include <cxxabi.h>
#include <cxxabi.h>
#include <execinfo.h>
#endif
static bool stackTraceEnabled = false;
@@ -33,40 +28,37 @@ static bool suspendOnAssertEnabled = false;
/**
* Enable Stack trace after asserts.
*/
void wpi_stackTraceOnAssertEnable(bool enabled)
{
stackTraceEnabled = enabled;
}
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.
* 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;
void wpi_suspendOnAssertEnabled(bool enabled) {
suspendOnAssertEnabled = enabled;
}
static void wpi_handleTracing()
{
// if (stackTraceEnabled)
// {
// printf("\n-----------<Stack Trace>----------------\n");
// printCurrentStackTrace();
// }
printf("\n");
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.
* 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) {
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;
@@ -90,15 +82,15 @@ bool wpi_assert_impl(bool conditionValue, const char *conditionText,
/**
* 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.
* 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,
const std::string& equalityType,
const std::string& message,
const std::string& fileName,
uint32_t lineNumber,
const std::string &funcName) {
const std::string& funcName) {
// Error string buffer
std::stringstream error;
@@ -124,116 +116,97 @@ void wpi_assertEqual_common_impl(int valueA, int valueB,
* 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.
* 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;
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.
* 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;
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).
* @return The current time in microseconds according to the FPGA (since FPGA
* reset).
*/
uint64_t GetFPGATime()
{
return wpilib::internal::simTime * 1e6;
}
uint64_t GetFPGATime() { return wpilib::internal::simTime * 1e6; }
//TODO: implement symbol demangling and backtrace on windows
// 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;
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 (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 (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;
// 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;
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;
}
}
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);
free(mangledSymbols);
return trace.str();
return trace.str();
}
#else
static std::string demangle(char const *mangledSymbol)
{
return "no demangling on windows";
static std::string demangle(char const* mangledSymbol) {
return "no demangling on windows";
}
std::string GetStackTrace(uint32_t offset)
{
return "no stack trace on windows";
std::string GetStackTrace(uint32_t offset) {
return "no stack trace on windows";
}
#endif

25
wpilibc/simulation/src/Victor.cpp
View File

@@ -13,8 +13,7 @@
/**
* @param channel The PWM channel that the Victor is attached to.
*/
Victor::Victor(uint32_t channel) : SafePWM(channel)
{
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
@@ -43,38 +42,26 @@ Victor::Victor(uint32_t channel) : SafePWM(channel)
* 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 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);
}
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();
}
float Victor::Get() const { return GetSpeed(); }
/**
* Common interface for disabling a motor.
*/
void Victor::Disable()
{
SetRaw(kPwmDisabled);
}
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);
}
void Victor::PIDWrite(float output) { Set(output); }

14
wpilibc/simulation/src/simulation/SimContinuousOutput.cpp
View File

@@ -9,16 +9,14 @@
#include "simulation/MainNode.h"
SimContinuousOutput::SimContinuousOutput(std::string topic) {
pub = MainNode::Advertise<msgs::Float64>("~/simulator/"+topic);
std::cout << "Initialized ~/simulator/"+topic << std::endl;
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);
msgs::Float64 msg;
msg.set_data(speed);
pub->Publish(msg);
}
float SimContinuousOutput::Get() {
return speed;
}
float SimContinuousOutput::Get() { return speed; }

11
wpilibc/simulation/src/simulation/SimDigitalInput.cpp
View File

@@ -9,14 +9,13 @@
#include "simulation/MainNode.h"
SimDigitalInput::SimDigitalInput(std::string topic) {
sub = MainNode::Subscribe("~/simulator/"+topic, &SimDigitalInput::callback, this);
std::cout << "Initialized ~/simulator/"+topic << std::endl;
sub = MainNode::Subscribe("~/simulator/" + topic, &SimDigitalInput::callback,
this);
std::cout << "Initialized ~/simulator/" + topic << std::endl;
}
bool SimDigitalInput::Get() {
return value;
}
bool SimDigitalInput::Get() { return value; }
void SimDigitalInput::callback(const msgs::ConstBoolPtr &msg) {
void SimDigitalInput::callback(const msgs::ConstBoolPtr& msg) {
value = msg->data();
}

60
wpilibc/simulation/src/simulation/SimEncoder.cpp
View File

@@ -9,52 +9,44 @@
#include "simulation/MainNode.h"
SimEncoder::SimEncoder(std::string topic) {
commandPub = MainNode::Advertise<msgs::GzString>("~/simulator/"+topic+"/control");
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);
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;
}
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::Reset() { sendCommand("reset"); }
void SimEncoder::Start() {
sendCommand("start");
}
void SimEncoder::Start() { sendCommand("start"); }
void SimEncoder::Stop() {
sendCommand("stop");
}
void SimEncoder::Stop() { sendCommand("stop"); }
double SimEncoder::GetPosition() {
return position;
}
double SimEncoder::GetVelocity() {
return velocity;
}
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);
msgs::GzString msg;
msg.set_data(cmd);
commandPub->Publish(msg);
}
void SimEncoder::positionCallback(const msgs::ConstFloat64Ptr &msg) {
position = msg->data();
void SimEncoder::positionCallback(const msgs::ConstFloat64Ptr& msg) {
position = msg->data();
}
void SimEncoder::velocityCallback(const msgs::ConstFloat64Ptr &msg) {
velocity = msg->data();
void SimEncoder::velocityCallback(const msgs::ConstFloat64Ptr& msg) {
velocity = msg->data();
}

11
wpilibc/simulation/src/simulation/SimFloatInput.cpp
View File

@@ -9,14 +9,13 @@
#include "simulation/MainNode.h"
SimFloatInput::SimFloatInput(std::string topic) {
sub = MainNode::Subscribe("~/simulator/"+topic, &SimFloatInput::callback, this);
std::cout << "Initialized ~/simulator/"+topic << std::endl;
sub = MainNode::Subscribe("~/simulator/" + topic, &SimFloatInput::callback,
this);
std::cout << "Initialized ~/simulator/" + topic << std::endl;
}
double SimFloatInput::Get() {
return value;
}
double SimFloatInput::Get() { return value; }
void SimFloatInput::callback(const msgs::ConstFloat64Ptr &msg) {
void SimFloatInput::callback(const msgs::ConstFloat64Ptr& msg) {
value = msg->data();
}

48
wpilibc/simulation/src/simulation/SimGyro.cpp
View File

@@ -9,32 +9,29 @@
#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);
commandPub =
MainNode::Advertise<msgs::GzString>("~/simulator/" + topic + "/control");
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;
}
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");
}
void SimGyro::Reset() { sendCommand("reset"); }
double SimGyro::GetAngle() {
return position;
}
double SimGyro::GetVelocity() {
return velocity;
}
double SimGyro::GetAngle() { return position; }
double SimGyro::GetVelocity() { return velocity; }
void SimGyro::sendCommand(std::string cmd) {
msgs::GzString msg;
@@ -42,11 +39,10 @@ void SimGyro::sendCommand(std::string cmd) {
commandPub->Publish(msg);
}
void SimGyro::positionCallback(const msgs::ConstFloat64Ptr &msg) {
position = msg->data();
void SimGyro::positionCallback(const msgs::ConstFloat64Ptr& msg) {
position = msg->data();
}
void SimGyro::velocityCallback(const msgs::ConstFloat64Ptr &msg) {
velocity = msg->data();
void SimGyro::velocityCallback(const msgs::ConstFloat64Ptr& msg) {
velocity = msg->data();
}