Squashed 'third_party/allwpilib_2017/' content from commit 35ac87d
Change-Id: I7bb6f5556c30d3f5a092e68de0be9c710c60c9f4
git-subtree-dir: third_party/allwpilib_2017
git-subtree-split: 35ac87d6ff8b7f061c4f18c9ea316e5dccd4888a
diff --git a/wpilibc/sim/src/AnalogGyro.cpp b/wpilibc/sim/src/AnalogGyro.cpp
new file mode 100644
index 0000000..77b231c
--- /dev/null
+++ b/wpilibc/sim/src/AnalogGyro.cpp
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogGyro.h"
+
+#include <sstream>
+
+#include "LiveWindow/LiveWindow.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+const int AnalogGyro::kOversampleBits = 10;
+const int AnalogGyro::kAverageBits = 0;
+const double AnalogGyro::kSamplesPerSecond = 50.0;
+const double AnalogGyro::kCalibrationSampleTime = 5.0;
+const double AnalogGyro::kDefaultVoltsPerDegreePerSecond = 0.007;
+
+/**
+ * Initialize the gyro.
+ *
+ * Calibrate the gyro by running for a number of samples and computing the
+ * center value for this part. Then use the center value as the Accumulator
+ * center value for subsequent measurements. It's important to make sure that
+ * the robot is not moving while the centering calculations are in progress,
+ * this is typically done when the robot is first turned on while it's sitting
+ * at rest before the competition starts.
+ */
+void AnalogGyro::InitAnalogGyro(int channel) {
+ SetPIDSourceType(PIDSourceType::kDisplacement);
+
+ std::stringstream ss;
+ ss << "analog/" << channel;
+ impl = new SimGyro(ss.str());
+
+ LiveWindow::GetInstance()->AddSensor("AnalogGyro", channel, this);
+}
+
+/**
+ * AnalogGyro constructor with only a channel.
+ *
+ * @param channel The analog channel the gyro is connected to.
+ */
+AnalogGyro::AnalogGyro(int channel) { InitAnalogGyro(channel); }
+
+/**
+ * Reset the gyro.
+ *
+ * Resets the gyro to a heading of zero. This can be used if there is
+ * significant drift in the gyro and it needs to be recalibrated after it has
+ * been running.
+ */
+void AnalogGyro::Reset() { impl->Reset(); }
+
+void AnalogGyro::Calibrate() { Reset(); }
+
+/**
+ * Return the actual angle in degrees that the robot is currently facing.
+ *
+ * The angle is based on the current accumulator value corrected by the
+ * oversampling rate, the gyro type and the A/D calibration values. The angle
+ * is continuous, that is can go beyond 360 degrees. This make algorithms that
+ * wouldn't want to see a discontinuity in the gyro output as it sweeps past 0
+ * on the second time around.
+ *
+ * @return the current heading of the robot in degrees. This heading is based on
+ * integration of the returned rate from the gyro.
+ */
+double AnalogGyro::GetAngle() const { return impl->GetAngle(); }
+
+/**
+ * Return the rate of rotation of the gyro
+ *
+ * The rate is based on the most recent reading of the gyro analog value
+ *
+ * @return the current rate in degrees per second
+ */
+double AnalogGyro::GetRate() const { return impl->GetVelocity(); }
diff --git a/wpilibc/sim/src/AnalogInput.cpp b/wpilibc/sim/src/AnalogInput.cpp
new file mode 100644
index 0000000..0c00b6e
--- /dev/null
+++ b/wpilibc/sim/src/AnalogInput.cpp
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogInput.h"
+
+#include <sstream>
+
+#include "LiveWindow/LiveWindow.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+/**
+ * Construct an analog input.
+ *
+ * @param channel The channel number to represent.
+ */
+AnalogInput::AnalogInput(int channel) : m_channel(channel) {
+ std::stringstream ss;
+ ss << "analog/" << channel;
+ m_impl = new SimFloatInput(ss.str());
+
+ LiveWindow::GetInstance()->AddSensor("AnalogInput", channel, this);
+}
+
+/**
+ * Get a scaled sample straight from this channel.
+ *
+ * The value is scaled to units of Volts using the calibrated scaling data from
+ * GetLSBWeight() and GetOffset().
+ *
+ * @return A scaled sample straight from this channel.
+ */
+double 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.
+ */
+double AnalogInput::GetAverageVoltage() const { return m_impl->Get(); }
+
+/**
+ * Get the channel number.
+ *
+ * @return The channel number.
+ */
+int AnalogInput::GetChannel() const { return m_channel; }
+
+/**
+ * Get the Average value for the PID Source base object.
+ *
+ * @return The average voltage.
+ */
+double AnalogInput::PIDGet() { return GetAverageVoltage(); }
+
+void AnalogInput::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", GetAverageVoltage());
+ }
+}
+
+void AnalogInput::StartLiveWindowMode() {}
+
+void AnalogInput::StopLiveWindowMode() {}
+
+std::string AnalogInput::GetSmartDashboardType() const {
+ return "Analog Input";
+}
+
+void AnalogInput::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> AnalogInput::GetTable() const { return m_table; }
diff --git a/wpilibc/sim/src/AnalogPotentiometer.cpp b/wpilibc/sim/src/AnalogPotentiometer.cpp
new file mode 100644
index 0000000..dcffea6
--- /dev/null
+++ b/wpilibc/sim/src/AnalogPotentiometer.cpp
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogPotentiometer.h"
+
+using namespace frc;
+
+/**
+ * Common initialization code called by all constructors.
+ */
+void AnalogPotentiometer::initPot(AnalogInput* input, double scale,
+ double offset) {
+ m_scale = scale;
+ m_offset = offset;
+ m_analog_input = input;
+}
+
+AnalogPotentiometer::AnalogPotentiometer(int channel, double scale,
+ double offset) {
+ m_init_analog_input = true;
+ initPot(new AnalogInput(channel), scale, offset);
+}
+
+AnalogPotentiometer::AnalogPotentiometer(AnalogInput* input, double scale,
+ double offset) {
+ m_init_analog_input = false;
+ initPot(input, scale, offset);
+}
+
+AnalogPotentiometer::AnalogPotentiometer(AnalogInput& input, double scale,
+ double offset) {
+ m_init_analog_input = false;
+ initPot(&input, scale, offset);
+}
+
+AnalogPotentiometer::~AnalogPotentiometer() {
+ if (m_init_analog_input) {
+ delete m_analog_input;
+ m_init_analog_input = false;
+ }
+}
+
+/**
+ * Get the current reading of the potentiomere.
+ *
+ * @return The current position of the potentiometer.
+ */
+double AnalogPotentiometer::Get() const {
+ return m_analog_input->GetVoltage() * m_scale + m_offset;
+}
+
+/**
+ * Implement the PIDSource interface.
+ *
+ * @return The current reading.
+ */
+double AnalogPotentiometer::PIDGet() { return Get(); }
+
+/**
+ * @return the Smart Dashboard Type
+ */
+std::string AnalogPotentiometer::GetSmartDashboardType() const {
+ return "Analog Input";
+}
+
+/**
+ * Live Window code, only does anything if live window is activated.
+ */
+void AnalogPotentiometer::InitTable(std::shared_ptr<ITable> subtable) {
+ m_table = subtable;
+ UpdateTable();
+}
+
+void AnalogPotentiometer::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", Get());
+ }
+}
+
+std::shared_ptr<ITable> AnalogPotentiometer::GetTable() const {
+ return m_table;
+}
diff --git a/wpilibc/sim/src/DigitalInput.cpp b/wpilibc/sim/src/DigitalInput.cpp
new file mode 100644
index 0000000..a560761
--- /dev/null
+++ b/wpilibc/sim/src/DigitalInput.cpp
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "DigitalInput.h"
+
+#include <sstream>
+
+#include "WPIErrors.h"
+
+using namespace frc;
+
+/**
+ * Create an instance of a Digital Input class.
+ * Creates a digital input given a channel and uses the default module.
+ *
+ * @param channel The digital channel (1..14).
+ */
+DigitalInput::DigitalInput(int channel) : m_channel(channel) {
+ std::stringstream ss;
+ ss << "dio/" << channel;
+ m_impl = new SimDigitalInput(ss.str());
+}
+
+/**
+ * Get the value from a digital input channel.
+ * Retrieve the value of a single digital input channel from the FPGA.
+ */
+int DigitalInput::Get() const { return m_impl->Get(); }
+
+/**
+ * @return The GPIO channel number that this object represents.
+ */
+int DigitalInput::GetChannel() const { return m_channel; }
+
+void DigitalInput::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutBoolean("Value", Get());
+ }
+}
+
+void DigitalInput::StartLiveWindowMode() {}
+
+void DigitalInput::StopLiveWindowMode() {}
+
+std::string DigitalInput::GetSmartDashboardType() const {
+ return "DigitalInput";
+}
+
+void DigitalInput::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> DigitalInput::GetTable() const { return m_table; }
diff --git a/wpilibc/sim/src/DoubleSolenoid.cpp b/wpilibc/sim/src/DoubleSolenoid.cpp
new file mode 100644
index 0000000..1607dc8
--- /dev/null
+++ b/wpilibc/sim/src/DoubleSolenoid.cpp
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "DoubleSolenoid.h"
+
+#include "LiveWindow/LiveWindow.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+/**
+ * Constructor.
+ *
+ * @param forwardChannel The forward channel on the module to control.
+ * @param reverseChannel The reverse channel on the module to control.
+ */
+DoubleSolenoid::DoubleSolenoid(int forwardChannel, int reverseChannel)
+ : DoubleSolenoid(1, forwardChannel, reverseChannel) {}
+
+/**
+ * Constructor.
+ *
+ * @param moduleNumber The solenoid module (1 or 2).
+ * @param forwardChannel The forward channel on the module to control.
+ * @param reverseChannel The reverse channel on the module to control.
+ */
+DoubleSolenoid::DoubleSolenoid(int moduleNumber, int forwardChannel,
+ int reverseChannel) {
+ m_reversed = false;
+ if (reverseChannel < forwardChannel) { // Swap ports to get the right address
+ int channel = reverseChannel;
+ reverseChannel = forwardChannel;
+ forwardChannel = channel;
+ m_reversed = true;
+ }
+ std::stringstream ss;
+ ss << "pneumatic/" << moduleNumber << "/" << forwardChannel << "/"
+ << moduleNumber << "/" << reverseChannel;
+ m_impl = new SimContinuousOutput(ss.str());
+
+ LiveWindow::GetInstance()->AddActuator("DoubleSolenoid", moduleNumber,
+ forwardChannel, this);
+}
+
+DoubleSolenoid::~DoubleSolenoid() {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Set the value of a solenoid.
+ *
+ * @param value Move the solenoid to forward, reverse, or don't move it.
+ */
+void DoubleSolenoid::Set(Value value) {
+ m_value = value;
+ switch (value) {
+ case kOff:
+ m_impl->Set(0);
+ break;
+ case kForward:
+ m_impl->Set(m_reversed ? -1 : 1);
+ break;
+ case kReverse:
+ m_impl->Set(m_reversed ? 1 : -1);
+ break;
+ }
+}
+
+/**
+ * Read the current value of the solenoid.
+ *
+ * @return The current value of the solenoid.
+ */
+DoubleSolenoid::Value DoubleSolenoid::Get() const { return m_value; }
+
+void DoubleSolenoid::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value,
+ bool isNew) {
+ if (!value->IsString()) return;
+ Value lvalue = kOff;
+ if (value->GetString() == "Forward")
+ lvalue = kForward;
+ else if (value->GetString() == "Reverse")
+ lvalue = kReverse;
+ Set(lvalue);
+}
+
+void DoubleSolenoid::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutString(
+ "Value", (Get() == kForward ? "Forward"
+ : (Get() == kReverse ? "Reverse" : "Off")));
+ }
+}
+
+void DoubleSolenoid::StartLiveWindowMode() {
+ Set(kOff);
+ if (m_table != nullptr) {
+ m_table->AddTableListener("Value", this, true);
+ }
+}
+
+void DoubleSolenoid::StopLiveWindowMode() {
+ Set(kOff);
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string DoubleSolenoid::GetSmartDashboardType() const {
+ return "Double Solenoid";
+}
+
+void DoubleSolenoid::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> DoubleSolenoid::GetTable() const { return m_table; }
diff --git a/wpilibc/sim/src/DriverStation.cpp b/wpilibc/sim/src/DriverStation.cpp
new file mode 100644
index 0000000..1e43556
--- /dev/null
+++ b/wpilibc/sim/src/DriverStation.cpp
@@ -0,0 +1,410 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "DriverStation.h"
+
+#include <boost/mem_fn.hpp>
+
+#include "HAL/cpp/Log.h"
+#include "Timer.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+#include "simulation/MainNode.h"
+
+using namespace frc;
+
+const int DriverStation::kBatteryChannel;
+const int DriverStation::kJoystickPorts;
+const int DriverStation::kJoystickAxes;
+const double DriverStation::kUpdatePeriod = 0.02;
+int DriverStation::m_updateNumber = 0;
+
+/**
+ * DriverStation contructor.
+ *
+ * This is only called once the first time GetInstance() is called
+ */
+DriverStation::DriverStation() {
+ state = gazebo::msgs::DriverStationPtr(new gazebo::msgs::DriverStation());
+ stateSub =
+ MainNode::Subscribe("~/ds/state", &DriverStation::stateCallback, this);
+ // TODO: for loop + boost bind
+ joysticks[0] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
+ joysticksSub[0] = MainNode::Subscribe(
+ "~/ds/joysticks/0", &DriverStation::joystickCallback0, this);
+ joysticks[1] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
+ joysticksSub[1] = MainNode::Subscribe(
+ "~/ds/joysticks/1", &DriverStation::joystickCallback1, this);
+ joysticks[2] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
+ joysticksSub[2] = MainNode::Subscribe(
+ "~/ds/joysticks/2", &DriverStation::joystickCallback2, this);
+ joysticks[3] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
+ joysticksSub[3] = MainNode::Subscribe(
+ "~/ds/joysticks/5", &DriverStation::joystickCallback3, this);
+ joysticks[4] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
+ joysticksSub[4] = MainNode::Subscribe(
+ "~/ds/joysticks/4", &DriverStation::joystickCallback4, this);
+ joysticks[5] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
+ joysticksSub[5] = MainNode::Subscribe(
+ "~/ds/joysticks/5", &DriverStation::joystickCallback5, this);
+}
+
+/**
+ * Return a pointer to the singleton DriverStation.
+ */
+DriverStation& DriverStation::GetInstance() {
+ static DriverStation instance;
+ return instance;
+}
+
+/**
+ * Read the battery voltage. Hardcoded to 12 volts for Simulation.
+ *
+ * @return The battery voltage.
+ */
+double DriverStation::GetBatteryVoltage() const {
+ return 12.0; // 12 volts all the time!
+}
+
+/**
+ * Get the value of the axis on a joystick.
+ * This depends on the mapping of the joystick connected to the specified port.
+ *
+ * @param stick The joystick to read.
+ * @param axis The analog axis value to read from the joystick.
+ * @return The value of the axis on the joystick.
+ */
+double DriverStation::GetStickAxis(int stick, int axis) {
+ if (axis < 0 || axis > (kJoystickAxes - 1)) {
+ wpi_setWPIError(BadJoystickAxis);
+ return 0.0;
+ }
+ if (stick < 0 || stick > 5) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0.0;
+ }
+
+ std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+ if (joysticks[stick] == nullptr || axis >= joysticks[stick]->axes().size()) {
+ return 0.0;
+ }
+ return joysticks[stick]->axes(axis);
+}
+
+/**
+ * The state of a specific button (1 - 12) on the joystick.
+ *
+ * This method only works in simulation, but is more efficient than
+ * GetStickButtons.
+ *
+ * @param stick The joystick to read.
+ * @param button The button number to check.
+ * @return If the button is pressed.
+ */
+bool DriverStation::GetStickButton(int stick, int button) {
+ if (stick < 0 || stick >= 6) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange,
+ "stick must be between 0 and 5");
+ return false;
+ }
+
+ std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+ if (joysticks[stick] == nullptr ||
+ button >= joysticks[stick]->buttons().size()) {
+ return false;
+ }
+ return joysticks[stick]->buttons(button - 1);
+}
+
+/**
+ * The state of the buttons on the joystick.
+ *
+ * 12 buttons (4 msb are unused) from the joystick.
+ *
+ * @param stick The joystick to read.
+ * @return The state of the buttons on the joystick.
+ */
+int16_t DriverStation::GetStickButtons(int stick) {
+ if (stick < 0 || stick >= 6) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange,
+ "stick must be between 0 and 5");
+ return false;
+ }
+ int16_t btns = 0, btnid;
+
+ std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+ gazebo::msgs::FRCJoystickPtr joy = joysticks[stick];
+ for (btnid = 0; btnid < joy->buttons().size() && btnid < 12; btnid++) {
+ if (joysticks[stick]->buttons(btnid)) {
+ btns |= (1 << btnid);
+ }
+ }
+ return btns;
+}
+
+// 5V divided by 10 bits
+#define kDSAnalogInScaling (5.0 / 1023.0)
+
+/**
+ * Get an analog voltage from the Driver Station.
+ *
+ * The analog values are returned as voltage values for the Driver Station
+ * analog inputs. These inputs are typically used for advanced operator
+ * interfaces consisting of potentiometers or resistor networks representing
+ * values on a rotary switch.
+ *
+ * @param channel The analog input channel on the driver station to read from.
+ * Valid range is 1 - 4.
+ * @return The analog voltage on the input.
+ */
+double DriverStation::GetAnalogIn(int channel) {
+ wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetAnalogIn");
+ return 0.0;
+}
+
+/**
+ * Get values from the digital inputs on the Driver Station.
+ *
+ * Return digital values from the Drivers Station. These values are typically
+ * used for buttons and switches on advanced operator interfaces.
+ *
+ * @param channel The digital input to get. Valid range is 1 - 8.
+ */
+bool DriverStation::GetDigitalIn(int channel) {
+ wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalIn");
+ return false;
+}
+
+/**
+ * Set a value for the digital outputs on the Driver Station.
+ *
+ * Control digital outputs on the Drivers Station. These values are typically
+ * used for giving feedback on a custom operator station such as LEDs.
+ *
+ * @param channel The digital output to set. Valid range is 1 - 8.
+ * @param value The state to set the digital output.
+ */
+void DriverStation::SetDigitalOut(int 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(int channel) {
+ wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalOut");
+ return false;
+}
+
+bool DriverStation::IsEnabled() const {
+ std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+ return state != nullptr ? state->enabled() : false;
+}
+
+bool DriverStation::IsDisabled() const { return !IsEnabled(); }
+
+bool DriverStation::IsAutonomous() const {
+ std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+ return state != nullptr
+ ? state->state() == gazebo::msgs::DriverStation_State_AUTO
+ : false;
+}
+
+bool DriverStation::IsOperatorControl() const {
+ return !(IsAutonomous() || IsTest());
+}
+
+bool DriverStation::IsTest() const {
+ std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+ return state != nullptr
+ ? state->state() == gazebo::msgs::DriverStation_State_TEST
+ : false;
+}
+
+/**
+ * Is the driver station attached to a Field Management System?
+ * @return True if the robot is competing on a field being controlled by a Field
+ * Management System
+ */
+bool DriverStation::IsFMSAttached() const {
+ return false; // No FMS in simulation
+}
+
+/**
+ * Return the alliance that the driver station says it is on.
+ * This could return kRed or kBlue.
+ * @return The Alliance enum
+ */
+DriverStation::Alliance DriverStation::GetAlliance() const {
+ // if (m_controlData->dsID_Alliance == 'R') return kRed;
+ // if (m_controlData->dsID_Alliance == 'B') return kBlue;
+ // wpi_assert(false);
+ return kInvalid; // TODO: Support alliance colors
+}
+
+/**
+ * Return the driver station location on the field.
+ * This could return 1, 2, or 3.
+ * @return The location of the driver station
+ */
+int DriverStation::GetLocation() const {
+ return -1; // TODO: Support locations
+}
+
+/**
+ * Wait until a new packet comes from the driver station.
+ *
+ * This blocks on a semaphore, so the waiting is efficient.
+ *
+ * This is a good way to delay processing until there is new driver station data
+ * to act on.
+ */
+void DriverStation::WaitForData() { WaitForData(0); }
+
+/**
+ * Wait until a new packet comes from the driver station, or wait for a timeout.
+ *
+ * If the timeout is less then or equal to 0, wait indefinitely.
+ *
+ * Timeout is in milliseconds
+ *
+ * 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.
+ *
+ * @param timeout Timeout time in seconds
+ *
+ * @return true if new data, otherwise false
+ */
+bool DriverStation::WaitForData(double timeout) {
+#if defined(_MSC_VER) && _MSC_VER < 1900
+ auto timeoutTime = std::chrono::steady_clock::now() +
+ std::chrono::duration<int64_t, std::nano>(
+ static_cast<int64_t>(timeout * 1e9));
+#else
+ auto timeoutTime =
+ std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+#endif
+
+ std::unique_lock<priority_mutex> lock(m_waitForDataMutex);
+ while (!m_updatedControlLoopData) {
+ if (timeout > 0) {
+ auto timedOut = m_waitForDataCond.wait_until(lock, timeoutTime);
+ if (timedOut == std::cv_status::timeout) {
+ return false;
+ }
+ } else {
+ m_waitForDataCond.wait(lock);
+ }
+ }
+ m_updatedControlLoopData = false;
+ return true;
+}
+
+/**
+ * Return the approximate match time.
+ * The FMS does not currently send the official match time to the robots
+ * This returns the time since the enable signal sent from the Driver Station
+ * At the beginning of autonomous, the time is reset to 0.0 seconds
+ * At the beginning of teleop, the time is reset to +15.0 seconds
+ * If the robot is disabled, this returns 0.0 seconds
+ * Warning: This is not an official time (so it cannot be used to argue with
+ * referees)
+ * @return Match time in seconds since the beginning of autonomous
+ */
+double DriverStation::GetMatchTime() const {
+ if (m_approxMatchTimeOffset < 0.0) return 0.0;
+ return Timer::GetFPGATimestamp() - m_approxMatchTimeOffset;
+}
+
+/**
+ * Report an error to the DriverStation messages window.
+ * The error is also printed to the program console.
+ */
+void DriverStation::ReportError(llvm::StringRef 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(llvm::StringRef error) {
+ std::cout << error << std::endl;
+}
+
+/**
+ * Report an error to the DriverStation messages window.
+ * The error is also printed to the program console.
+ */
+void DriverStation::ReportError(bool is_error, int code, llvm::StringRef error,
+ llvm::StringRef location,
+ llvm::StringRef stack) {
+ if (!location.empty())
+ std::cout << (is_error ? "Error" : "Warning") << " at " << location << ": ";
+ std::cout << error << std::endl;
+ if (!stack.empty()) std::cout << stack << std::endl;
+}
+
+/**
+ * Return the team number that the Driver Station is configured for.
+ * @return The team number
+ */
+uint16_t DriverStation::GetTeamNumber() const { return 348; }
+
+void DriverStation::stateCallback(
+ const gazebo::msgs::ConstDriverStationPtr& msg) {
+ {
+ std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+ *state = *msg;
+ }
+ {
+ std::lock_guard<priority_mutex> lock(m_waitForDataMutex);
+ m_updatedControlLoopData = true;
+ }
+ m_waitForDataCond.notify_all();
+}
+
+void DriverStation::joystickCallback(
+ const gazebo::msgs::ConstFRCJoystickPtr& msg, int i) {
+ std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+ *(joysticks[i]) = *msg;
+}
+
+void DriverStation::joystickCallback0(
+ const gazebo::msgs::ConstFRCJoystickPtr& msg) {
+ joystickCallback(msg, 0);
+}
+
+void DriverStation::joystickCallback1(
+ const gazebo::msgs::ConstFRCJoystickPtr& msg) {
+ joystickCallback(msg, 1);
+}
+
+void DriverStation::joystickCallback2(
+ const gazebo::msgs::ConstFRCJoystickPtr& msg) {
+ joystickCallback(msg, 2);
+}
+
+void DriverStation::joystickCallback3(
+ const gazebo::msgs::ConstFRCJoystickPtr& msg) {
+ joystickCallback(msg, 3);
+}
+
+void DriverStation::joystickCallback4(
+ const gazebo::msgs::ConstFRCJoystickPtr& msg) {
+ joystickCallback(msg, 4);
+}
+
+void DriverStation::joystickCallback5(
+ const gazebo::msgs::ConstFRCJoystickPtr& msg) {
+ joystickCallback(msg, 5);
+}
diff --git a/wpilibc/sim/src/Encoder.cpp b/wpilibc/sim/src/Encoder.cpp
new file mode 100644
index 0000000..cef4bef
--- /dev/null
+++ b/wpilibc/sim/src/Encoder.cpp
@@ -0,0 +1,388 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Encoder.h"
+
+#include <sstream>
+
+#include "LiveWindow/LiveWindow.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+/**
+ * Common initialization code for Encoders.
+ * This code allocates resources for Encoders and is common to all constructors.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param reverseDirection If true, counts down instead of up (this is all
+ * relative)
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X
+ * decoding. If 4X is selected, then an encoder FPGA
+ * object is used and the returned counts will be 4x
+ * the encoder spec'd value since all rising and
+ * falling edges are counted. If 1X or 2X are selected
+ * then a counter object will be used and the returned
+ * value will either exactly match the spec'd count or
+ * be double (2x) the spec'd count.
+ */
+void Encoder::InitEncoder(int channelA, int channelB, bool reverseDirection,
+ EncodingType encodingType) {
+ m_table = nullptr;
+ this->channelA = channelA;
+ this->channelB = channelB;
+ m_encodingType = encodingType;
+ m_encodingScale = encodingType == k4X ? 4 : encodingType == k2X ? 2 : 1;
+
+ int index = 0;
+ m_distancePerPulse = 1.0;
+
+ LiveWindow::GetInstance()->AddSensor("Encoder", channelA, this);
+
+ if (channelB < channelA) { // Swap ports
+ int channel = channelB;
+ channelB = channelA;
+ channelA = channel;
+ m_reverseDirection = !reverseDirection;
+ } else {
+ m_reverseDirection = reverseDirection;
+ }
+ std::stringstream ss;
+ ss << "dio/" << channelA << "/" << channelB;
+ impl = new SimEncoder(ss.str());
+ impl->Start();
+}
+
+/**
+ * Encoder constructor.
+ *
+ * Construct a Encoder given a and b channels.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param aChannel The a channel digital input channel.
+ * @param bChannel The b channel digital input channel.
+ * @param reverseDirection If true, counts down instead of up (this is all
+ * relative)
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X
+ * decoding. If 4X is selected, then an encoder FPGA
+ * object is used and the returned counts will be 4x
+ * the encoder spec'd value since all rising and
+ * falling edges are counted. If 1X or 2X are selected
+ * then a counter object will be used and the returned
+ * value will either exactly match the spec'd count or
+ * be double (2x) the spec'd count.
+ */
+Encoder::Encoder(int aChannel, int bChannel, bool reverseDirection,
+ EncodingType encodingType) {
+ InitEncoder(aChannel, bChannel, reverseDirection, encodingType);
+}
+
+/**
+ * Encoder constructor.
+ *
+ * Construct a Encoder given a and b channels as digital inputs. This is used in
+ * the case where the digital inputs are shared. The Encoder class will not
+ * allocate the digital inputs and assume that they already are counted.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param aSource The source that should be used for the a channel.
+ * @param bSource the source that should be used for the b channel.
+ * @param reverseDirection If true, counts down instead of up (this is all
+ * relative)
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X
+ * decoding. If 4X is selected, then an encoder FPGA
+ * object is used and the returned counts will be 4x
+ * the encoder spec'd value since all rising and
+ * falling edges are counted. If 1X or 2X are selected
+ * then a counter object will be used and the returned
+ * value will either exactly match the spec'd count or
+ * be double (2x) the spec'd count.
+ */
+/* TODO: [Not Supported] Encoder::Encoder(DigitalSource *aSource, DigitalSource
+*bSource, bool reverseDirection, EncodingType encodingType) :
+ m_encoder(nullptr),
+ m_counter(nullptr)
+{
+ m_aSource = aSource;
+ m_bSource = bSource;
+ m_allocatedASource = false;
+ m_allocatedBSource = false;
+ if (m_aSource == nullptr || m_bSource == nullptr)
+ wpi_setWPIError(NullParameter);
+ else
+ InitEncoder(reverseDirection, encodingType);
+ }*/
+
+/**
+ * Encoder constructor.
+ *
+ * Construct a Encoder given a and b channels as digital inputs. This is used in
+ * the case where the digital inputs are shared. The Encoder class will not
+ * allocate the digital inputs and assume that they already are counted.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param aSource The source that should be used for the a channel.
+ * @param bSource the source that should be used for the b channel.
+ * @param reverseDirection If true, counts down instead of up (this is all
+ * relative)
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X
+ * decoding. If 4X is selected, then an encoder FPGA
+ * object is used and the returned counts will be 4x
+ * the encoder spec'd value since all rising and
+ * falling edges are counted. If 1X or 2X are selected
+ * then a counter object will be used and the returned
+ * value will either exactly match the spec'd count or
+ * be double (2x) the spec'd count.
+ */
+/*// TODO: [Not Supported] Encoder::Encoder(DigitalSource &aSource,
+DigitalSource &bSource, bool reverseDirection, EncodingType encodingType) :
+ m_encoder(nullptr),
+ m_counter(nullptr)
+{
+ m_aSource = &aSource;
+ m_bSource = &bSource;
+ m_allocatedASource = false;
+ m_allocatedBSource = false;
+ InitEncoder(reverseDirection, encodingType);
+ }*/
+
+/**
+ * Reset the Encoder distance to zero.
+ *
+ * Resets the current count to zero on the encoder.
+ */
+void Encoder::Reset() { impl->Reset(); }
+
+/**
+ * Determine if the encoder is stopped.
+ *
+ * Using the MaxPeriod value, a boolean is returned that is true if the encoder
+ * is considered stopped and false if it is still moving. A stopped encoder is
+ * one where the most recent pulse width exceeds the MaxPeriod.
+ *
+ * @return True if the encoder is considered stopped.
+ */
+bool Encoder::GetStopped() const {
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * The last direction the encoder value changed.
+ *
+ * @return The last direction the encoder value changed.
+ */
+bool Encoder::GetDirection() const {
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * The scale needed to convert a raw counter value into a number of encoder
+ * pulses.
+ */
+double Encoder::DecodingScaleFactor() const {
+ switch (m_encodingType) {
+ case k1X:
+ return 1.0;
+ case k2X:
+ return 0.5;
+ case k4X:
+ return 0.25;
+ default:
+ return 0.0;
+ }
+}
+
+/**
+ * The encoding scale factor 1x, 2x, or 4x, per the requested encodingType.
+ *
+ * Used to divide raw edge counts down to spec'd counts.
+ */
+int 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
+ */
+int 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.
+ */
+int Encoder::Get() const {
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Returns the period of the most recent pulse.
+ *
+ * Returns the period of the most recent Encoder pulse in seconds.
+ * This method compenstates for the decoding type.
+ *
+ * @deprecated Use GetRate() in favor of this method. This returns unscaled
+ * periods and GetRate() scales using value from
+ * SetDistancePerPulse().
+ *
+ * @return Period in seconds of the most recent pulse.
+ */
+double Encoder::GetPeriod() const {
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Sets the maximum period for stopped detection.
+ *
+ * Sets the value that represents the maximum period of the Encoder before it
+ * will assume that the attached device is stopped. This timeout allows users
+ * to determine if the wheels or other shaft has stopped rotating.
+ * This method compensates for the decoding type.
+ *
+ * @deprecated Use SetMinRate() in favor of this method. This takes unscaled
+ * periods and SetMinRate() scales using value from
+ * SetDistancePerPulse().
+ *
+ * @param maxPeriod The maximum time between rising and falling edges before the
+ * FPGA will report the device stopped. This is expressed in
+ * seconds.
+ */
+void Encoder::SetMaxPeriod(double maxPeriod) {
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Get the distance the robot has driven since the last reset.
+ *
+ * @return The distance driven since the last reset as scaled by the value from
+ * SetDistancePerPulse().
+ */
+double Encoder::GetDistance() const {
+ return m_distancePerPulse * impl->GetPosition();
+}
+
+/**
+ * Get the current rate of the encoder.
+ *
+ * Units are distance per second as scaled by the value from
+ * SetDistancePerPulse().
+ *
+ * @return The current rate of the encoder.
+ */
+double Encoder::GetRate() const {
+ return m_distancePerPulse * impl->GetVelocity();
+}
+
+/**
+ * Set the minimum rate of the device before the hardware reports it stopped.
+ *
+ * @param minRate The minimum rate. The units are in distance per second as
+ * scaled by the value from SetDistancePerPulse().
+ */
+void Encoder::SetMinRate(double minRate) {
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Set the distance per pulse for this encoder.
+ *
+ * This sets the multiplier used to determine the distance driven based on the
+ * count value from the encoder. Do not include the decoding type in this scale.
+ * The library already compensates for the decoding type. Set this value based
+ * on the encoder's rated Pulses per Revolution and factor in gearing reductions
+ * following the encoder shaft. This distance can be in any units you like,
+ * linear or angular.
+ *
+ * @param distancePerPulse The scale factor that will be used to convert pulses
+ * to useful units.
+ */
+void Encoder::SetDistancePerPulse(double distancePerPulse) {
+ if (m_reverseDirection) {
+ m_distancePerPulse = -distancePerPulse;
+ } else {
+ m_distancePerPulse = distancePerPulse;
+ }
+}
+
+/**
+ * Set the direction sensing for this encoder.
+ *
+ * This sets the direction sensing on the encoder so that it could count in the
+ * correct software direction regardless of the mounting.
+ *
+ * @param reverseDirection true if the encoder direction should be reversed
+ */
+void Encoder::SetReverseDirection(bool reverseDirection) {
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Set which parameter of the encoder you are using as a process control
+ * variable.
+ *
+ * @param pidSource An enum to select the parameter.
+ */
+void Encoder::SetPIDSourceType(PIDSourceType pidSource) {
+ m_pidSource = pidSource;
+}
+
+/**
+ * Implement the PIDSource interface.
+ *
+ * @return The current value of the selected source parameter.
+ */
+double Encoder::PIDGet() {
+ switch (m_pidSource) {
+ case PIDSourceType::kDisplacement:
+ return GetDistance();
+ case PIDSourceType::kRate:
+ return GetRate();
+ default:
+ return 0.0;
+ }
+}
+
+void Encoder::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Speed", GetRate());
+ m_table->PutNumber("Distance", GetDistance());
+ m_table->PutNumber("Distance per Tick", m_reverseDirection
+ ? -m_distancePerPulse
+ : m_distancePerPulse);
+ }
+}
+
+void Encoder::StartLiveWindowMode() {}
+
+void Encoder::StopLiveWindowMode() {}
+
+std::string Encoder::GetSmartDashboardType() const {
+ if (m_encodingType == k4X)
+ return "Quadrature Encoder";
+ else
+ return "Encoder";
+}
+
+void Encoder::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> Encoder::GetTable() const { return m_table; }
diff --git a/wpilibc/sim/src/GenericHID.cpp b/wpilibc/sim/src/GenericHID.cpp
new file mode 100644
index 0000000..c76e03d
--- /dev/null
+++ b/wpilibc/sim/src/GenericHID.cpp
@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "GenericHID.h"
+
+#include "DriverStation.h"
+
+using namespace frc;
+
+GenericHID::GenericHID(int port) : m_ds(DriverStation::GetInstance()) {
+ m_port = port;
+}
+
+/**
+ * Get the value of the axis.
+ *
+ * @param axis The axis to read, starting at 0.
+ * @return The value of the axis.
+ */
+double GenericHID::GetRawAxis(int axis) const {
+ return m_ds.GetStickAxis(m_port, axis);
+}
+
+/**
+ * Get the button value (starting at button 1)
+ *
+ * The buttons are returned in a single 16 bit value with one bit representing
+ * the state of each button. The appropriate button is returned as a boolean
+ * value.
+ *
+ * @param button The button number to be read (starting at 1)
+ * @return The state of the button.
+ **/
+bool GenericHID::GetRawButton(int button) const {
+ return m_ds.GetStickButton(m_port, button);
+}
+
+/**
+ * Get the angle in degrees of a POV on the HID.
+ *
+ * The POV angles start at 0 in the up direction, and increase clockwise
+ * (e.g. right is 90, upper-left is 315).
+ *
+ * @param pov The index of the POV to read (starting at 0)
+ * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+ */
+int GenericHID::GetPOV(int pov) const { return 0; }
+
+/**
+ * Get the number of POVs for the HID.
+ *
+ * @return the number of POVs for the current HID
+ */
+int GenericHID::GetPOVCount() const { return 0; }
+
+/**
+ * Get the port number of the HID.
+ *
+ * @return The port number of the HID.
+ */
+int GenericHID::GetPort() const { return m_port; }
+
+/**
+ * Get the type of the HID.
+ *
+ * @return the type of the HID.
+ */
+GenericHID::HIDType GenericHID::GetType() const { return HIDType::kUnknown; }
+
+/**
+ * Get the name of the HID.
+ *
+ * @return the name of the HID.
+ */
+std::string GenericHID::GetName() const { return ""; }
+
+/**
+ * Set a single HID output value for the HID.
+ *
+ * @param outputNumber The index of the output to set (1-32)
+ * @param value The value to set the output to
+ */
+
+void GenericHID::SetOutput(int outputNumber, bool value) {
+ m_outputs =
+ (m_outputs & ~(1 << (outputNumber - 1))) | (value << (outputNumber - 1));
+}
+
+/**
+ * Set all output values for the HID.
+ *
+ * @param value The 32 bit output value (1 bit for each output)
+ */
+void GenericHID::SetOutputs(int value) { m_outputs = value; }
+
+/**
+ * Set the rumble output for the HID.
+ *
+ * The DS currently supports 2 rumble values, left rumble and right rumble.
+ *
+ * @param type Which rumble value to set
+ * @param value The normalized value (0 to 1) to set the rumble to
+ */
+void GenericHID::SetRumble(RumbleType type, double value) {
+ if (value < 0)
+ value = 0;
+ else if (value > 1)
+ value = 1;
+ if (type == kLeftRumble) {
+ m_leftRumble = value * 65535;
+ } else {
+ m_rightRumble = value * 65535;
+ }
+}
diff --git a/wpilibc/sim/src/IterativeRobot.cpp b/wpilibc/sim/src/IterativeRobot.cpp
new file mode 100644
index 0000000..9a37c05
--- /dev/null
+++ b/wpilibc/sim/src/IterativeRobot.cpp
@@ -0,0 +1,223 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "IterativeRobot.h"
+
+#include "DriverStation.h"
+#include "LiveWindow/LiveWindow.h"
+#include "networktables/NetworkTable.h"
+
+// not sure what this is used for yet.
+#ifdef _UNIX
+#include <unistd.h>
+#endif
+
+using namespace frc;
+
+/**
+ * Provide an alternate "main loop" via StartCompetition().
+ *
+ * This specific StartCompetition() implements "main loop" behavior like that of
+ * the FRC control system in 2008 and earlier, with a primary (slow) loop that
+ * is called periodically, and a "fast loop" (a.k.a. "spin loop") that is
+ * called as fast as possible with no delay between calls.
+ */
+void IterativeRobot::StartCompetition() {
+ LiveWindow* lw = LiveWindow::GetInstance();
+ // first and one-time initialization
+ 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;
+ }
+ // 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;
+ }
+ // 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;
+ }
+ // 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);
+ }
+ // TODO: HALNetworkCommunicationObserveUserProgramTeleop();
+ TeleopPeriodic();
+ }
+ // wait for driver station data so the loop doesn't hog the CPU
+ m_ds.WaitForData();
+ }
+}
+
+/**
+ * Robot-wide initialization code should go here.
+ *
+ * Users should override this method for default Robot-wide initialization which
+ * will be called when the robot is first powered on. It will be called
+ * exactly 1 time.
+ */
+void IterativeRobot::RobotInit() {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for disabled mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time the robot enters disabled mode.
+ */
+void IterativeRobot::DisabledInit() {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for autonomous mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time the robot enters autonomous mode.
+ */
+void IterativeRobot::AutonomousInit() {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for teleop mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time the robot enters teleop mode.
+ */
+void IterativeRobot::TeleopInit() {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for test mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time the robot enters test mode.
+ */
+void IterativeRobot::TestInit() {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Periodic code for all modes should go here.
+ *
+ * Users should override this method for code which will be called periodically
+ * at a regular rate while the robot is in any mode.
+ */
+void IterativeRobot::RobotPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+}
+
+/**
+ * Periodic code for disabled mode should go here.
+ *
+ * Users should override this method for code which will be called periodically
+ * at a regular rate while the robot is in disabled mode.
+ */
+void IterativeRobot::DisabledPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+}
+
+/**
+ * Periodic code for autonomous mode should go here.
+ *
+ * Users should override this method for code which will be called periodically
+ * at a regular rate while the robot is in autonomous mode.
+ */
+void IterativeRobot::AutonomousPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+}
+
+/**
+ * Periodic code for teleop mode should go here.
+ *
+ * Users should override this method for code which will be called periodically
+ * at a regular rate while the robot is in teleop mode.
+ */
+void IterativeRobot::TeleopPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+}
+
+/**
+ * Periodic code for test mode should go here.
+ *
+ * Users should override this method for code which will be called periodically
+ * at a regular rate while the robot is in test mode.
+ */
+void IterativeRobot::TestPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+}
diff --git a/wpilibc/sim/src/Jaguar.cpp b/wpilibc/sim/src/Jaguar.cpp
new file mode 100644
index 0000000..84c3470
--- /dev/null
+++ b/wpilibc/sim/src/Jaguar.cpp
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Jaguar.h"
+
+#include "LiveWindow/LiveWindow.h"
+
+using namespace frc;
+
+/**
+ * @param channel The PWM channel that the Jaguar is attached to.
+ */
+Jaguar::Jaguar(int channel) : SafePWM(channel) {
+ /*
+ * Input profile defined by Luminary Micro.
+ *
+ * Full reverse ranges from 0.671325ms to 0.6972211ms
+ * Proportional reverse ranges from 0.6972211ms to 1.4482078ms
+ * Neutral ranges from 1.4482078ms to 1.5517922ms
+ * Proportional forward ranges from 1.5517922ms to 2.3027789ms
+ * Full forward ranges from 2.3027789ms to 2.328675ms
+ */
+ SetBounds(2.31, 1.55, 1.507, 1.454, .697);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetRaw(m_centerPwm);
+
+ LiveWindow::GetInstance()->AddActuator("Jaguar", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ */
+void Jaguar::Set(double speed) { 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.
+ */
+double Jaguar::Get() const { return GetSpeed(); }
+
+/**
+ * Common interface for disabling a motor.
+ */
+void Jaguar::Disable() { SetRaw(kPwmDisabled); }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void Jaguar::PIDWrite(double output) { Set(output); }
diff --git a/wpilibc/sim/src/Joystick.cpp b/wpilibc/sim/src/Joystick.cpp
new file mode 100644
index 0000000..fc697b1
--- /dev/null
+++ b/wpilibc/sim/src/Joystick.cpp
@@ -0,0 +1,274 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Joystick.h"
+
+#include <cmath>
+
+#include "DriverStation.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+const int Joystick::kDefaultXAxis;
+const int Joystick::kDefaultYAxis;
+const int Joystick::kDefaultZAxis;
+const int Joystick::kDefaultTwistAxis;
+const int Joystick::kDefaultThrottleAxis;
+const int Joystick::kDefaultTriggerButton;
+const int Joystick::kDefaultTopButton;
+static Joystick* joysticks[DriverStation::kJoystickPorts];
+static bool joySticksInitialized = false;
+
+/**
+ * Construct an instance of a joystick.
+ *
+ * The joystick index is the USB port on the Driver Station.
+ *
+ * @param port The port on the Driver Station that the joystick is plugged into
+ * (0-5).
+ */
+Joystick::Joystick(int port) : Joystick(port, kNumAxisTypes, kNumButtonTypes) {
+ m_axes[kXAxis] = kDefaultXAxis;
+ m_axes[kYAxis] = kDefaultYAxis;
+ m_axes[kZAxis] = kDefaultZAxis;
+ m_axes[kTwistAxis] = kDefaultTwistAxis;
+ m_axes[kThrottleAxis] = kDefaultThrottleAxis;
+
+ m_buttons[kTriggerButton] = kDefaultTriggerButton;
+ m_buttons[kTopButton] = kDefaultTopButton;
+}
+
+/**
+ * Version of the constructor to be called by sub-classes.
+ *
+ * This constructor allows the subclass to configure the number of constants
+ * for axes and buttons.
+ *
+ * @param port The port on the Driver Station that the joystick is
+ * plugged into.
+ * @param numAxisTypes The number of axis types in the enum.
+ * @param numButtonTypes The number of button types in the enum.
+ */
+Joystick::Joystick(int port, int numAxisTypes, int numButtonTypes)
+ : JoystickBase(port),
+ m_ds(DriverStation::GetInstance()),
+ m_axes(numAxisTypes),
+ m_buttons(numButtonTypes) {
+ if (!joySticksInitialized) {
+ for (auto& joystick : joysticks) joystick = nullptr;
+ joySticksInitialized = true;
+ }
+ if (GetPort() >= DriverStation::kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ } else {
+ joysticks[GetPort()] = this;
+ }
+}
+
+Joystick* Joystick::GetStickForPort(int port) {
+ Joystick* stick = joysticks[port];
+ if (stick == nullptr) {
+ stick = new Joystick(port);
+ joysticks[port] = stick;
+ }
+ return stick;
+}
+
+/**
+ * Get the X value of the joystick.
+ *
+ * This depends on the mapping of the joystick connected to the current port.
+ *
+ * @param hand This parameter is ignored for the Joystick class and is only
+ * here to complete the GenericHID interface.
+ */
+double Joystick::GetX(JoystickHand hand) const {
+ return GetRawAxis(m_axes[kXAxis]);
+}
+
+/**
+ * Get the Y value of the joystick.
+ *
+ * This depends on the mapping of the joystick connected to the current port.
+ *
+ * @param hand This parameter is ignored for the Joystick class and is only
+ * here to complete the GenericHID interface.
+ */
+double 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.
+ */
+double Joystick::GetZ(JoystickHand hand) 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.
+ */
+double 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.
+ */
+double Joystick::GetThrottle() const {
+ return GetRawAxis(m_axes[kThrottleAxis]);
+}
+
+/**
+ * For the current joystick, return the axis determined by the argument.
+ *
+ * This is for cases where the joystick axis is returned programatically,
+ * otherwise one of the previous functions would be preferable (for example
+ * GetX()).
+ *
+ * @param axis The axis to read.
+ * @return The value of the axis.
+ */
+double Joystick::GetAxis(AxisType axis) const {
+ switch (axis) {
+ case kXAxis:
+ return this->GetX();
+ case kYAxis:
+ return this->GetY();
+ case kZAxis:
+ return this->GetZ();
+ case kTwistAxis:
+ return this->GetTwist();
+ case kThrottleAxis:
+ return this->GetThrottle();
+ default:
+ wpi_setWPIError(BadJoystickAxis);
+ return 0.0;
+ }
+}
+
+/**
+ * Read the state of the trigger on the joystick.
+ *
+ * Look up which button has been assigned to the trigger and read its state.
+ *
+ * @param hand This parameter is ignored for the Joystick class and is only
+ * here to complete the GenericHID interface.
+ * @return The state of the trigger.
+ */
+bool Joystick::GetTrigger(JoystickHand hand) const {
+ return GetRawButton(m_buttons[kTriggerButton]);
+}
+
+/**
+ * Read the state of the top button on the joystick.
+ *
+ * Look up which button has been assigned to the top and read its state.
+ *
+ * @param hand This parameter is ignored for the Joystick class and is only
+ * here to complete the GenericHID interface.
+ * @return The state of the top button.
+ */
+bool Joystick::GetTop(JoystickHand hand) const {
+ return GetRawButton(m_buttons[kTopButton]);
+}
+
+/**
+ * Get buttons based on an enumerated type.
+ *
+ * The button type will be looked up in the list of buttons and then read.
+ *
+ * @param button The type of button to read.
+ * @return The state of the button.
+ */
+bool Joystick::GetButton(ButtonType button) const {
+ switch (button) {
+ case kTriggerButton:
+ return GetTrigger();
+ case kTopButton:
+ return GetTop();
+ default:
+ return false;
+ }
+}
+
+/**
+ * Get the number of axis for a joystick
+ *
+ * @return the number of axis for the current joystick
+ */
+int Joystick::GetAxisCount() const { return 0; }
+
+/**
+ * Get the axis type of a joystick axis.
+ *
+ * @return the axis type of a joystick axis.
+ */
+int Joystick::GetAxisType(int axis) const { return 0; }
+
+/**
+ * Get the number of buttons for a joystick.
+ *
+ * @return the number of buttons on the current joystick
+ */
+int Joystick::GetButtonCount() const { return 0; }
+
+/**
+ * Get the channel currently associated with the specified axis.
+ *
+ * @param axis The axis to look up the channel for.
+ * @return The channel fr the axis.
+ */
+int Joystick::GetAxisChannel(AxisType axis) const { return m_axes[axis]; }
+
+/**
+ * Set the channel associated with a specified axis.
+ *
+ * @param axis The axis to set the channel for.
+ * @param channel The channel to set the axis to.
+ */
+void Joystick::SetAxisChannel(AxisType axis, int channel) {
+ m_axes[axis] = channel;
+}
+
+/**
+ * Get the magnitude of the direction vector formed by the joystick's
+ * current position relative to its origin.
+ *
+ * @return The magnitude of the direction vector
+ */
+double Joystick::GetMagnitude() const {
+ return std::sqrt(std::pow(GetX(), 2) + std::pow(GetY(), 2));
+}
+
+/**
+ * Get the direction of the vector formed by the joystick and its origin
+ * in radians.
+ *
+ * @return The direction of the vector in radians
+ */
+double Joystick::GetDirectionRadians() const {
+ return std::atan2(GetX(), -GetY());
+}
+
+/**
+ * Get the direction of the vector formed by the joystick and its origin
+ * in degrees.
+ *
+ * uses std::acos(-1) to represent Pi due to absence of readily accessible Pi
+ * constant in C++
+ *
+ * @return The direction of the vector in degrees
+ */
+double Joystick::GetDirectionDegrees() const {
+ return (180 / std::acos(-1)) * GetDirectionRadians();
+}
diff --git a/wpilibc/sim/src/MotorSafetyHelper.cpp b/wpilibc/sim/src/MotorSafetyHelper.cpp
new file mode 100644
index 0000000..df4ecde
--- /dev/null
+++ b/wpilibc/sim/src/MotorSafetyHelper.cpp
@@ -0,0 +1,146 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "MotorSafetyHelper.h"
+
+#include <sstream>
+
+#include "DriverStation.h"
+#include "MotorSafety.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+std::set<MotorSafetyHelper*> MotorSafetyHelper::m_helperList;
+priority_recursive_mutex MotorSafetyHelper::m_listMutex;
+
+/**
+ * The constructor for a MotorSafetyHelper object.
+ *
+ * The helper object is constructed for every object that wants to implement the
+ * Motor Safety protocol. The helper object has the code to actually do the
+ * timing and call the motors Stop() method when the timeout expires. The motor
+ * object is expected to call the Feed() method whenever the motors value is
+ * updated.
+ *
+ * @param safeObject a pointer to the motor object implementing MotorSafety.
+ * This is used to call the Stop() method on the motor.
+ */
+MotorSafetyHelper::MotorSafetyHelper(MotorSafety* safeObject)
+ : m_safeObject(safeObject) {
+ m_enabled = false;
+ m_expiration = DEFAULT_SAFETY_EXPIRATION;
+ m_stopTime = Timer::GetFPGATimestamp();
+
+ std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
+ m_helperList.insert(this);
+}
+
+MotorSafetyHelper::~MotorSafetyHelper() {
+ std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
+ m_helperList.erase(this);
+}
+
+/**
+ * Feed the motor safety object.
+ *
+ * Resets the timer on this object that is used to do the timeouts.
+ */
+void MotorSafetyHelper::Feed() {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
+}
+
+/**
+ * Set the expiration time for the corresponding motor safety object.
+ *
+ * @param expirationTime The timeout value in seconds.
+ */
+void MotorSafetyHelper::SetExpiration(double expirationTime) {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ m_expiration = expirationTime;
+}
+
+/**
+ * Retrieve the timeout value for the corresponding motor safety object.
+ *
+ * @return the timeout value in seconds.
+ */
+double MotorSafetyHelper::GetExpiration() const {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ return m_expiration;
+}
+
+/**
+ * Determine if the motor is still operating or has timed out.
+ *
+ * @return a true value if the motor is still operating normally and hasn't
+ * timed out.
+ */
+bool MotorSafetyHelper::IsAlive() const {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
+}
+
+/**
+ * Check if this motor has exceeded its timeout.
+ *
+ * This method is called periodically to determine if this motor has exceeded
+ * its timeout value. If it has, the stop method is called, and the motor is
+ * shut down until its value is updated again.
+ */
+void MotorSafetyHelper::Check() {
+ DriverStation& ds = DriverStation::GetInstance();
+ if (!m_enabled || ds.IsDisabled() || ds.IsTest()) return;
+
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ if (m_stopTime < Timer::GetFPGATimestamp()) {
+ std::ostringstream desc;
+ m_safeObject->GetDescription(desc);
+ desc << "... Output not updated often enough.";
+ wpi_setWPIErrorWithContext(Timeout, desc.str().c_str());
+ m_safeObject->StopMotor();
+ }
+}
+
+/**
+ * Enable/disable motor safety for this device.
+ *
+ * Turn on and off the motor safety option for this PWM object.
+ *
+ * @param enabled True if motor safety is enforced for this object
+ */
+void MotorSafetyHelper::SetSafetyEnabled(bool enabled) {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ m_enabled = enabled;
+}
+
+/**
+ * Return the state of the motor safety enabled flag.
+ *
+ * Return if the motor safety is currently enabled for this devicce.
+ *
+ * @return True if motor safety is enforced for this device
+ */
+bool MotorSafetyHelper::IsSafetyEnabled() const {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ return m_enabled;
+}
+
+/**
+ * Check the motors to see if any have timed out.
+ *
+ * This static method is called periodically to poll all the motors and stop
+ * any that have timed out.
+ */
+void MotorSafetyHelper::CheckMotors() {
+ std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
+ for (auto elem : m_helperList) {
+ elem->Check();
+ }
+}
diff --git a/wpilibc/sim/src/Notifier.cpp b/wpilibc/sim/src/Notifier.cpp
new file mode 100644
index 0000000..7950dae
--- /dev/null
+++ b/wpilibc/sim/src/Notifier.cpp
@@ -0,0 +1,267 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Notifier.h"
+
+#include "Timer.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+std::list<Notifier*> Notifier::timerQueue;
+priority_recursive_mutex Notifier::queueMutex;
+std::atomic<int> Notifier::refcount{0};
+std::thread Notifier::m_task;
+std::atomic<bool> Notifier::m_stopped(false);
+
+/**
+ * Create a Notifier for timer event notification.
+ *
+ * @param handler The handler is called at the notification time which is set
+ * using StartSingle or StartPeriodic.
+ */
+Notifier::Notifier(TimerEventHandler handler) {
+ if (handler == nullptr)
+ wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
+ m_handler = handler;
+ m_periodic = false;
+ m_expirationTime = 0;
+ m_period = 0;
+ m_queued = false;
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ // do the first time intialization of static variables
+ if (refcount.fetch_add(1) == 0) {
+ m_task = std::thread(Run);
+ }
+ }
+}
+
+/**
+ * Free the resources for a timer event.
+ *
+ * All resources will be freed and the timer event will be removed from the
+ * queue if necessary.
+ */
+Notifier::~Notifier() {
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ DeleteFromQueue();
+
+ // Delete the static variables when the last one is going away
+ if (refcount.fetch_sub(1) == 1) {
+ m_stopped = true;
+ m_task.join();
+ }
+ }
+
+ // Acquire the semaphore; this makes certain that the handler is
+ // not being executed by the interrupt manager.
+ std::lock_guard<priority_mutex> lock(m_handlerMutex);
+}
+
+/**
+ * Update the alarm hardware to reflect the current first element in the queue.
+ *
+ * Compute the time the next alarm should occur based on the current time and
+ * the period for the first element in the timer queue.
+ *
+ * WARNING: this method does not do synchronization! It must be called from
+ * somewhere that is taking care of synchronizing access to the queue.
+ */
+void Notifier::UpdateAlarm() {}
+
+/**
+ * ProcessQueue is called whenever there is a timer interrupt.
+ *
+ * We need to wake up and process the current top item in the timer queue as
+ * long as its scheduled time is after the current time. Then the item is
+ * removed or rescheduled (repetitive events) in the queue.
+ */
+void Notifier::ProcessQueue(int mask, void* params) {
+ Notifier* current;
+
+ // keep processing events until no more
+ while (true) {
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ double currentTime = GetClock();
+
+ if (timerQueue.empty()) {
+ break;
+ }
+ current = timerQueue.front();
+ if (current->m_expirationTime > currentTime) {
+ break; // no more timer events to process
+ }
+ // remove next entry before processing it
+ timerQueue.pop_front();
+
+ current->m_queued = false;
+ if (current->m_periodic) {
+ // if periodic, requeue the event
+ // compute when to put into queue
+ current->InsertInQueue(true);
+ } else {
+ // not periodic; removed from queue
+ current->m_queued = false;
+ }
+ // Take handler mutex while holding queue semaphore to make sure
+ // the handler will execute to completion in case we are being deleted.
+ current->m_handlerMutex.lock();
+ }
+
+ current->m_handler(); // call the event handler
+ current->m_handlerMutex.unlock();
+ }
+ // reschedule the first item in the queue
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ UpdateAlarm();
+}
+
+/**
+ * Insert this Notifier into the timer queue in right place.
+ *
+ * WARNING: this method does not do synchronization! It must be called from
+ * somewhere that is taking care of synchronizing access to the queue.
+ *
+ * @param reschedule If false, the scheduled alarm is based on the curent time
+ * and UpdateAlarm method is called which will enable the
+ * alarm if necessary. If true, update the time by adding the
+ * period (no drift) when rescheduled periodic from
+ * ProcessQueue.
+ *
+ * This ensures that the public methods only update the queue after finishing
+ * inserting.
+ */
+void Notifier::InsertInQueue(bool reschedule) {
+ if (reschedule) {
+ m_expirationTime += m_period;
+ } else {
+ m_expirationTime = GetClock() + m_period;
+ }
+
+ // Attempt to insert new entry into queue
+ for (auto i = timerQueue.begin(); i != timerQueue.end(); i++) {
+ if ((*i)->m_expirationTime > m_expirationTime) {
+ timerQueue.insert(i, this);
+ m_queued = true;
+ }
+ }
+
+ /* If the new entry wasn't queued, either the queue was empty or the first
+ * element was greater than the new entry.
+ */
+ if (!m_queued) {
+ timerQueue.push_front(this);
+
+ if (!reschedule) {
+ /* Since the first element changed, update alarm, unless we already
+ * plan to
+ */
+ UpdateAlarm();
+ }
+
+ m_queued = true;
+ }
+}
+
+/**
+ * Delete this Notifier from the timer queue.
+ *
+ * WARNING: this method does not do synchronization! It must be called from
+ * somewhere that is taking care of synchronizing access to the queue.
+ *
+ * Remove this Notifier from the timer queue and adjust the next interrupt time
+ * to reflect the current top of the queue.
+ */
+void Notifier::DeleteFromQueue() {
+ if (m_queued) {
+ m_queued = false;
+ wpi_assert(!timerQueue.empty());
+ if (timerQueue.front() == this) {
+ // remove the first item in the list - update the alarm
+ timerQueue.pop_front();
+ UpdateAlarm();
+ } else {
+ timerQueue.remove(this);
+ }
+ }
+}
+
+/**
+ * Register for single event notification.
+ *
+ * A timer event is queued for a single event after the specified delay.
+ *
+ * @param delay Seconds to wait before the handler is called.
+ */
+void Notifier::StartSingle(double delay) {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ m_periodic = false;
+ m_period = delay;
+ DeleteFromQueue();
+ InsertInQueue(false);
+}
+
+/**
+ * Register for periodic event notification.
+ *
+ * A timer event is queued for periodic event notification. Each time the
+ * interrupt occurs, the event will be immediately requeued for the same time
+ * interval.
+ *
+ * @param period Period in seconds to call the handler starting one period after
+ * the call to this method.
+ */
+void Notifier::StartPeriodic(double period) {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ m_periodic = true;
+ m_period = period;
+ DeleteFromQueue();
+ InsertInQueue(false);
+}
+
+/**
+ * Stop timer events from occuring.
+ *
+ * Stop any repeating timer events from occuring. This will also remove any
+ * single notification events from the queue. If a timer-based call to the
+ * registered handler is in progress, this function will block until the
+ * handler call is complete.
+ */
+void Notifier::Stop() {
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ DeleteFromQueue();
+ }
+ // Wait for a currently executing handler to complete before returning from
+ // Stop()
+ std::lock_guard<priority_mutex> sync(m_handlerMutex);
+}
+
+void Notifier::Run() {
+ while (!m_stopped) {
+ Notifier::ProcessQueue(0, nullptr);
+ bool isEmpty;
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ isEmpty = timerQueue.empty();
+ }
+ if (!isEmpty) {
+ double expirationTime;
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ expirationTime = timerQueue.front()->m_expirationTime;
+ }
+ Wait(expirationTime - GetClock());
+ } else {
+ Wait(0.05);
+ }
+ }
+}
diff --git a/wpilibc/sim/src/PIDController.cpp b/wpilibc/sim/src/PIDController.cpp
new file mode 100644
index 0000000..0231eb8
--- /dev/null
+++ b/wpilibc/sim/src/PIDController.cpp
@@ -0,0 +1,643 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "PIDController.h"
+
+#include <cmath>
+
+#include "Notifier.h"
+#include "PIDOutput.h"
+#include "PIDSource.h"
+
+using namespace frc;
+
+static const std::string kP = "p";
+static const std::string kI = "i";
+static const std::string kD = "d";
+static const std::string kF = "f";
+static const std::string kSetpoint = "setpoint";
+static const std::string kEnabled = "enabled";
+
+/**
+ * Allocate a PID object with the given constants for P, I, D.
+ *
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output value
+ * @param period the loop time for doing calculations. This particularly effects
+ * calculations of the integral and differental terms. The
+ * default is 50ms.
+ */
+PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource* source,
+ PIDOutput* output, double period)
+ : PIDController(Kp, Ki, Kd, 0.0, source, output, period) {}
+
+/**
+ * Allocate a PID object with the given constants for P, I, D.
+ *
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output value
+ * @param period the loop time for doing calculations. This particularly effects
+ * calculations of the integral and differental terms. The
+ * default is 50ms.
+ */
+PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
+ PIDSource* source, PIDOutput* output,
+ double period) {
+ m_table = nullptr;
+
+ m_P = Kp;
+ m_I = Ki;
+ m_D = Kd;
+ m_F = Kf;
+
+ m_maximumOutput = 1.0;
+ m_minimumOutput = -1.0;
+
+ m_maximumInput = 0;
+ m_minimumInput = 0;
+
+ m_continuous = false;
+ m_enabled = false;
+ m_setpoint = 0;
+
+ m_prevError = 0;
+ m_totalError = 0;
+ m_tolerance = .05;
+
+ m_result = 0;
+
+ m_pidInput = source;
+ m_pidOutput = output;
+ m_period = period;
+
+ m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
+ m_controlLoop->StartPeriodic(m_period);
+
+ static int instances = 0;
+ instances++;
+
+ m_toleranceType = kNoTolerance;
+}
+
+PIDController::~PIDController() {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Read the input, calculate the output accordingly, and write to the output.
+ *
+ * This should only be called by the Notifier.
+ */
+void PIDController::Calculate() {
+ bool enabled;
+ PIDSource* pidInput;
+
+ {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ if (m_pidInput == 0) return;
+ if (m_pidOutput == 0) return;
+ enabled = m_enabled;
+ pidInput = m_pidInput;
+ }
+
+ if (enabled) {
+ double input = pidInput->PIDGet();
+ double result;
+ PIDOutput* pidOutput;
+
+ {
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+ m_error = m_setpoint - input;
+ if (m_continuous) {
+ if (std::fabs(m_error) > (m_maximumInput - m_minimumInput) / 2) {
+ if (m_error > 0) {
+ m_error = m_error - (m_maximumInput - m_minimumInput);
+ } else {
+ m_error = m_error + (m_maximumInput - m_minimumInput);
+ }
+ }
+ }
+
+ if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
+ if (m_P != 0) {
+ double potentialPGain = (m_totalError + m_error) * m_P;
+ if (potentialPGain < m_maximumOutput) {
+ if (potentialPGain > m_minimumOutput) {
+ m_totalError += m_error;
+ } else {
+ m_totalError = m_minimumOutput / m_P;
+ }
+ } else {
+ m_totalError = m_maximumOutput / m_P;
+ }
+ }
+
+ m_result = m_D * m_error + m_P * m_totalError + CalculateFeedForward();
+ } else {
+ if (m_I != 0) {
+ double potentialIGain = (m_totalError + m_error) * m_I;
+ if (potentialIGain < m_maximumOutput) {
+ if (potentialIGain > m_minimumOutput) {
+ m_totalError += m_error;
+ } else {
+ m_totalError = m_minimumOutput / m_I;
+ }
+ } else {
+ m_totalError = m_maximumOutput / m_I;
+ }
+ }
+
+ m_result = m_P * m_error + m_I * m_totalError +
+ m_D * (m_error - m_prevError) + CalculateFeedForward();
+ }
+ m_prevError = m_error;
+
+ if (m_result > m_maximumOutput)
+ m_result = m_maximumOutput;
+ else if (m_result < m_minimumOutput)
+ m_result = m_minimumOutput;
+
+ pidOutput = m_pidOutput;
+ result = m_result;
+ }
+
+ pidOutput->PIDWrite(result);
+ }
+}
+
+/**
+ * Calculate the feed forward term.
+ *
+ * Both of the provided feed forward calculations are velocity feed forwards.
+ * If a different feed forward calculation is desired, the user can override
+ * this function and provide his or her own. This function does no
+ * synchronization because the PIDController class only calls it in synchronized
+ * code, so be careful if calling it oneself.
+ *
+ * If a velocity PID controller is being used, the F term should be set to 1
+ * over the maximum setpoint for the output. If a position PID controller is
+ * being used, the F term should be set to 1 over the maximum speed for the
+ * output measured in setpoint units per this controller's update period (see
+ * the default period in this class's constructor).
+ */
+double PIDController::CalculateFeedForward() {
+ if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
+ return m_F * GetSetpoint();
+ } else {
+ double temp = m_F * GetDeltaSetpoint();
+ m_prevSetpoint = m_setpoint;
+ m_setpointTimer.Reset();
+ return temp;
+ }
+}
+
+/**
+ * Set the PID Controller gain parameters.
+ *
+ * Set the proportional, integral, and differential coefficients.
+ *
+ * @param p Proportional coefficient
+ * @param i Integral coefficient
+ * @param d Differential coefficient
+ */
+void PIDController::SetPID(double p, double i, double d) {
+ {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_P = p;
+ m_I = i;
+ m_D = d;
+ }
+
+ if (m_table != nullptr) {
+ m_table->PutNumber("p", m_P);
+ m_table->PutNumber("i", m_I);
+ m_table->PutNumber("d", m_D);
+ }
+}
+
+/**
+ * Set the PID Controller gain parameters.
+ *
+ * Set the proportional, integral, and differential coefficients.
+ *
+ * @param p Proportional coefficient
+ * @param i Integral coefficient
+ * @param d Differential coefficient
+ * @param f Feed forward coefficient
+ */
+void PIDController::SetPID(double p, double i, double d, double f) {
+ {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_P = p;
+ m_I = i;
+ m_D = d;
+ m_F = f;
+ }
+
+ if (m_table != nullptr) {
+ m_table->PutNumber("p", m_P);
+ m_table->PutNumber("i", m_I);
+ m_table->PutNumber("d", m_D);
+ m_table->PutNumber("f", m_F);
+ }
+}
+
+/**
+ * Get the Proportional coefficient.
+ *
+ * @return proportional coefficient
+ */
+double PIDController::GetP() const {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ return m_P;
+}
+
+/**
+ * Get the Integral coefficient.
+ *
+ * @return integral coefficient
+ */
+double PIDController::GetI() const {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ return m_I;
+}
+
+/**
+ * Get the Differential coefficient.
+ *
+ * @return differential coefficient
+ */
+double PIDController::GetD() const {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ return m_D;
+}
+
+/**
+ * Get the Feed forward coefficient.
+ *
+ * @return Feed forward coefficient
+ */
+double PIDController::GetF() const {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ return m_F;
+}
+
+/**
+ * Return the current PID result.
+ *
+ * This is always centered on zero and constrained the the max and min outs.
+ *
+ * @return the latest calculated output
+ */
+double PIDController::Get() const {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ return m_result;
+}
+
+/**
+ * Set the PID controller to consider the input to be continuous.
+ *
+ * Rather then using the max and min in as constraints, it considers them to
+ * be the same point and automatically calculates the shortest route to
+ * the setpoint.
+ *
+ * @param continuous Set to true turns on continuous, false turns off continuous
+ */
+void PIDController::SetContinuous(bool continuous) {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_continuous = continuous;
+}
+
+/**
+ * Sets the maximum and minimum values expected from the input.
+ *
+ * @param minimumInput the minimum value expected from the input
+ * @param maximumInput the maximum value expected from the output
+ */
+void PIDController::SetInputRange(double minimumInput, double maximumInput) {
+ {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_minimumInput = minimumInput;
+ m_maximumInput = maximumInput;
+ }
+
+ SetSetpoint(m_setpoint);
+}
+
+/**
+ * Sets the minimum and maximum values to write.
+ *
+ * @param minimumOutput the minimum value to write to the output
+ * @param maximumOutput the maximum value to write to the output
+ */
+void PIDController::SetOutputRange(double minimumOutput, double maximumOutput) {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_minimumOutput = minimumOutput;
+ m_maximumOutput = maximumOutput;
+}
+
+/**
+ * Set the setpoint for the PIDController.
+ *
+ * @param setpoint the desired setpoint
+ */
+void PIDController::SetSetpoint(double setpoint) {
+ {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ if (m_maximumInput > m_minimumInput) {
+ if (setpoint > m_maximumInput)
+ m_setpoint = m_maximumInput;
+ else if (setpoint < m_minimumInput)
+ m_setpoint = m_minimumInput;
+ else
+ m_setpoint = setpoint;
+ } else {
+ m_setpoint = setpoint;
+ }
+ }
+
+ if (m_table != nullptr) {
+ m_table->PutNumber("setpoint", m_setpoint);
+ }
+}
+
+/**
+ * Returns the current setpoint of the PIDController.
+ *
+ * @return the current setpoint
+ */
+double PIDController::GetSetpoint() const {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ return m_setpoint;
+}
+
+/**
+ * Returns the change in setpoint over time of the PIDController.
+ *
+ * @return the change in setpoint over time
+ */
+double PIDController::GetDeltaSetpoint() const {
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+ return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
+}
+
+/**
+ * Returns the current difference of the input from the setpoint.
+ *
+ * @return the current error
+ */
+double PIDController::GetError() const {
+ double setpoint = GetSetpoint();
+ {
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+ return GetContinuousError(setpoint - m_pidInput->PIDGet());
+ }
+}
+
+/**
+ * Sets what type of input the PID controller will use.
+ */
+void PIDController::SetPIDSourceType(PIDSourceType pidSource) {
+ m_pidInput->SetPIDSourceType(pidSource);
+}
+
+/**
+ * Returns the type of input the PID controller is using.
+ *
+ * @return the PID controller input type
+ */
+PIDSourceType PIDController::GetPIDSourceType() const {
+ return m_pidInput->GetPIDSourceType();
+}
+
+/**
+ * Returns the current average of the error over the past few iterations.
+ *
+ * You can specify the number of iterations to average with SetToleranceBuffer()
+ * (defaults to 1). This is the same value that is used for OnTarget().
+ *
+ * @return the average error
+ */
+double PIDController::GetAvgError() const {
+ double avgError = 0;
+ {
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+ // Don't divide by zero.
+ if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
+ }
+ return avgError;
+}
+
+/**
+ * Set the percentage error which is considered tolerable for use with
+ * OnTarget.
+ *
+ * @param percent percentage error which is tolerable
+ */
+void PIDController::SetTolerance(double percent) {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_toleranceType = kPercentTolerance;
+ m_tolerance = percent;
+}
+
+/**
+ * Set the absolute error which is considered tolerable for use with
+ * OnTarget.
+ *
+ * @param absTolerance absolute error which is tolerable
+ */
+void PIDController::SetAbsoluteTolerance(double absTolerance) {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_toleranceType = kAbsoluteTolerance;
+ m_tolerance = absTolerance;
+}
+
+/**
+ * Set the percentage error which is considered tolerable for use with
+ * OnTarget.
+ *
+ * @param percent percentage error which is tolerable
+ */
+void PIDController::SetPercentTolerance(double percent) {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_toleranceType = kPercentTolerance;
+ m_tolerance = percent;
+}
+
+/**
+ * Set the number of previous error samples to average for tolerancing.
+ *
+ * When determining whether a mechanism is on target, the user may want to use a
+ * rolling average of previous measurements instead of a precise position or
+ * velocity. This is useful for noisy sensors which return a few erroneous
+ * measurements when the mechanism is on target. However, the mechanism will
+ * not register as on target for at least the specified bufLength cycles.
+ * @param bufLength Number of previous cycles to average. Defaults to 1.
+ */
+void PIDController::SetToleranceBuffer(int bufLength) {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_bufLength = bufLength;
+
+ // Cut the buffer down to size if needed.
+ while (m_buf.size() > bufLength) {
+ m_bufTotal -= m_buf.front();
+ m_buf.pop();
+ }
+}
+
+/**
+ * Return true if the error is within the percentage of the total input range,
+ * determined by SetTolerance.
+ *
+ * This asssumes that the maximum and minimum input were set using SetInput.
+ * Currently this just reports on target as the actual value passes through the
+ * setpoint. Ideally it should be based on being within the tolerance for some
+ * period of time.
+ */
+bool PIDController::OnTarget() const {
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+ if (m_buf.size() == 0) return false;
+ double error = GetError();
+ switch (m_toleranceType) {
+ case kPercentTolerance:
+ return std::fabs(error) <
+ m_tolerance / 100 * (m_maximumInput - m_minimumInput);
+ break;
+ case kAbsoluteTolerance:
+ return std::fabs(error) < m_tolerance;
+ break;
+ case kNoTolerance: // TODO: this case needs an error
+ return false;
+ }
+ return false;
+}
+
+/**
+ * Begin running the PIDController.
+ */
+void PIDController::Enable() {
+ {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_enabled = true;
+ }
+
+ if (m_table != nullptr) {
+ m_table->PutBoolean("enabled", true);
+ }
+}
+
+/**
+ * Stop running the PIDController, this sets the output to zero before stopping.
+ */
+void PIDController::Disable() {
+ {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_pidOutput->PIDWrite(0);
+ m_enabled = false;
+ }
+
+ if (m_table != nullptr) {
+ m_table->PutBoolean("enabled", false);
+ }
+}
+
+/**
+ * Return true if PIDController is enabled.
+ */
+bool PIDController::IsEnabled() const {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ return m_enabled;
+}
+
+/**
+ * Reset the previous error,, the integral term, and disable the controller.
+ */
+void PIDController::Reset() {
+ Disable();
+
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_prevError = 0;
+ m_totalError = 0;
+ m_result = 0;
+}
+
+std::string PIDController::GetSmartDashboardType() const {
+ return "PIDController";
+}
+
+void PIDController::InitTable(std::shared_ptr<ITable> table) {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+ m_table = table;
+ if (m_table != nullptr) {
+ m_table->PutNumber(kP, GetP());
+ m_table->PutNumber(kI, GetI());
+ m_table->PutNumber(kD, GetD());
+ m_table->PutNumber(kF, GetF());
+ m_table->PutNumber(kSetpoint, GetSetpoint());
+ m_table->PutBoolean(kEnabled, IsEnabled());
+ m_table->AddTableListener(this, false);
+ }
+}
+
+/**
+ * Wraps error around for continuous inputs. The original error is returned if
+ * continuous mode is disabled. This is an unsynchronized function.
+ *
+ * @param error The current error of the PID controller.
+ * @return Error for continuous inputs.
+ */
+double PIDController::GetContinuousError(double error) const {
+ if (m_continuous) {
+ if (std::fabs(error) > (m_maximumInput - m_minimumInput) / 2) {
+ if (error > 0) {
+ return error - (m_maximumInput - m_minimumInput);
+ } else {
+ return error + (m_maximumInput - m_minimumInput);
+ }
+ }
+ }
+
+ return error;
+}
+
+std::shared_ptr<ITable> PIDController::GetTable() const { return m_table; }
+
+void PIDController::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (key == kP || key == kI || key == kD || key == kF) {
+ if (m_P != m_table->GetNumber(kP, 0.0) ||
+ m_I != m_table->GetNumber(kI, 0.0) ||
+ m_D != m_table->GetNumber(kD, 0.0) ||
+ m_F != m_table->GetNumber(kF, 0.0)) {
+ SetPID(m_table->GetNumber(kP, 0.0), m_table->GetNumber(kI, 0.0),
+ m_table->GetNumber(kD, 0.0), m_table->GetNumber(kF, 0.0));
+ }
+ } else if (key == kSetpoint && value->IsDouble() &&
+ m_setpoint != value->GetDouble()) {
+ SetSetpoint(value->GetDouble());
+ } else if (key == kEnabled && value->IsBoolean() &&
+ m_enabled != value->GetBoolean()) {
+ if (value->GetBoolean()) {
+ Enable();
+ } else {
+ Disable();
+ }
+ }
+}
+
+void PIDController::UpdateTable() {}
+
+void PIDController::StartLiveWindowMode() { Disable(); }
+
+void PIDController::StopLiveWindowMode() {}
diff --git a/wpilibc/sim/src/PWM.cpp b/wpilibc/sim/src/PWM.cpp
new file mode 100644
index 0000000..efdaae7
--- /dev/null
+++ b/wpilibc/sim/src/PWM.cpp
@@ -0,0 +1,243 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "PWM.h"
+
+#include <sstream>
+
+#include "Utility.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+const double PWM::kDefaultPwmPeriod = 5.05;
+const double PWM::kDefaultPwmCenter = 1.5;
+const int PWM::kDefaultPwmStepsDown = 1000;
+const int PWM::kPwmDisabled = 0;
+
+/**
+ * Allocate a PWM given a channel number.
+ *
+ * Checks channel value range and allocates the appropriate channel.
+ * The allocation is only done to help users ensure that they don't double
+ * assign channels.
+ *
+ * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP
+ * port
+ */
+PWM::PWM(int channel) {
+ std::stringstream ss;
+
+ if (!CheckPWMChannel(channel)) {
+ ss << "PWM Channel " << channel;
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, ss.str());
+ return;
+ }
+
+ ss << "pwm/" << channel;
+ impl = new SimContinuousOutput(ss.str());
+ m_channel = channel;
+ m_eliminateDeadband = false;
+
+ m_centerPwm = kPwmDisabled; // In simulation, the same thing.
+}
+
+PWM::~PWM() {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Optionally eliminate the deadband from a speed controller.
+ *
+ * @param eliminateDeadband If true, set the motor curve on the Jaguar to
+ * eliminate the deadband in the middle of the range.
+ * Otherwise, keep the full range without modifying
+ * any values.
+ */
+void PWM::EnableDeadbandElimination(bool eliminateDeadband) {
+ m_eliminateDeadband = eliminateDeadband;
+}
+
+/**
+ * Set the bounds on the PWM values.
+ *
+ * This sets the bounds on the PWM values for a particular each type of
+ * controller. The values determine the upper and lower speeds as well as the
+ * deadband bracket.
+ *
+ * @param max The Minimum pwm value
+ * @param deadbandMax The high end of the deadband range
+ * @param center The center speed (off)
+ * @param deadbandMin The low end of the deadband range
+ * @param min The minimum pwm value
+ */
+void PWM::SetBounds(int max, int deadbandMax, int center, int deadbandMin,
+ int min) {
+ // Nothing to do in simulation.
+}
+
+/**
+ * Set the bounds on the PWM pulse widths.
+ *
+ * This sets the bounds on the PWM values for a particular type of controller.
+ * The values determine the upper and lower speeds as well as the deadband
+ * bracket.
+ *
+ * @param max The max PWM pulse width in ms
+ * @param deadbandMax The high end of the deadband range pulse width in ms
+ * @param center The center (off) pulse width in ms
+ * @param deadbandMin The low end of the deadband pulse width in ms
+ * @param min The minimum pulse width in ms
+ */
+void PWM::SetBounds(double max, double deadbandMax, double center,
+ double deadbandMin, double min) {
+ // Nothing to do in simulation.
+}
+
+/**
+ * Set the PWM value based on a position.
+ *
+ * This is intended to be used by servos.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @param pos The position to set the servo between 0.0 and 1.0.
+ */
+void PWM::SetPosition(double pos) {
+ if (pos < 0.0) {
+ pos = 0.0;
+ } else if (pos > 1.0) {
+ pos = 1.0;
+ }
+
+ impl->Set(pos);
+}
+
+/**
+ * Get the PWM value in terms of a position.
+ *
+ * This is intended to be used by servos.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @return The position the servo is set to between 0.0 and 1.0.
+ */
+double PWM::GetPosition() const {
+ double value = impl->Get();
+ if (value < 0.0) {
+ return 0.0;
+ } else if (value > 1.0) {
+ return 1.0;
+ } else {
+ return value;
+ }
+}
+
+/**
+ * Set the PWM value based on a speed.
+ *
+ * This is intended to be used by speed controllers.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinPositivePwm() called.
+ * @pre SetCenterPwm() called.
+ * @pre SetMaxNegativePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @param speed The speed to set the speed controller between -1.0 and 1.0.
+ */
+void PWM::SetSpeed(double speed) {
+ // clamp speed to be in the range 1.0 >= speed >= -1.0
+ if (speed < -1.0) {
+ speed = -1.0;
+ } else if (speed > 1.0) {
+ speed = 1.0;
+ }
+
+ impl->Set(speed);
+}
+
+/**
+ * Get the PWM value in terms of speed.
+ *
+ * This is intended to be used by speed controllers.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinPositivePwm() called.
+ * @pre SetMaxNegativePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @return The most recently set speed between -1.0 and 1.0.
+ */
+double PWM::GetSpeed() const {
+ double value = impl->Get();
+ if (value > 1.0) {
+ return 1.0;
+ } else if (value < -1.0) {
+ return -1.0;
+ } else {
+ return value;
+ }
+}
+
+/**
+ * Set the PWM value directly to the hardware.
+ *
+ * Write a raw value to a PWM channel.
+ *
+ * @param value Raw PWM value.
+ */
+void PWM::SetRaw(uint16_t value) {
+ wpi_assert(value == kPwmDisabled);
+ impl->Set(0);
+}
+
+/**
+ * Slow down the PWM signal for old devices.
+ *
+ * @param mult The period multiplier to apply to this channel
+ */
+void PWM::SetPeriodMultiplier(PeriodMultiplier mult) {
+ // Do nothing in simulation.
+}
+
+void PWM::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (!value->IsDouble()) return;
+ SetSpeed(value->GetDouble());
+}
+
+void PWM::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", GetSpeed());
+ }
+}
+
+void PWM::StartLiveWindowMode() {
+ SetSpeed(0);
+ if (m_table != nullptr) {
+ m_table->AddTableListener("Value", this, true);
+ }
+}
+
+void PWM::StopLiveWindowMode() {
+ SetSpeed(0);
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string PWM::GetSmartDashboardType() const { return "Speed Controller"; }
+
+void PWM::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> PWM::GetTable() const { return m_table; }
diff --git a/wpilibc/sim/src/Relay.cpp b/wpilibc/sim/src/Relay.cpp
new file mode 100644
index 0000000..898612a
--- /dev/null
+++ b/wpilibc/sim/src/Relay.cpp
@@ -0,0 +1,245 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Relay.h"
+
+#include <sstream>
+
+#include "LiveWindow/LiveWindow.h"
+#include "MotorSafetyHelper.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+/**
+ * Relay constructor given a channel.
+ *
+ * This code initializes the relay and reserves all resources that need to be
+ * locked. Initially the relay is set to both lines at 0v.
+ *
+ * @param channel The channel number (0-3).
+ * @param direction The direction that the Relay object will control.
+ */
+Relay::Relay(int channel, Relay::Direction direction)
+ : m_channel(channel), m_direction(direction) {
+ std::stringstream ss;
+ if (!SensorBase::CheckRelayChannel(m_channel)) {
+ ss << "Relay Channel " << m_channel;
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, ss.str());
+ return;
+ }
+
+ m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
+ m_safetyHelper->SetSafetyEnabled(false);
+
+ ss << "relay/" << m_channel;
+ impl = new SimContinuousOutput(ss.str()); // TODO: Allow two different relays
+ // (targetting the different halves
+ // of a relay) to be combined to
+ // control one motor.
+ LiveWindow::GetInstance()->AddActuator("Relay", 1, m_channel, this);
+ go_pos = go_neg = false;
+}
+
+/**
+ * Free the resource associated with a relay.
+ *
+ * The relay channels are set to free and the relay output is turned off.
+ */
+Relay::~Relay() {
+ impl->Set(0);
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Set the relay state.
+ *
+ * Valid values depend on which directions of the relay are controlled by the
+ * object.
+ *
+ * When set to kBothDirections, the relay can be any of the four states:
+ * 0v-0v, 0v-12v, 12v-0v, 12v-12v
+ *
+ * When set to kForwardOnly or kReverseOnly, you can specify the constant for
+ * the direction or you can simply specify kOff and kOn. Using only kOff and
+ * kOn is recommended.
+ *
+ * @param value The state to set the relay.
+ */
+void Relay::Set(Relay::Value value) {
+ switch (value) {
+ case kOff:
+ if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+ go_pos = false;
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+ go_neg = false;
+ }
+ break;
+ case kOn:
+ if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+ go_pos = true;
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+ go_neg = true;
+ }
+ break;
+ case kForward:
+ if (m_direction == kReverseOnly) {
+ wpi_setWPIError(IncompatibleMode);
+ break;
+ }
+ if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+ go_pos = true;
+ }
+ if (m_direction == kBothDirections) {
+ go_neg = false;
+ }
+ break;
+ case kReverse:
+ if (m_direction == kForwardOnly) {
+ wpi_setWPIError(IncompatibleMode);
+ break;
+ }
+ if (m_direction == kBothDirections) {
+ go_pos = false;
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+ go_neg = true;
+ }
+ break;
+ }
+ impl->Set((go_pos ? 1 : 0) + (go_neg ? -1 : 0));
+}
+
+/**
+ * Get the Relay State
+ *
+ * Gets the current state of the relay.
+ *
+ * When set to kForwardOnly or kReverseOnly, value is returned as kOn/kOff not
+ * kForward/kReverse (per the recommendation in Set).
+ *
+ * @return The current state of the relay as a Relay::Value
+ */
+Relay::Value Relay::Get() const {
+ // TODO: Don't assume that the go_pos and go_neg fields are correct?
+ if ((go_pos || m_direction == kReverseOnly) &&
+ (go_neg || m_direction == kForwardOnly)) {
+ return kOn;
+ } else if (go_pos) {
+ return kForward;
+ } else if (go_neg) {
+ return kReverse;
+ } else {
+ return kOff;
+ }
+}
+
+int Relay::GetChannel() const { return m_channel; }
+
+/**
+ * Set the expiration time for the Relay object.
+ *
+ * @param timeout The timeout (in seconds) for this relay object
+ */
+void Relay::SetExpiration(double timeout) {
+ m_safetyHelper->SetExpiration(timeout);
+}
+
+/**
+ * Return the expiration time for the relay object.
+ *
+ * @return The expiration time value.
+ */
+double Relay::GetExpiration() const { return m_safetyHelper->GetExpiration(); }
+
+/**
+ * Check if the relay object is currently alive or stopped due to a timeout.
+ *
+ * @return a bool value that is true if the motor has NOT timed out and should
+ * still be running.
+ */
+bool Relay::IsAlive() const { return m_safetyHelper->IsAlive(); }
+
+/**
+ * Stop the motor associated with this PWM object.
+ *
+ * This is called by the MotorSafetyHelper object when it has a timeout for this
+ * relay and needs to stop it from running.
+ */
+void Relay::StopMotor() { Set(kOff); }
+
+/**
+ * Enable/disable motor safety for this device
+ *
+ * Turn on and off the motor safety option for this relay object.
+ *
+ * @param enabled True if motor safety is enforced for this object
+ */
+void Relay::SetSafetyEnabled(bool enabled) {
+ m_safetyHelper->SetSafetyEnabled(enabled);
+}
+
+/**
+ * Check if motor safety is enabled for this object.
+ *
+ * @return True if motor safety is enforced for this object
+ */
+bool Relay::IsSafetyEnabled() const {
+ return m_safetyHelper->IsSafetyEnabled();
+}
+
+void Relay::GetDescription(std::ostringstream& desc) const {
+ desc << "Relay " << GetChannel();
+}
+
+void Relay::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (!value->IsString()) return;
+ if (value->GetString() == "Off")
+ Set(kOff);
+ else if (value->GetString() == "Forward")
+ Set(kForward);
+ else if (value->GetString() == "Reverse")
+ Set(kReverse);
+}
+
+void Relay::UpdateTable() {
+ if (m_table != nullptr) {
+ if (Get() == kOn) {
+ m_table->PutString("Value", "On");
+ } else if (Get() == kForward) {
+ m_table->PutString("Value", "Forward");
+ } else if (Get() == kReverse) {
+ m_table->PutString("Value", "Reverse");
+ } else {
+ m_table->PutString("Value", "Off");
+ }
+ }
+}
+
+void Relay::StartLiveWindowMode() {
+ if (m_table != nullptr) {
+ m_table->AddTableListener("Value", this, true);
+ }
+}
+
+void Relay::StopLiveWindowMode() {
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string Relay::GetSmartDashboardType() const { return "Relay"; }
+
+void Relay::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> Relay::GetTable() const { return m_table; }
diff --git a/wpilibc/sim/src/RobotBase.cpp b/wpilibc/sim/src/RobotBase.cpp
new file mode 100644
index 0000000..82a2e65
--- /dev/null
+++ b/wpilibc/sim/src/RobotBase.cpp
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotBase.h"
+
+#include "RobotState.h"
+#include "SmartDashboard/SmartDashboard.h"
+#include "Utility.h"
+
+using namespace frc;
+
+/**
+ * Constructor for a generic robot program.
+ *
+ * User code should be placed in the constuctor that runs before the Autonomous
+ * or Operator Control period starts. The constructor will run to completion
+ * before Autonomous is entered.
+ *
+ * This must be used to ensure that the communications code starts. In the
+ * future it would be nice to put this code into it's own task that loads on
+ * boot so ensure that it runs.
+ */
+RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
+ RobotState::SetImplementation(DriverStation::GetInstance());
+ SmartDashboard::init();
+ time_sub = MainNode::Subscribe("~/time", &wpilib::internal::time_callback);
+}
+
+/**
+ * Determine if the Robot is currently enabled.
+ *
+ * @return True if the Robot is currently enabled by the field controls.
+ */
+bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); }
+
+/**
+ * Determine if the Robot is currently disabled.
+ *
+ * @return True if the Robot is currently disabled by the field controls.
+ */
+bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); }
+
+/**
+ * Determine if the robot is currently in Autnomous mode.
+ *
+ * @return True if the robot is currently operating Autonomously as determined
+ * by the field controls.
+ */
+bool RobotBase::IsAutonomous() const { return m_ds.IsAutonomous(); }
+
+/**
+ * Determine if the robot is currently in Operator Control mode.
+ *
+ * @return True if the robot is currently operating in Tele-Op mode as
+ * determined by the field controls.
+ */
+bool RobotBase::IsOperatorControl() const { return m_ds.IsOperatorControl(); }
+
+/**
+ * Determine if the robot is currently in Test mode.
+ *
+ * @return True if the robot is currently running tests as determined by the
+ * field controls.
+ */
+bool RobotBase::IsTest() const { return m_ds.IsTest(); }
diff --git a/wpilibc/sim/src/RobotDrive.cpp b/wpilibc/sim/src/RobotDrive.cpp
new file mode 100644
index 0000000..8974465
--- /dev/null
+++ b/wpilibc/sim/src/RobotDrive.cpp
@@ -0,0 +1,728 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include "GenericHID.h"
+#include "Joystick.h"
+#include "Talon.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+const int RobotDrive::kMaxNumberOfMotors;
+
+static auto make_shared_nodelete(SpeedController* ptr) {
+ return std::shared_ptr<SpeedController>(ptr, NullDeleter<SpeedController>());
+}
+
+/*
+ * Driving functions.
+ *
+ * These functions provide an interface to multiple motors that is used for C
+ * programming.
+ * The Drive(speed, direction) function is the main part of the set that makes
+ * it easy to set speeds and direction independently in one call.
+ */
+
+/**
+ * Common function to initialize all the robot drive constructors.
+ * Create a motor safety object (the real reason for the common code) and
+ * initialize all the motor assignments. The default timeout is set for the
+ * robot drive.
+ */
+void RobotDrive::InitRobotDrive() {
+ // FIXME: m_safetyHelper = new MotorSafetyHelper(this);
+ // FIXME: m_safetyHelper->SetSafetyEnabled(true);
+}
+
+/**
+ * Constructor for RobotDrive with 2 motors specified with channel numbers.
+ *
+ * Set up parameters for a two wheel drive system where the
+ * left and right motor pwm channels are specified in the call.
+ * This call assumes Talons for controlling the motors.
+ *
+ * @param leftMotorChannel The PWM channel number that drives the left motor.
+ * @param rightMotorChannel The PWM channel number that drives the right motor.
+ */
+RobotDrive::RobotDrive(int leftMotorChannel, int rightMotorChannel) {
+ InitRobotDrive();
+ m_rearLeftMotor = std::make_shared<Talon>(leftMotorChannel);
+ m_rearRightMotor = std::make_shared<Talon>(rightMotorChannel);
+ SetLeftRightMotorOutputs(0.0, 0.0);
+ m_deleteSpeedControllers = true;
+}
+
+/**
+ * Constructor for RobotDrive with 4 motors specified with channel numbers.
+ *
+ * Set up parameters for a four wheel drive system where all four motor
+ * pwm channels are specified in the call.
+ * This call assumes Talons for controlling the motors.
+ *
+ * @param frontLeftMotor Front left motor channel number
+ * @param rearLeftMotor Rear Left motor channel number
+ * @param frontRightMotor Front right motor channel number
+ * @param rearRightMotor Rear Right motor channel number
+ */
+RobotDrive::RobotDrive(int frontLeftMotor, int rearLeftMotor,
+ int frontRightMotor, int rearRightMotor) {
+ InitRobotDrive();
+ m_rearLeftMotor = std::make_shared<Talon>(rearLeftMotor);
+ m_rearRightMotor = std::make_shared<Talon>(rearRightMotor);
+ m_frontLeftMotor = std::make_shared<Talon>(frontLeftMotor);
+ m_frontRightMotor = std::make_shared<Talon>(frontRightMotor);
+ SetLeftRightMotorOutputs(0.0, 0.0);
+ m_deleteSpeedControllers = true;
+}
+
+/**
+ * Constructor for RobotDrive with 2 motors specified as SpeedController
+ * objects.
+ *
+ * The SpeedController version of the constructor enables programs to use the
+ * RobotDrive classes with subclasses of the SpeedController objects, for
+ * example, versions with ramping or reshaping of the curve to suit motor bias
+ * or deadband elimination.
+ *
+ * @param leftMotor The left SpeedController object used to drive the robot.
+ * @param rightMotor the right SpeedController object used to drive the robot.
+ */
+RobotDrive::RobotDrive(SpeedController* leftMotor,
+ SpeedController* rightMotor) {
+ InitRobotDrive();
+ if (leftMotor == nullptr || rightMotor == nullptr) {
+ wpi_setWPIError(NullParameter);
+ m_rearLeftMotor = m_rearRightMotor = nullptr;
+ return;
+ }
+ m_rearLeftMotor = make_shared_nodelete(leftMotor);
+ m_rearRightMotor = make_shared_nodelete(rightMotor);
+}
+
+// TODO: Change to rvalue references & move syntax.
+RobotDrive::RobotDrive(SpeedController& leftMotor,
+ SpeedController& rightMotor) {
+ InitRobotDrive();
+ m_rearLeftMotor = make_shared_nodelete(&leftMotor);
+ m_rearRightMotor = make_shared_nodelete(&rightMotor);
+}
+
+RobotDrive::RobotDrive(std::shared_ptr<SpeedController> leftMotor,
+ std::shared_ptr<SpeedController> rightMotor) {
+ InitRobotDrive();
+ if (leftMotor == nullptr || rightMotor == nullptr) {
+ wpi_setWPIError(NullParameter);
+ m_rearLeftMotor = m_rearRightMotor = nullptr;
+ return;
+ }
+ m_rearLeftMotor = leftMotor;
+ m_rearRightMotor = rightMotor;
+}
+
+/**
+ * Constructor for RobotDrive with 4 motors specified as SpeedController
+ * objects.
+ *
+ * Speed controller input version of RobotDrive (see previous comments).
+ *
+ * @param rearLeftMotor The back left SpeedController object used to drive the
+ * robot.
+ * @param frontLeftMotor The front left SpeedController object used to drive
+ * the robot
+ * @param rearRightMotor The back right SpeedController object used to drive
+ * the robot.
+ * @param frontRightMotor The front right SpeedController object used to drive
+ * the robot.
+ */
+RobotDrive::RobotDrive(SpeedController* frontLeftMotor,
+ SpeedController* rearLeftMotor,
+ SpeedController* frontRightMotor,
+ SpeedController* rearRightMotor) {
+ InitRobotDrive();
+ if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
+ frontRightMotor == nullptr || rearRightMotor == nullptr) {
+ wpi_setWPIError(NullParameter);
+ return;
+ }
+ m_frontLeftMotor = make_shared_nodelete(frontLeftMotor);
+ m_rearLeftMotor = make_shared_nodelete(rearLeftMotor);
+ m_frontRightMotor = make_shared_nodelete(frontRightMotor);
+ m_rearRightMotor = make_shared_nodelete(rearRightMotor);
+}
+
+RobotDrive::RobotDrive(SpeedController& frontLeftMotor,
+ SpeedController& rearLeftMotor,
+ SpeedController& frontRightMotor,
+ SpeedController& rearRightMotor) {
+ InitRobotDrive();
+ m_frontLeftMotor = make_shared_nodelete(&frontLeftMotor);
+ m_rearLeftMotor = make_shared_nodelete(&rearLeftMotor);
+ m_frontRightMotor = make_shared_nodelete(&frontRightMotor);
+ m_rearRightMotor = make_shared_nodelete(&rearRightMotor);
+}
+
+RobotDrive::RobotDrive(std::shared_ptr<SpeedController> frontLeftMotor,
+ std::shared_ptr<SpeedController> rearLeftMotor,
+ std::shared_ptr<SpeedController> frontRightMotor,
+ std::shared_ptr<SpeedController> rearRightMotor) {
+ InitRobotDrive();
+ if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
+ frontRightMotor == nullptr || rearRightMotor == nullptr) {
+ wpi_setWPIError(NullParameter);
+ return;
+ }
+ m_frontLeftMotor = frontLeftMotor;
+ m_rearLeftMotor = rearLeftMotor;
+ m_frontRightMotor = frontRightMotor;
+ m_rearRightMotor = rearRightMotor;
+}
+
+/**
+ * Drive the motors at "outputMagnitude" and "curve".
+ * Both outputMagnitude and curve are -1.0 to +1.0 values, where 0.0 represents
+ * stopped and not turning. curve < 0 will turn left and curve > 0 will turn
+ * right.
+ *
+ * The algorithm for steering provides a constant turn radius for any normal
+ * speed range, both forward and backward. Increasing m_sensitivity causes
+ * sharper turns for fixed values of curve.
+ *
+ * This function will most likely be used in an autonomous routine.
+ *
+ * @param outputMagnitude The speed setting for the outside wheel in a turn,
+ * forward or backwards, +1 to -1.
+ * @param curve The rate of turn, constant for different forward
+ * speeds. Set curve < 0 for left turn or curve > 0 for
+ * right turn. Set curve = e^(-r/w) to get a turn radius
+ * r for wheelbase w of your robot. Conversely, turn
+ * radius r = -ln(curve)*w for a given value of curve and
+ * wheelbase w.
+ */
+void RobotDrive::Drive(double outputMagnitude, double curve) {
+ double leftOutput, rightOutput;
+ static bool reported = false;
+ if (!reported) {
+ reported = true;
+ }
+
+ if (curve < 0) {
+ double value = std::log(-curve);
+ double ratio = (value - m_sensitivity) / (value + m_sensitivity);
+ if (ratio == 0) ratio = .0000000001;
+ leftOutput = outputMagnitude / ratio;
+ rightOutput = outputMagnitude;
+ } else if (curve > 0) {
+ double value = std::log(curve);
+ double ratio = (value - m_sensitivity) / (value + m_sensitivity);
+ if (ratio == 0) ratio = .0000000001;
+ leftOutput = outputMagnitude;
+ rightOutput = outputMagnitude / ratio;
+ } else {
+ leftOutput = outputMagnitude;
+ rightOutput = outputMagnitude;
+ }
+ SetLeftRightMotorOutputs(leftOutput, rightOutput);
+}
+
+/**
+ * Provide tank steering using the stored robot configuration.
+ *
+ * Drive the robot using two joystick inputs. The Y-axis will be selected from
+ * each Joystick object.
+ *
+ * @param leftStick The joystick to control the left side of the robot.
+ * @param rightStick The joystick to control the right side of the robot.
+ */
+void RobotDrive::TankDrive(GenericHID* leftStick, GenericHID* rightStick,
+ bool squaredInputs) {
+ if (leftStick == nullptr || rightStick == nullptr) {
+ wpi_setWPIError(NullParameter);
+ return;
+ }
+ TankDrive(leftStick->GetY(), rightStick->GetY(), squaredInputs);
+}
+
+void RobotDrive::TankDrive(GenericHID& leftStick, GenericHID& rightStick,
+ bool squaredInputs) {
+ TankDrive(leftStick.GetY(), rightStick.GetY(), squaredInputs);
+}
+
+/**
+ * Provide tank steering using the stored robot configuration.
+ *
+ * This function lets you pick the axis to be used on each Joystick object for
+ * the left and right sides of the robot.
+ *
+ * @param leftStick The Joystick object to use for the left side of the robot.
+ * @param leftAxis The axis to select on the left side Joystick object.
+ * @param rightStick The Joystick object to use for the right side of the robot.
+ * @param rightAxis The axis to select on the right side Joystick object.
+ */
+void RobotDrive::TankDrive(GenericHID* leftStick, int leftAxis,
+ GenericHID* rightStick, int rightAxis,
+ bool squaredInputs) {
+ if (leftStick == nullptr || rightStick == nullptr) {
+ wpi_setWPIError(NullParameter);
+ return;
+ }
+ TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis),
+ squaredInputs);
+}
+
+void RobotDrive::TankDrive(GenericHID& leftStick, int leftAxis,
+ GenericHID& rightStick, int rightAxis,
+ bool squaredInputs) {
+ TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis),
+ squaredInputs);
+}
+
+/**
+ * Provide tank steering using the stored robot configuration.
+ *
+ * This function lets you directly provide joystick values from any source.
+ *
+ * @param leftValue The value of the left stick.
+ * @param rightValue The value of the right stick.
+ */
+void RobotDrive::TankDrive(double leftValue, double rightValue,
+ bool squaredInputs) {
+ static bool reported = false;
+ if (!reported) {
+ reported = true;
+ }
+
+ // square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power
+ leftValue = Limit(leftValue);
+ rightValue = Limit(rightValue);
+ if (squaredInputs) {
+ if (leftValue >= 0.0) {
+ leftValue = (leftValue * leftValue);
+ } else {
+ leftValue = -(leftValue * leftValue);
+ }
+ if (rightValue >= 0.0) {
+ rightValue = (rightValue * rightValue);
+ } else {
+ rightValue = -(rightValue * rightValue);
+ }
+ }
+
+ SetLeftRightMotorOutputs(leftValue, rightValue);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ *
+ * Given a single Joystick, the class assumes the Y axis for the move value and
+ * the X axis for the rotate value.
+ * (Should add more information here regarding the way that arcade drive works.)
+ *
+ * @param stick The joystick to use for Arcade single-stick driving.
+ * The Y-axis will be selected for forwards/backwards and
+ * the X-axis will be selected for rotation rate.
+ * @param squaredInputs If true, the sensitivity will be increased for small
+ * values
+ */
+void RobotDrive::ArcadeDrive(GenericHID* stick, bool squaredInputs) {
+ // simply call the full-featured ArcadeDrive with the appropriate values
+ ArcadeDrive(stick->GetY(), stick->GetX(), squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ *
+ * Given a single Joystick, the class assumes the Y axis for the move value and
+ * the X axis for the rotate value.
+ * (Should add more information here regarding the way that arcade drive works.)
+ *
+ * @param stick The joystick to use for Arcade single-stick driving.
+ * The Y-axis will be selected for forwards/backwards and
+ * the X-axis will be selected for rotation rate.
+ * @param squaredInputs If true, the sensitivity will be increased for small
+ * values
+ */
+void RobotDrive::ArcadeDrive(GenericHID& stick, bool squaredInputs) {
+ // simply call the full-featured ArcadeDrive with the appropriate values
+ ArcadeDrive(stick.GetY(), stick.GetX(), squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ *
+ * Given two joystick instances and two axis, compute the values to send to
+ * either two or four motors.
+ *
+ * @param moveStick The Joystick object that represents the forward/backward
+ * direction
+ * @param moveAxis The axis on the moveStick object to use for
+ * forwards/backwards (typically Y_AXIS)
+ * @param rotateStick The Joystick object that represents the rotation value
+ * @param rotateAxis The axis on the rotation object to use for the rotate
+ * right/left (typically X_AXIS)
+ * @param squaredInputs Setting this parameter to true increases the sensitivity
+ * at lower speeds
+ */
+void RobotDrive::ArcadeDrive(GenericHID* moveStick, int moveAxis,
+ GenericHID* rotateStick, int rotateAxis,
+ bool squaredInputs) {
+ double moveValue = moveStick->GetRawAxis(moveAxis);
+ double rotateValue = rotateStick->GetRawAxis(rotateAxis);
+
+ ArcadeDrive(moveValue, rotateValue, squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ *
+ * Given two joystick instances and two axis, compute the values to send to
+ * either two or four motors.
+ *
+ * @param moveStick The Joystick object that represents the forward/backward
+ * direction
+ * @param moveAxis The axis on the moveStick object to use for
+ * forwards/backwards (typically Y_AXIS)
+ * @param rotateStick The Joystick object that represents the rotation value
+ * @param rotateAxis The axis on the rotation object to use for the rotate
+ * right/left (typically X_AXIS)
+ * @param squaredInputs Setting this parameter to true increases the sensitivity
+ * at lower speeds
+ */
+void RobotDrive::ArcadeDrive(GenericHID& moveStick, int moveAxis,
+ GenericHID& rotateStick, int rotateAxis,
+ bool squaredInputs) {
+ double moveValue = moveStick.GetRawAxis(moveAxis);
+ double rotateValue = rotateStick.GetRawAxis(rotateAxis);
+
+ ArcadeDrive(moveValue, rotateValue, squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ *
+ * This function lets you directly provide joystick values from any source.
+ *
+ * @param moveValue The value to use for fowards/backwards
+ * @param rotateValue The value to use for the rotate right/left
+ * @param squaredInputs If set, increases the sensitivity at low speeds
+ */
+void RobotDrive::ArcadeDrive(double moveValue, double rotateValue,
+ bool squaredInputs) {
+ static bool reported = false;
+ if (!reported) {
+ reported = true;
+ }
+
+ // local variables to hold the computed PWM values for the motors
+ double leftMotorOutput;
+ double rightMotorOutput;
+
+ moveValue = Limit(moveValue);
+ rotateValue = Limit(rotateValue);
+
+ if (squaredInputs) {
+ // square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power
+ if (moveValue >= 0.0) {
+ moveValue = (moveValue * moveValue);
+ } else {
+ moveValue = -(moveValue * moveValue);
+ }
+ if (rotateValue >= 0.0) {
+ rotateValue = (rotateValue * rotateValue);
+ } else {
+ rotateValue = -(rotateValue * rotateValue);
+ }
+ }
+
+ if (moveValue > 0.0) {
+ if (rotateValue > 0.0) {
+ leftMotorOutput = moveValue - rotateValue;
+ rightMotorOutput = std::max(moveValue, rotateValue);
+ } else {
+ leftMotorOutput = std::max(moveValue, -rotateValue);
+ rightMotorOutput = moveValue + rotateValue;
+ }
+ } else {
+ if (rotateValue > 0.0) {
+ leftMotorOutput = -std::max(-moveValue, rotateValue);
+ rightMotorOutput = moveValue + rotateValue;
+ } else {
+ leftMotorOutput = moveValue - rotateValue;
+ rightMotorOutput = -std::max(-moveValue, -rotateValue);
+ }
+ }
+ SetLeftRightMotorOutputs(leftMotorOutput, rightMotorOutput);
+}
+
+/**
+ * Drive method for Mecanum wheeled robots.
+ *
+ * A method for driving with Mecanum wheeled robots. There are 4 wheels
+ * on the robot, arranged so that the front and back wheels are toed in 45
+ * degrees. When looking at the wheels from the top, the roller axles should
+ * form an X across the robot.
+ *
+ * This is designed to be directly driven by joystick axes.
+ *
+ * @param x The speed that the robot should drive in the X direction.
+ * [-1.0..1.0]
+ * @param y The speed that the robot should drive in the Y direction.
+ * This input is inverted to match the forward == -1.0 that
+ * joysticks produce. [-1.0..1.0]
+ * @param rotation The rate of rotation for the robot that is completely
+ * independent of the translation. [-1.0..1.0]
+ * @param gyroAngle The current angle reading from the gyro. Use this to
+ * implement field-oriented controls.
+ */
+void RobotDrive::MecanumDrive_Cartesian(double x, double y, double rotation,
+ double gyroAngle) {
+ static bool reported = false;
+ if (!reported) {
+ reported = true;
+ }
+
+ double xIn = x;
+ double yIn = y;
+ // Negate y for the joystick.
+ yIn = -yIn;
+ // Compenstate for gyro angle.
+ RotateVector(xIn, yIn, gyroAngle);
+
+ double wheelSpeeds[kMaxNumberOfMotors];
+ wheelSpeeds[kFrontLeftMotor] = xIn + yIn + rotation;
+ wheelSpeeds[kFrontRightMotor] = -xIn + yIn - rotation;
+ wheelSpeeds[kRearLeftMotor] = -xIn + yIn + rotation;
+ wheelSpeeds[kRearRightMotor] = xIn + yIn - rotation;
+
+ Normalize(wheelSpeeds);
+
+ m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] *
+ m_invertedMotors[kFrontLeftMotor] * m_maxOutput);
+ m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] *
+ m_invertedMotors[kFrontRightMotor] * m_maxOutput);
+ m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] *
+ m_invertedMotors[kRearLeftMotor] * m_maxOutput);
+ m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] *
+ m_invertedMotors[kRearRightMotor] * m_maxOutput);
+
+ // FIXME: m_safetyHelper->Feed();
+}
+
+/**
+ * Drive method for Mecanum wheeled robots.
+ *
+ * A method for driving with Mecanum wheeled robots. There are 4 wheels
+ * on the robot, arranged so that the front and back wheels are toed in 45
+ * degrees. When looking at the wheels from the top, the roller axles should
+ * form an X across the robot.
+ *
+ * @param magnitude The speed that the robot should drive in a given direction.
+ * [-1.0..1.0]
+ * @param direction The direction the robot should drive in degrees. The
+ * direction and maginitute are independent of the rotation
+ * rate.
+ * @param rotation The rate of rotation for the robot that is completely
+ * independent of the magnitute or direction. [-1.0..1.0]
+ */
+void RobotDrive::MecanumDrive_Polar(double magnitude, double direction,
+ double rotation) {
+ static bool reported = false;
+ if (!reported) {
+ reported = true;
+ }
+
+ // Normalized for full power along the Cartesian axes.
+ magnitude = Limit(magnitude) * std::sqrt(2.0);
+ // The rollers are at 45 degree angles.
+ double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
+ double cosD = std::cos(dirInRad);
+ double sinD = std::sin(dirInRad);
+
+ double wheelSpeeds[kMaxNumberOfMotors];
+ wheelSpeeds[kFrontLeftMotor] = sinD * magnitude + rotation;
+ wheelSpeeds[kFrontRightMotor] = cosD * magnitude - rotation;
+ wheelSpeeds[kRearLeftMotor] = cosD * magnitude + rotation;
+ wheelSpeeds[kRearRightMotor] = sinD * magnitude - rotation;
+
+ Normalize(wheelSpeeds);
+
+ m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] *
+ m_invertedMotors[kFrontLeftMotor] * m_maxOutput);
+ m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] *
+ m_invertedMotors[kFrontRightMotor] * m_maxOutput);
+ m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] *
+ m_invertedMotors[kRearLeftMotor] * m_maxOutput);
+ m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] *
+ m_invertedMotors[kRearRightMotor] * m_maxOutput);
+
+ // FIXME: m_safetyHelper->Feed();
+}
+
+/**
+ * Holonomic Drive method for Mecanum wheeled robots.
+ *
+ * This is an alias to MecanumDrive_Polar() for backward compatability
+ *
+ * @param magnitude The speed that the robot should drive in a given direction.
+ * [-1.0..1.0]
+ * @param direction The direction the robot should drive. The direction and
+ * magnitude are independent of the rotation rate.
+ * @param rotation The rate of rotation for the robot that is completely
+ * independent of the magnitude or direction. [-1.0..1.0]
+ */
+void RobotDrive::HolonomicDrive(double magnitude, double direction,
+ double rotation) {
+ MecanumDrive_Polar(magnitude, direction, rotation);
+}
+
+/**
+ * Set the speed of the right and left motors.
+ *
+ * This is used once an appropriate drive setup function is called such as
+ * TwoWheelDrive(). The motors are set to "leftOutput" and "rightOutput"
+ * and includes flipping the direction of one side for opposing motors.
+ *
+ * @param leftOutput The speed to send to the left side of the robot.
+ * @param rightOutput The speed to send to the right side of the robot.
+ */
+void RobotDrive::SetLeftRightMotorOutputs(double leftOutput,
+ double rightOutput) {
+ wpi_assert(m_rearLeftMotor != nullptr && m_rearRightMotor != nullptr);
+
+ if (m_frontLeftMotor != nullptr)
+ m_frontLeftMotor->Set(Limit(leftOutput) *
+ m_invertedMotors[kFrontLeftMotor] * m_maxOutput);
+ m_rearLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] *
+ m_maxOutput);
+
+ if (m_frontRightMotor != nullptr)
+ m_frontRightMotor->Set(-Limit(rightOutput) *
+ m_invertedMotors[kFrontRightMotor] * m_maxOutput);
+ m_rearRightMotor->Set(-Limit(rightOutput) *
+ m_invertedMotors[kRearRightMotor] * m_maxOutput);
+
+ // FIXME: m_safetyHelper->Feed();
+}
+
+/**
+ * Limit motor values to the -1.0 to +1.0 range.
+ */
+double RobotDrive::Limit(double num) {
+ if (num > 1.0) {
+ return 1.0;
+ }
+ if (num < -1.0) {
+ return -1.0;
+ }
+ return num;
+}
+
+/**
+ * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
+ */
+void RobotDrive::Normalize(double* wheelSpeeds) {
+ double maxMagnitude = std::fabs(wheelSpeeds[0]);
+ int i;
+ for (i = 1; i < kMaxNumberOfMotors; i++) {
+ double temp = std::fabs(wheelSpeeds[i]);
+ if (maxMagnitude < temp) maxMagnitude = temp;
+ }
+ if (maxMagnitude > 1.0) {
+ for (i = 0; i < kMaxNumberOfMotors; i++) {
+ wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
+ }
+ }
+}
+
+/**
+ * Rotate a vector in Cartesian space.
+ */
+void RobotDrive::RotateVector(double& x, double& y, double angle) {
+ double cosA = std::cos(angle * (3.14159 / 180.0));
+ double sinA = std::sin(angle * (3.14159 / 180.0));
+ double xOut = x * cosA - y * sinA;
+ double yOut = x * sinA + y * cosA;
+ x = xOut;
+ y = yOut;
+}
+
+/**
+ * Invert a motor direction.
+ *
+ * This is used when a motor should run in the opposite direction as the drive
+ * code would normally run it. Motors that are direct drive would be inverted,
+ * the Drive code assumes that the motors are geared with one reversal.
+ *
+ * @param motor The motor index to invert.
+ * @param isInverted True if the motor should be inverted when operated.
+ */
+void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted) {
+ if (motor < 0 || motor > 3) {
+ wpi_setWPIError(InvalidMotorIndex);
+ return;
+ }
+ m_invertedMotors[motor] = isInverted ? -1 : 1;
+}
+
+/**
+ * Set the turning sensitivity.
+ *
+ * This only impacts the Drive() entry-point.
+ *
+ * @param sensitivity Effectively sets the turning sensitivity (or turn radius
+ * for a given value)
+ */
+void RobotDrive::SetSensitivity(double sensitivity) {
+ m_sensitivity = sensitivity;
+}
+
+/**
+ * Configure the scaling factor for using RobotDrive with motor controllers in a
+ * mode other than PercentVbus.
+ *
+ * @param maxOutput Multiplied with the output percentage computed by the drive
+ * functions.
+ */
+void RobotDrive::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; }
+
+void RobotDrive::SetExpiration(double timeout) {
+ // FIXME: m_safetyHelper->SetExpiration(timeout);
+}
+
+double RobotDrive::GetExpiration() const {
+ return -1; // FIXME: return m_safetyHelper->GetExpiration();
+}
+
+bool RobotDrive::IsAlive() const {
+ return true; // FIXME: m_safetyHelper->IsAlive();
+}
+
+bool RobotDrive::IsSafetyEnabled() const {
+ return false; // FIXME: return m_safetyHelper->IsSafetyEnabled();
+}
+
+void RobotDrive::SetSafetyEnabled(bool enabled) {
+ // FIXME: m_safetyHelper->SetSafetyEnabled(enabled);
+}
+
+void RobotDrive::GetDescription(std::ostringstream& desc) const {
+ desc << "RobotDrive";
+}
+
+void RobotDrive::StopMotor() {
+ if (m_frontLeftMotor != nullptr) m_frontLeftMotor->Disable();
+ if (m_frontRightMotor != nullptr) m_frontRightMotor->Disable();
+ if (m_rearLeftMotor != nullptr) m_rearLeftMotor->Disable();
+ if (m_rearRightMotor != nullptr) m_rearRightMotor->Disable();
+}
diff --git a/wpilibc/sim/src/SafePWM.cpp b/wpilibc/sim/src/SafePWM.cpp
new file mode 100644
index 0000000..4e42958
--- /dev/null
+++ b/wpilibc/sim/src/SafePWM.cpp
@@ -0,0 +1,94 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "SafePWM.h"
+
+#include <memory>
+#include <sstream>
+
+using namespace frc;
+
+/**
+ * Constructor for a SafePWM object taking a channel number.
+ *
+ * @param channel The PWM channel number (0..19).
+ */
+SafePWM::SafePWM(int channel) : PWM(channel) {
+ m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
+ m_safetyHelper->SetSafetyEnabled(false);
+}
+
+/**
+ * Set the expiration time for the PWM object.
+ *
+ * @param timeout The timeout (in seconds) for this motor object
+ */
+void SafePWM::SetExpiration(double timeout) {
+ m_safetyHelper->SetExpiration(timeout);
+}
+
+/**
+ * Return the expiration time for the PWM object.
+ *
+ * @returns The expiration time value.
+ */
+double SafePWM::GetExpiration() const {
+ return m_safetyHelper->GetExpiration();
+}
+
+/**
+ * Check if the PWM object is currently alive or stopped due to a timeout.
+ *
+ * @return a bool value that is true if the motor has NOT timed out and should
+ * still be running.
+ */
+bool SafePWM::IsAlive() const { return m_safetyHelper->IsAlive(); }
+
+/**
+ * Stop the motor associated with this PWM object.
+ *
+ * This is called by the MotorSafetyHelper object when it has a timeout for this
+ * PWM and needs to stop it from running.
+ */
+void SafePWM::StopMotor() { SetRaw(kPwmDisabled); }
+
+/**
+ * Enable/disable motor safety for this device.
+ *
+ * Turn on and off the motor safety option for this PWM object.
+ *
+ * @param enabled True if motor safety is enforced for this object
+ */
+void SafePWM::SetSafetyEnabled(bool enabled) {
+ m_safetyHelper->SetSafetyEnabled(enabled);
+}
+
+/**
+ * Check if motor safety is enabled for this object.
+ *
+ * @returns True if motor safety is enforced for this object
+ */
+bool SafePWM::IsSafetyEnabled() const {
+ return m_safetyHelper->IsSafetyEnabled();
+}
+
+void SafePWM::GetDescription(std::ostringstream& desc) const {
+ desc << "PWM " << GetChannel();
+}
+
+/**
+ * Feed the MotorSafety timer when setting the speed.
+ *
+ * This method is called by the subclass motor whenever it updates its speed,
+ * thereby reseting the timeout value.
+ *
+ * @param speed Value to pass to the PWM class
+ */
+void SafePWM::SetSpeed(double speed) {
+ PWM::SetSpeed(speed);
+ m_safetyHelper->Feed();
+}
diff --git a/wpilibc/sim/src/SampleRobot.cpp b/wpilibc/sim/src/SampleRobot.cpp
new file mode 100644
index 0000000..7488d24
--- /dev/null
+++ b/wpilibc/sim/src/SampleRobot.cpp
@@ -0,0 +1,145 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "SampleRobot.h"
+
+#include <cstdio>
+
+#include "LiveWindow/LiveWindow.h"
+#include "Timer.h"
+#include "networktables/NetworkTable.h"
+
+#if defined(_UNIX)
+#include <unistd.h>
+#elif defined(_WIN32)
+#include <windows.h>
+void sleep(unsigned int milliseconds) { Sleep(milliseconds); }
+#endif
+
+using namespace frc;
+
+SampleRobot::SampleRobot() : m_robotMainOverridden(true) {}
+
+/**
+ * Robot-wide initialization code should go here.
+ *
+ * Programmers should override this method for default Robot-wide initialization
+ * which will be called each time the robot enters the disabled state.
+ */
+void SampleRobot::RobotInit() {
+ std::printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Disabled should go here.
+ *
+ * Programmers should override this method to run code that should run while the
+ * field is disabled.
+ */
+void SampleRobot::Disabled() {
+ std::printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Autonomous should go here.
+ *
+ * Programmers should override this method to run code that should run while the
+ * field is in the autonomous period. This will be called once each time the
+ * robot enters the autonomous state.
+ */
+void SampleRobot::Autonomous() {
+ std::printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Operator control (tele-operated) code should go here.
+ *
+ * Programmers should override this method to run code that should run while the
+ * field is in the Operator Control (tele-operated) period. This is called once
+ * each time the robot enters the teleop state.
+ */
+void SampleRobot::OperatorControl() {
+ std::printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Test program should go here.
+ *
+ * Programmers should override this method to run code that executes while the
+ * robot is in test mode. This will be called once whenever the robot enters
+ * test mode.
+ */
+void SampleRobot::Test() {
+ std::printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Robot main program for free-form programs.
+ *
+ * This should be overridden by user subclasses if the intent is to not use the
+ * Autonomous() and OperatorControl() methods. In that case, the program is
+ * responsible for sensing when to run the autonomous and operator control
+ * functions in their program.
+ *
+ * This method will be called immediately after the constructor is called. If it
+ * has not been overridden by a user subclass (i.e. the default version runs),
+ * then the Autonomous() and OperatorControl() methods will be called.
+ */
+void SampleRobot::RobotMain() { m_robotMainOverridden = false; }
+
+/**
+ * Start a competition.
+ *
+ * This code needs to track the order of the field starting to ensure that
+ * everything happens in the right order. Repeatedly run the correct method,
+ * either Autonomous or OperatorControl or Test when the robot is enabled.
+ * After running the correct method, wait for some state to change, either the
+ * other mode starts or the robot is disabled. Then go back and wait for the
+ * robot to be enabled again.
+ */
+void SampleRobot::StartCompetition() {
+ LiveWindow* lw = LiveWindow::GetInstance();
+
+ NetworkTable::GetTable("LiveWindow")
+ ->GetSubTable("~STATUS~")
+ ->PutBoolean("LW Enabled", false);
+
+ RobotMain();
+
+ if (!m_robotMainOverridden) {
+ // first and one-time initialization
+ lw->SetEnabled(false);
+ RobotInit();
+
+ while (true) {
+ if (IsDisabled()) {
+ m_ds.InDisabled(true);
+ Disabled();
+ m_ds.InDisabled(false);
+ while (IsDisabled()) sleep(1); // m_ds.WaitForData();
+ } else if (IsAutonomous()) {
+ m_ds.InAutonomous(true);
+ Autonomous();
+ m_ds.InAutonomous(false);
+ while (IsAutonomous() && IsEnabled()) sleep(1); // m_ds.WaitForData();
+ } else if (IsTest()) {
+ lw->SetEnabled(true);
+ m_ds.InTest(true);
+ Test();
+ m_ds.InTest(false);
+ while (IsTest() && IsEnabled()) sleep(1); // m_ds.WaitForData();
+ lw->SetEnabled(false);
+ } else {
+ m_ds.InOperatorControl(true);
+ OperatorControl();
+ m_ds.InOperatorControl(false);
+ while (IsOperatorControl() && IsEnabled())
+ sleep(1); // m_ds.WaitForData();
+ }
+ }
+ }
+}
diff --git a/wpilibc/sim/src/SensorBase.cpp b/wpilibc/sim/src/SensorBase.cpp
new file mode 100644
index 0000000..cd708ad
--- /dev/null
+++ b/wpilibc/sim/src/SensorBase.cpp
@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "SensorBase.h"
+
+#include "WPIErrors.h"
+
+using namespace frc;
+
+const int SensorBase::kDigitalChannels;
+const int SensorBase::kAnalogInputs;
+const int SensorBase::kSolenoidChannels;
+const int SensorBase::kSolenoidModules;
+const int SensorBase::kPwmChannels;
+const int SensorBase::kRelayChannels;
+const int SensorBase::kPDPChannels;
+
+/**
+ * Check that the solenoid module number is valid.
+ *
+ * @return Solenoid module number is valid
+ */
+bool SensorBase::CheckSolenoidModule(int moduleNumber) {
+ return moduleNumber >= 0 && moduleNumber < kSolenoidModules;
+}
+
+/**
+ * Check that the digital channel number is valid.
+ *
+ * Verify that the channel number is one of the legal channel numbers. Channel
+ * numbers are 0-based.
+ *
+ * @return Digital channel is valid
+ */
+bool SensorBase::CheckDigitalChannel(int 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 0-based.
+ *
+ * @return Relay channel is valid
+ */
+bool SensorBase::CheckRelayChannel(int 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 0-based.
+ *
+ * @return PWM channel is valid
+ */
+bool SensorBase::CheckPWMChannel(int 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 0-based.
+ *
+ * @return Analog channel is valid
+ */
+bool SensorBase::CheckAnalogInputChannel(int 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 0-based.
+ *
+ * @return Analog channel is valid
+ */
+bool SensorBase::CheckAnalogOutputChannel(int 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(int channel) {
+ if (channel >= 0 && channel < kSolenoidChannels) return true;
+ return false;
+}
+
+/**
+ * Verify that the power distribution channel number is within limits.
+ *
+ * @return PDP channel is valid
+ */
+bool SensorBase::CheckPDPChannel(int channel) {
+ if (channel >= 0 && channel < kPDPChannels) return true;
+ return false;
+}
diff --git a/wpilibc/sim/src/Solenoid.cpp b/wpilibc/sim/src/Solenoid.cpp
new file mode 100644
index 0000000..ef19ffb
--- /dev/null
+++ b/wpilibc/sim/src/Solenoid.cpp
@@ -0,0 +1,94 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Solenoid.h"
+
+#include <sstream>
+
+#include "LiveWindow/LiveWindow.h"
+#include "WPIErrors.h"
+#include "simulation/simTime.h"
+
+using namespace frc;
+
+/**
+ * Constructor.
+ *
+ * @param channel The channel on the solenoid module to control (1..8).
+ */
+Solenoid::Solenoid(int channel) : Solenoid(1, channel) {}
+
+/**
+ * Constructor.
+ *
+ * @param moduleNumber The solenoid module (1 or 2).
+ * @param channel The channel on the solenoid module to control (1..8).
+ */
+Solenoid::Solenoid(int moduleNumber, int channel) {
+ std::stringstream ss;
+ ss << "pneumatic/" << moduleNumber << "/" << channel;
+ m_impl = new SimContinuousOutput(ss.str());
+
+ LiveWindow::GetInstance()->AddActuator("Solenoid", moduleNumber, channel,
+ this);
+}
+
+Solenoid::~Solenoid() {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Set the value of a solenoid.
+ *
+ * @param on Turn the solenoid output off or on.
+ */
+void Solenoid::Set(bool on) {
+ m_on = on;
+ m_impl->Set(on ? 1 : -1);
+}
+
+/**
+ * Read the current value of the solenoid.
+ *
+ * @return The current value of the solenoid.
+ */
+bool Solenoid::Get() const { return m_on; }
+
+void Solenoid::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (!value->IsBoolean()) return;
+ Set(value->GetBoolean());
+}
+
+void Solenoid::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutBoolean("Value", Get());
+ }
+}
+
+void Solenoid::StartLiveWindowMode() {
+ Set(false);
+ if (m_table != nullptr) {
+ m_table->AddTableListener("Value", this, true);
+ }
+}
+
+void Solenoid::StopLiveWindowMode() {
+ Set(false);
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string Solenoid::GetSmartDashboardType() const { return "Solenoid"; }
+
+void Solenoid::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> Solenoid::GetTable() const { return m_table; }
diff --git a/wpilibc/sim/src/Talon.cpp b/wpilibc/sim/src/Talon.cpp
new file mode 100644
index 0000000..652b2ae
--- /dev/null
+++ b/wpilibc/sim/src/Talon.cpp
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Talon.h"
+
+#include "LiveWindow/LiveWindow.h"
+
+using namespace frc;
+
+/**
+ * @param channel The PWM channel that the Talon is attached to.
+ */
+Talon::Talon(int channel) : SafePWM(channel) {
+ /* Note that the Talon uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the Talon User Manual available
+ * from CTRE.
+ *
+ * - 211 = full "forward"
+ * - 133 = the "high end" of the deadband range
+ * - 129 = center of the deadband range (off)
+ * - 125 = the "low end" of the deadband range
+ * - 49 = full "reverse"
+ */
+ SetBounds(2.037, 1.539, 1.513, 1.487, .989);
+ SetPeriodMultiplier(kPeriodMultiplier_2X);
+ SetRaw(m_centerPwm);
+
+ LiveWindow::GetInstance()->AddActuator("Talon", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ */
+void Talon::Set(double speed) { 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.
+ */
+double Talon::Get() const { return GetSpeed(); }
+
+/**
+ * Common interface for disabling a motor.
+ */
+void Talon::Disable() { SetRaw(kPwmDisabled); }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void Talon::PIDWrite(double output) { Set(output); }
diff --git a/wpilibc/sim/src/Timer.cpp b/wpilibc/sim/src/Timer.cpp
new file mode 100644
index 0000000..93e33e2
--- /dev/null
+++ b/wpilibc/sim/src/Timer.cpp
@@ -0,0 +1,192 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Timer.h"
+
+#include "Utility.h"
+#include "simulation/simTime.h"
+
+// Internal stuff
+#include "simulation/MainNode.h"
+#include "simulation/SimFloatInput.h"
+namespace wpilib {
+namespace internal {
+double simTime = 0;
+std::condition_variable time_wait;
+std::mutex time_wait_mutex;
+
+void time_callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+ simTime = msg->data();
+ time_wait.notify_all();
+}
+}
+} // namespace wpilib
+
+namespace frc {
+
+/**
+ * Pause the task for a specified time.
+ *
+ * Pause the execution of the program for a specified period of time given in
+ * seconds. Motors will continue to run at their last assigned values, and
+ * sensors will continue to update. Only the task containing the wait will
+ * pause until the wait time is expired.
+ *
+ * @param seconds Length of time to pause, in seconds.
+ */
+void Wait(double seconds) {
+ if (seconds < 0.0) return;
+
+ double start = wpilib::internal::simTime;
+
+ std::unique_lock<std::mutex> lock(wpilib::internal::time_wait_mutex);
+ while ((wpilib::internal::simTime - start) < seconds) {
+ wpilib::internal::time_wait.wait(lock);
+ }
+}
+
+/*
+ * Return the FPGA system clock time in seconds.
+ *
+ * This is deprecated and just forwards to Timer::GetFPGATimestamp().
+ *
+ * @returns Robot running time in seconds.
+ */
+double GetClock() { return Timer::GetFPGATimestamp(); }
+
+/**
+ * @brief Gives real-time clock system time with nanosecond resolution
+ * @return The time, just in case you want the robot to start autonomous at 8pm
+ * on Saturday (except in simulation).
+ */
+double GetTime() {
+ return Timer::GetFPGATimestamp(); // The epoch starts when Gazebo starts
+}
+
+} // namespace frc
+
+using namespace frc;
+
+// for compatibility with msvc12--see C2864
+const double Timer::kRolloverTime = (1ll << 32) / 1e6;
+/**
+ * Create a new timer object.
+ *
+ * Create a new timer object and reset the time to zero. The timer is initially
+ * not running and must be started.
+ */
+Timer::Timer() : m_startTime(0.0), m_accumulatedTime(0.0), m_running(false) {
+ // Creates a semaphore to control access to critical regions.
+ // Initially 'open'
+ Reset();
+}
+
+/**
+ * Get the current time from the timer.
+ *
+ * If the clock is running it is derived from the current system clock the
+ * start time stored in the timer class. If the clock is not running, then
+ * return the time when it was last stopped.
+ *
+ * @return Current time value for this timer in seconds
+ */
+double Timer::Get() const {
+ double result;
+ double currentTime = GetFPGATimestamp();
+
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ if (m_running) {
+ // This math won't work if the timer rolled over (71 minutes after boot).
+ // TODO: Check for it and compensate.
+ result = (currentTime - m_startTime) + m_accumulatedTime;
+ } else {
+ result = m_accumulatedTime;
+ }
+
+ return result;
+}
+
+/**
+ * Reset the timer by setting the time to 0.
+ *
+ * Make the timer startTime the current time so new requests will be relative to
+ * now.
+ */
+void Timer::Reset() {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_accumulatedTime = 0;
+ m_startTime = GetFPGATimestamp();
+}
+
+/**
+ * Start the timer running.
+ *
+ * Just set the running flag to true indicating that all time requests should be
+ * relative to the system clock.
+ */
+void Timer::Start() {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ if (!m_running) {
+ m_startTime = GetFPGATimestamp();
+ m_running = true;
+ }
+}
+
+/**
+ * Stop the timer.
+ *
+ * This computes the time as of now and clears the running flag, causing all
+ * subsequent time requests to be read from the accumulated time rather than
+ * looking at the system clock.
+ */
+void Timer::Stop() {
+ double temp = Get();
+
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ if (m_running) {
+ m_accumulatedTime = temp;
+ m_running = false;
+ }
+}
+
+/**
+ * Check if the period specified has passed and if it has, advance the start
+ * time by that period. This is useful to decide if it's time to do periodic
+ * work without drifting later by the time it took to get around to checking.
+ *
+ * @param period The period to check for (in seconds).
+ * @return If the period has passed.
+ */
+bool Timer::HasPeriodPassed(double period) {
+ if (Get() > period) {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ // Advance the start time by the period.
+ // Don't set it to the current time... we want to avoid drift.
+ m_startTime += period;
+ return true;
+ }
+ return false;
+}
+
+/*
+ * Return the FPGA system clock time in seconds.
+ *
+ * Return the time from the FPGA hardware clock in seconds since the FPGA
+ * started. Rolls over after 71 minutes.
+ *
+ * @return Robot running time in seconds.
+ */
+double Timer::GetFPGATimestamp() {
+ // FPGA returns the timestamp in microseconds
+ // Call the helper GetFPGATime() in Utility.cpp
+ return wpilib::internal::simTime;
+}
+
+/*
+ * Not in a match.
+ */
+double Timer::GetMatchTime() { return Timer::GetFPGATimestamp(); }
diff --git a/wpilibc/sim/src/Utility.cpp b/wpilibc/sim/src/Utility.cpp
new file mode 100644
index 0000000..97c02de
--- /dev/null
+++ b/wpilibc/sim/src/Utility.cpp
@@ -0,0 +1,216 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Utility.h"
+
+#ifndef _WIN32
+#include <cxxabi.h>
+#include <execinfo.h>
+#endif
+
+#include <cstdio>
+#include <cstdlib>
+#include <iostream>
+#include <sstream>
+
+#include "Timer.h"
+#include "llvm/SmallString.h"
+#include "simulation/simTime.h"
+
+using namespace frc;
+
+static bool stackTraceEnabled = false;
+static bool suspendOnAssertEnabled = false;
+
+/**
+ * Enable Stack trace after asserts.
+ */
+void wpi_stackTraceOnAssertEnable(bool enabled) { stackTraceEnabled = enabled; }
+
+/**
+ * Enable suspend on asssert.
+ *
+ * If enabled, the user task will be suspended whenever an assert fails. This
+ * will allow the user to attach to the task with the debugger and examine
+ * variables around the failure.
+ */
+void wpi_suspendOnAssertEnabled(bool enabled) {
+ suspendOnAssertEnabled = enabled;
+}
+
+static void wpi_handleTracing() {
+ // if (stackTraceEnabled)
+ // {
+ // std::printf("\n-----------<Stack Trace>----------------\n");
+ // printCurrentStackTrace();
+ // }
+ std::printf("\n");
+}
+
+/**
+ * Assert implementation.
+ * This allows breakpoints to be set on an assert.
+ * The users don't call this, but instead use the wpi_assert macros in
+ * Utility.h.
+ */
+bool wpi_assert_impl(bool conditionValue, llvm::StringRef conditionText,
+ llvm::StringRef message, llvm::StringRef fileName,
+ int lineNumber, llvm::StringRef funcName) {
+ if (!conditionValue) {
+ std::stringstream errorStream;
+
+ errorStream << "Assertion \"" << conditionText << "\" ";
+ errorStream << "on line " << lineNumber << " ";
+
+ llvm::SmallString<128> fileTemp;
+ errorStream << "of " << basename(fileName.c_str(fileTemp)) << " ";
+
+ if (message[0] != '\0') {
+ errorStream << "failed: " << message << std::endl;
+ } else {
+ errorStream << "failed." << std::endl;
+ }
+
+ // Print to console and send to remote dashboard
+ std::cout << "\n\n>>>>" << errorStream.str();
+ wpi_handleTracing();
+ }
+
+ return conditionValue;
+}
+
+/**
+ * Common error routines for wpi_assertEqual_impl and wpi_assertNotEqual_impl
+ * This should not be called directly; it should only be used by
+ * wpi_assertEqual_impl and wpi_assertNotEqual_impl.
+ */
+void wpi_assertEqual_common_impl(int valueA, int valueB,
+ llvm::StringRef equalityType,
+ llvm::StringRef message,
+ llvm::StringRef fileName, int lineNumber,
+ llvm::StringRef funcName) {
+ // Error string buffer
+ std::stringstream error;
+
+ // If an error message was specified, include it
+ // Build error string
+ if (message.size() > 0) {
+ error << "Assertion failed: \"" << message << "\", \"" << valueA << "\" "
+ << equalityType << " \"" << valueB << "\" in " << funcName << "() in "
+ << fileName << " at line " << lineNumber << "\n";
+ } else {
+ error << "Assertion failed: \"" << valueA << "\" " << equalityType << " \""
+ << valueB << "\" in " << funcName << "() in " << fileName
+ << " at line " << lineNumber << "\n";
+ }
+
+ // Print to console and send to remote dashboard
+ std::cout << "\n\n>>>>" << error.str();
+
+ wpi_handleTracing();
+}
+
+/**
+ * Assert equal implementation.
+ * This determines whether the two given integers are equal. If not,
+ * the value of each is printed along with an optional message string.
+ * The users don't call this, but instead use the wpi_assertEqual macros in
+ * Utility.h.
+ */
+bool wpi_assertEqual_impl(int valueA, int valueB, llvm::StringRef message,
+ llvm::StringRef fileName, int lineNumber,
+ llvm::StringRef funcName) {
+ if (!(valueA == valueB)) {
+ wpi_assertEqual_common_impl(valueA, valueB, "!=", message, fileName,
+ lineNumber, funcName);
+ }
+ return valueA == valueB;
+}
+
+/**
+ * Assert not equal implementation.
+ * This determines whether the two given integers are equal. If so,
+ * the value of each is printed along with an optional message string.
+ * The users don't call this, but instead use the wpi_assertNotEqual macros in
+ * Utility.h.
+ */
+bool wpi_assertNotEqual_impl(int valueA, int valueB, llvm::StringRef message,
+ llvm::StringRef fileName, int lineNumber,
+ llvm::StringRef funcName) {
+ if (!(valueA != valueB)) {
+ wpi_assertEqual_common_impl(valueA, valueB, "==", message, fileName,
+ lineNumber, funcName);
+ }
+ return valueA != valueB;
+}
+
+namespace frc {
+
+/**
+ * Read the microsecond-resolution timer on the FPGA.
+ *
+ * @return The current time in microseconds according to the FPGA (since FPGA
+ * reset).
+ */
+uint64_t GetFPGATime() { return wpilib::internal::simTime * 1e6; }
+
+// TODO: implement symbol demangling and backtrace on windows
+#if not defined(_WIN32)
+
+/**
+ * Demangle a C++ symbol, used for printing stack traces.
+ */
+static std::string demangle(char const* mangledSymbol) {
+ char buffer[256];
+ size_t length;
+ int32_t status;
+
+ if (std::sscanf(mangledSymbol, "%*[^(]%*[^_]%255[^)+]", buffer)) {
+ char* symbol = abi::__cxa_demangle(buffer, nullptr, &length, &status);
+
+ if (status == 0) {
+ return symbol;
+ } else {
+ // If the symbol couldn't be demangled, it's probably a C function,
+ // so just return it as-is.
+ return buffer;
+ }
+ }
+
+ // If everything else failed, just return the mangled symbol
+ return mangledSymbol;
+}
+
+/**
+ * Get a stack trace, ignoring the first "offset" symbols.
+ */
+std::string GetStackTrace(int 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;
+ }
+ }
+
+ std::free(mangledSymbols);
+
+ return trace.str();
+}
+
+#else
+static std::string demangle(char const* mangledSymbol) {
+ return "no demangling on windows";
+}
+std::string GetStackTrace(int offset) { return "no stack trace on windows"; }
+#endif
+
+} // namespace frc
diff --git a/wpilibc/sim/src/Victor.cpp b/wpilibc/sim/src/Victor.cpp
new file mode 100644
index 0000000..ec61719
--- /dev/null
+++ b/wpilibc/sim/src/Victor.cpp
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Victor.h"
+
+#include "LiveWindow/LiveWindow.h"
+
+using namespace frc;
+
+/**
+ * @param channel The PWM channel that the Victor is attached to.
+ */
+Victor::Victor(int channel) : SafePWM(channel) {
+ /* Note that the Victor uses the following bounds for PWM values. These values
+ * were determined empirically and optimized for the Victor 888. These values
+ * should work reasonably well for Victor 884 controllers as well but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Victor 884 User
+ * Manual available from IFI.
+ *
+ * - 206 = full "forward"
+ * - 131 = the "high end" of the deadband range
+ * - 128 = center of the deadband range (off)
+ * - 125 = the "low end" of the deadband range
+ * - 56 = full "reverse"
+ */
+
+ SetBounds(2.027, 1.525, 1.507, 1.49, 1.026);
+ SetPeriodMultiplier(kPeriodMultiplier_2X);
+ SetRaw(m_centerPwm);
+
+ LiveWindow::GetInstance()->AddActuator("Victor", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ */
+void Victor::Set(double speed) { 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.
+ */
+double Victor::Get() const { return GetSpeed(); }
+
+/**
+ * Common interface for disabling a motor.
+ */
+void Victor::Disable() { SetRaw(kPwmDisabled); }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void Victor::PIDWrite(double output) { Set(output); }
diff --git a/wpilibc/sim/src/XboxController.cpp b/wpilibc/sim/src/XboxController.cpp
new file mode 100644
index 0000000..441ecf0
--- /dev/null
+++ b/wpilibc/sim/src/XboxController.cpp
@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "XboxController.h"
+
+#include "DriverStation.h"
+
+using namespace frc;
+
+/**
+ * Construct an instance of an Xbox controller.
+ *
+ * The joystick index is the USB port on the Driver Station.
+ *
+ * @param port The port on the Driver Station that the joystick is plugged into
+ * (0-5).
+ */
+XboxController::XboxController(int port)
+ : GamepadBase(port), m_ds(DriverStation::GetInstance()) {}
+
+/**
+ * Get the X axis value of the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ */
+double XboxController::GetX(JoystickHand hand) const {
+ if (hand == kLeftHand) {
+ return GetRawAxis(0);
+ } else {
+ return GetRawAxis(4);
+ }
+}
+
+/**
+ * Get the Y axis value of the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ */
+double XboxController::GetY(JoystickHand hand) const {
+ if (hand == kLeftHand) {
+ return GetRawAxis(1);
+ } else {
+ return GetRawAxis(5);
+ }
+}
+
+/**
+ * Read the value of the bumper button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ */
+bool XboxController::GetBumper(JoystickHand hand) const {
+ if (hand == kLeftHand) {
+ return GetRawButton(5);
+ } else {
+ return GetRawButton(6);
+ }
+}
+
+/**
+ * Read the value of the stick button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+bool XboxController::GetStickButton(JoystickHand hand) const {
+ if (hand == kLeftHand) {
+ return GetRawButton(9);
+ } else {
+ return GetRawButton(10);
+ }
+}
+
+/**
+ * Get the trigger axis value of the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ */
+double XboxController::GetTriggerAxis(JoystickHand hand) const {
+ if (hand == kLeftHand) {
+ return GetRawAxis(2);
+ } else {
+ return GetRawAxis(3);
+ }
+}
+
+/**
+ * Read the value of the A button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+bool XboxController::GetAButton() const { return GetRawButton(1); }
+
+/**
+ * Read the value of the B button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+bool XboxController::GetBButton() const { return GetRawButton(2); }
+
+/**
+ * Read the value of the X button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+bool XboxController::GetXButton() const { return GetRawButton(3); }
+
+/**
+ * Read the value of the Y button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+bool XboxController::GetYButton() const { return GetRawButton(4); }
+
+/**
+ * Read the value of the back button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+bool XboxController::GetBackButton() const { return GetRawButton(7); }
+
+/**
+ * Read the value of the start button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+bool XboxController::GetStartButton() const { return GetRawButton(8); }
diff --git a/wpilibc/sim/src/simulation/SimContinuousOutput.cpp b/wpilibc/sim/src/simulation/SimContinuousOutput.cpp
new file mode 100644
index 0000000..9a907cf
--- /dev/null
+++ b/wpilibc/sim/src/simulation/SimContinuousOutput.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "simulation/SimContinuousOutput.h"
+
+#include "simulation/MainNode.h"
+
+using namespace frc;
+
+SimContinuousOutput::SimContinuousOutput(std::string topic) {
+ pub = MainNode::Advertise<gazebo::msgs::Float64>("~/simulator/" + topic);
+ std::cout << "Initialized ~/simulator/" + topic << std::endl;
+}
+
+void SimContinuousOutput::Set(double speed) {
+ gazebo::msgs::Float64 msg;
+ msg.set_data(speed);
+ pub->Publish(msg);
+}
+
+double SimContinuousOutput::Get() { return speed; }
diff --git a/wpilibc/sim/src/simulation/SimDigitalInput.cpp b/wpilibc/sim/src/simulation/SimDigitalInput.cpp
new file mode 100644
index 0000000..c4dbde6
--- /dev/null
+++ b/wpilibc/sim/src/simulation/SimDigitalInput.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "simulation/SimDigitalInput.h"
+
+#include "simulation/MainNode.h"
+
+using namespace frc;
+
+SimDigitalInput::SimDigitalInput(std::string topic) {
+ sub = MainNode::Subscribe("~/simulator/" + topic, &SimDigitalInput::callback,
+ this);
+ std::cout << "Initialized ~/simulator/" + topic << std::endl;
+}
+
+bool SimDigitalInput::Get() { return value; }
+
+void SimDigitalInput::callback(const gazebo::msgs::ConstBoolPtr& msg) {
+ value = msg->data();
+}
diff --git a/wpilibc/sim/src/simulation/SimEncoder.cpp b/wpilibc/sim/src/simulation/SimEncoder.cpp
new file mode 100644
index 0000000..141a371
--- /dev/null
+++ b/wpilibc/sim/src/simulation/SimEncoder.cpp
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "simulation/SimEncoder.h"
+
+#include "simulation/MainNode.h"
+
+using namespace frc;
+
+SimEncoder::SimEncoder(std::string topic) {
+ commandPub = MainNode::Advertise<gazebo::msgs::GzString>("~/simulator/" +
+ topic + "/control");
+
+ posSub = MainNode::Subscribe("~/simulator/" + topic + "/position",
+ &SimEncoder::positionCallback, this);
+ velSub = MainNode::Subscribe("~/simulator/" + topic + "/velocity",
+ &SimEncoder::velocityCallback, this);
+
+ if (commandPub->WaitForConnection(
+ gazebo::common::Time(5.0))) { // Wait up to five seconds.
+ std::cout << "Initialized ~/simulator/" + topic << std::endl;
+ } else {
+ std::cerr << "Failed to initialize ~/simulator/" + topic +
+ ": does the encoder exist?"
+ << std::endl;
+ }
+}
+
+void SimEncoder::Reset() { sendCommand("reset"); }
+
+void SimEncoder::Start() { sendCommand("start"); }
+
+void SimEncoder::Stop() { sendCommand("stop"); }
+
+double SimEncoder::GetPosition() { return position; }
+
+double SimEncoder::GetVelocity() { return velocity; }
+
+void SimEncoder::sendCommand(std::string cmd) {
+ gazebo::msgs::GzString msg;
+ msg.set_data(cmd);
+ commandPub->Publish(msg);
+}
+
+void SimEncoder::positionCallback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+ position = msg->data();
+}
+
+void SimEncoder::velocityCallback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+ velocity = msg->data();
+}
diff --git a/wpilibc/sim/src/simulation/SimFloatInput.cpp b/wpilibc/sim/src/simulation/SimFloatInput.cpp
new file mode 100644
index 0000000..8bf8ae3
--- /dev/null
+++ b/wpilibc/sim/src/simulation/SimFloatInput.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "simulation/SimFloatInput.h"
+
+#include "simulation/MainNode.h"
+
+using namespace frc;
+
+SimFloatInput::SimFloatInput(std::string topic) {
+ sub = MainNode::Subscribe("~/simulator/" + topic, &SimFloatInput::callback,
+ this);
+ std::cout << "Initialized ~/simulator/" + topic << std::endl;
+}
+
+double SimFloatInput::Get() { return value; }
+
+void SimFloatInput::callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+ value = msg->data();
+}
diff --git a/wpilibc/sim/src/simulation/SimGyro.cpp b/wpilibc/sim/src/simulation/SimGyro.cpp
new file mode 100644
index 0000000..33663bd
--- /dev/null
+++ b/wpilibc/sim/src/simulation/SimGyro.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "simulation/SimGyro.h"
+
+#include "simulation/MainNode.h"
+
+using namespace frc;
+
+SimGyro::SimGyro(std::string topic) {
+ commandPub = MainNode::Advertise<gazebo::msgs::GzString>("~/simulator/" +
+ topic + "/control");
+
+ posSub = MainNode::Subscribe("~/simulator/" + topic + "/position",
+ &SimGyro::positionCallback, this);
+ velSub = MainNode::Subscribe("~/simulator/" + topic + "/velocity",
+ &SimGyro::velocityCallback, this);
+
+ if (commandPub->WaitForConnection(
+ gazebo::common::Time(5.0))) { // Wait up to five seconds.
+ std::cout << "Initialized ~/simulator/" + topic << std::endl;
+ } else {
+ std::cerr << "Failed to initialize ~/simulator/" + topic +
+ ": does the gyro exist?"
+ << std::endl;
+ }
+}
+
+void SimGyro::Reset() { sendCommand("reset"); }
+
+double SimGyro::GetAngle() { return position; }
+
+double SimGyro::GetVelocity() { return velocity; }
+
+void SimGyro::sendCommand(std::string cmd) {
+ gazebo::msgs::GzString msg;
+ msg.set_data(cmd);
+ commandPub->Publish(msg);
+}
+
+void SimGyro::positionCallback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+ position = msg->data();
+}
+
+void SimGyro::velocityCallback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+ velocity = msg->data();
+}