Squashed 'third_party/allwpilib_2016/' content from commit 7f61816
Change-Id: If9d9245880859cdf580f5d7f77045135d0521ce7
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 7f618166ed253a24629934fcf89c3decb0528a3b
diff --git a/wpilibc/simulation/.gitignore b/wpilibc/simulation/.gitignore
new file mode 100644
index 0000000..e83089b
--- /dev/null
+++ b/wpilibc/simulation/.gitignore
@@ -0,0 +1,3 @@
+#don't track the generate protobuf files
+include/simulation/msgs/
+src/simulation/msgs
diff --git a/wpilibc/simulation/CMakeLists.txt b/wpilibc/simulation/CMakeLists.txt
new file mode 100644
index 0000000..1241fd3
--- /dev/null
+++ b/wpilibc/simulation/CMakeLists.txt
@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 2.8)
+project(WPILibSim)
+
+if (WIN32)
+ #temporary until we build dlls
+ add_definitions(-DBUILDING_STATIC_LIBS)
+
+ # XXX: should be set via CMake variables in configure.bat
+ set(PTHREAD_INCLUDE_DIR "C:/Users/peter/gz-ws/pthread-w32/include")
+ set(PTHREAD_LIBRARY "C:/Users/peter/gz-ws/pthread-w32/libs/x64/pthreadVC2.lib")
+endif()
+
+get_filename_component(HAL_API_INCLUDES ../../hal/include REALPATH)
+get_filename_component(NWT_API_INCLUDES ../../networktables/cpp/include REALPATH)
+
+
+# also on windows use sprintf_s instead of snprintf
+# TODO: find a more permenenant solution
+if (WIN32)
+ add_definitions(-Dsnprintf=sprintf_s)
+endif()
+
+file(GLOB_RECURSE COM_SRC_FILES ../wpilibC++/src/*.cpp)
+
+
+set (INCLUDE_FOLDERS include
+ ../wpilibC++/include
+ ../../networktables/ntcore/include
+ ../../hal/include
+ ${GZ_MSGS_INCLUDE_DIR}
+ ${Boost_INCLUDE_DIR}
+ ${GAZEBO_INCLUDE_DIRS})
+
+if (WIN32)
+ #these paths will be fixed when a more permenant windows development solution is found
+ set(INCLUDE_FOLDERS ${INCLUDE_FOLDERS}
+ C:/Users/peter/gz-ws/protobuf-2.6.0-win64-vc12/src
+ C:/Users/peter/gz-ws/sdformat/src/win/tinyxml
+ C:/Users/peter/gz-ws/FreeImage-vc12-x64-release-debug/Source
+ C:/Users/peter/gz-ws/tbb43_20141023oss/include
+ ${PTHREAD_INCLUDE_DIR})
+endif()
+
+include_directories(${INCLUDE_FOLDERS})
+
+link_directories(${GAZEBO_LIBRARY_DIRS})
+
+if (WIN32)
+ add_library(WPILibSim ${SRC_FILES} ${COM_SRC_FILES})
+else()
+ add_library(WPILibSim SHARED ${SRC_FILES} ${COM_SRC_FILES})
+endif()
+
+target_link_libraries(WPILibSim gz_msgs ntcore ${PTHREAD_LIBRARY} ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} -fPIC) # NetworkTables
+
+if (WIN32)
+ set_target_properties(${project} PROPERTIES LINK_FLAGS "/DEBUG")
+endif()
+
+#copy to eclipse plugin
diff --git a/wpilibc/simulation/README.md b/wpilibc/simulation/README.md
new file mode 100644
index 0000000..7abfeda
--- /dev/null
+++ b/wpilibc/simulation/README.md
@@ -0,0 +1,2 @@
+# Building WPILib C++ Sim
+see top level building.md for details
diff --git a/wpilibc/simulation/include/AnalogInput.h b/wpilibc/simulation/include/AnalogInput.h
new file mode 100644
index 0000000..59a3a85
--- /dev/null
+++ b/wpilibc/simulation/include/AnalogInput.h
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "simulation/SimFloatInput.h"
+#include "SensorBase.h"
+#include "PIDSource.h"
+#include "LiveWindow/LiveWindowSendable.h"
+
+#include <memory>
+
+/**
+ * Analog input class.
+ *
+ * Connected to each analog channel is an averaging and oversampling engine. This engine accumulates
+ * the specified ( by SetAverageBits() and SetOversampleBits() ) number of samples before returning a new
+ * value. This is not a sliding window average. The only difference between the oversampled samples and
+ * the averaged samples is that the oversampled samples are simply accumulated effectively increasing the
+ * resolution, while the averaged samples are divided by the number of samples to retain the resolution,
+ * but get more stable values.
+ */
+class AnalogInput : public SensorBase, public PIDSource, public LiveWindowSendable
+{
+public:
+ static const uint8_t kAccumulatorModuleNumber = 1;
+ static const uint32_t kAccumulatorNumChannels = 2;
+ static const uint32_t kAccumulatorChannels[kAccumulatorNumChannels];
+
+ explicit AnalogInput(uint32_t channel);
+ virtual ~AnalogInput() = default;
+
+ float GetVoltage() const;
+ float GetAverageVoltage() const;
+
+ uint32_t GetChannel() const;
+
+ double PIDGet() override;
+
+ void UpdateTable() override;
+ void StartLiveWindowMode() override;
+ void StopLiveWindowMode() override;
+ std::string GetSmartDashboardType() const override;
+ void InitTable(std::shared_ptr<ITable> subTable) override;
+ std::shared_ptr<ITable> GetTable() const override;
+
+private:
+ uint32_t m_channel;
+ SimFloatInput* m_impl;
+ int64_t m_accumulatorOffset;
+
+ std::shared_ptr<ITable> m_table;
+};
diff --git a/wpilibc/simulation/include/AnalogPotentiometer.h b/wpilibc/simulation/include/AnalogPotentiometer.h
new file mode 100644
index 0000000..786f36f
--- /dev/null
+++ b/wpilibc/simulation/include/AnalogPotentiometer.h
@@ -0,0 +1,85 @@
+#pragma once
+
+#include "AnalogInput.h"
+#include "interfaces/Potentiometer.h"
+#include "LiveWindow/LiveWindowSendable.h"
+
+#include <memory>
+
+/**
+ * Class for reading analog potentiometers. Analog potentiometers read
+ * in an analog voltage that corresponds to a position. Usually the
+ * position is either degrees or meters. However, if no conversion is
+ * given it remains volts.
+ *
+ * @author Alex Henning
+ */
+class AnalogPotentiometer : public Potentiometer, public LiveWindowSendable {
+public:
+ /**
+ * AnalogPotentiometer constructor.
+ *
+ * Use the scaling and offset values so that the output produces
+ * meaningful values. I.E: you have a 270 degree potentiometer and
+ * you want the output to be degrees with the halfway point as 0
+ * degrees. The scale value is 270.0(degrees)/5.0(volts) and the
+ * offset is -135.0 since the halfway point after scaling is 135
+ * degrees.
+ *
+ * @param channel The analog channel this potentiometer is plugged into.
+ * @param scale The scaling to multiply the voltage by to get a meaningful unit.
+ * @param offset The offset to add to the scaled value for controlling the zero value
+ */
+ AnalogPotentiometer(int channel, double scale = 1.0, double offset = 0.0);
+
+ AnalogPotentiometer(AnalogInput *input, double scale = 1.0, double offset = 0.0);
+
+ AnalogPotentiometer(AnalogInput &input, double scale = 1.0, double offset = 0.0);
+
+ virtual ~AnalogPotentiometer();
+
+ /**
+ * Get the current reading of the potentiomere.
+ *
+ * @return The current position of the potentiometer.
+ */
+ virtual double Get() const;
+
+
+ /**
+ * Implement the PIDSource interface.
+ *
+ * @return The current reading.
+ */
+ virtual double PIDGet() override;
+
+
+ /*
+ * Live Window code, only does anything if live window is activated.
+ */
+ virtual std::string GetSmartDashboardType() const override;
+ virtual void InitTable(std::shared_ptr<ITable> subtable) override;
+ virtual void UpdateTable() override;
+ virtual std::shared_ptr<ITable> GetTable() const override;
+
+ /**
+ * AnalogPotentiometers don't have to do anything special when entering the LiveWindow.
+ */
+ virtual void StartLiveWindowMode() override {}
+
+ /**
+ * AnalogPotentiometers don't have to do anything special when exiting the LiveWindow.
+ */
+ virtual void StopLiveWindowMode() override {}
+
+private:
+ double m_scale, m_offset;
+ AnalogInput* m_analog_input;
+ std::shared_ptr<ITable> m_table;
+ bool m_init_analog_input;
+
+ /**
+ * Common initialization code called by all constructors.
+ */
+ void initPot(AnalogInput *input, double scale, double offset);
+};
diff --git a/wpilibc/simulation/include/Counter.h b/wpilibc/simulation/include/Counter.h
new file mode 100644
index 0000000..338efc0
--- /dev/null
+++ b/wpilibc/simulation/include/Counter.h
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "HAL/HAL.hpp"
+#include "CounterBase.h"
+#include "SensorBase.h"
+#include "LiveWindow/LiveWindowSendable.h"
+
+#include <memory>
+
+/**
+ * Class for counting the number of ticks on a digital input channel.
+ * This is a general purpose class for counting repetitive events. It can return the number
+ * of counts, the period of the most recent cycle, and detect when the signal being counted
+ * has stopped by supplying a maximum cycle time.
+ *
+ * All counters will immediately start counting - Reset() them if you need them
+ * to be zeroed before use.
+ */
+class Counter : public SensorBase, public CounterBase, public LiveWindowSendable
+{
+public:
+
+ explicit Counter(Mode mode = kTwoPulse);
+ explicit Counter(uint32_t channel);
+ // TODO: [Not Supported] explicit Counter(DigitalSource *source);
+ // TODO: [Not Supported] explicit Counter(DigitalSource &source);
+ // TODO: [Not Supported] explicit Counter(AnalogTrigger *source);
+ // TODO: [Not Supported] explicit Counter(AnalogTrigger &source);
+ // TODO: [Not Supported] Counter(EncodingType encodingType, DigitalSource *upSource, DigitalSource *downSource, bool inverted);
+ virtual ~Counter();
+
+ void SetUpSource(uint32_t channel);
+ // TODO: [Not Supported] void SetUpSource(AnalogTrigger *analogTrigger, AnalogTriggerType triggerType);
+ // TODO: [Not Supported] void SetUpSource(AnalogTrigger &analogTrigger, AnalogTriggerType triggerType);
+ // TODO: [Not Supported] void SetUpSource(DigitalSource *source);
+ // TODO: [Not Supported] void SetUpSource(DigitalSource &source);
+ void SetUpSourceEdge(bool risingEdge, bool fallingEdge);
+ void ClearUpSource();
+
+ void SetDownSource(uint32_t channel);
+ // TODO: [Not Supported] void SetDownSource(AnalogTrigger *analogTrigger, AnalogTriggerType triggerType);
+ // TODO: [Not Supported] void SetDownSource(AnalogTrigger &analogTrigger, AnalogTriggerType triggerType);
+ // TODO: [Not Supported] void SetDownSource(DigitalSource *source);
+ // TODO: [Not Supported] void SetDownSource(DigitalSource &source);
+ void SetDownSourceEdge(bool risingEdge, bool fallingEdge);
+ void ClearDownSource();
+
+ void SetUpDownCounterMode();
+ void SetExternalDirectionMode();
+ void SetSemiPeriodMode(bool highSemiPeriod);
+ void SetPulseLengthMode(float threshold);
+
+ void SetReverseDirection(bool reverseDirection);
+
+ // CounterBase interface
+ int32_t Get() const override;
+ void Reset() override;
+ double GetPeriod() const override;
+ void SetMaxPeriod(double maxPeriod) override;
+ void SetUpdateWhenEmpty(bool enabled);
+ bool GetStopped() const override;
+ bool GetDirection() const override;
+
+ void SetSamplesToAverage(int samplesToAverage);
+ int GetSamplesToAverage() const;
+ uint32_t GetFPGAIndex() const
+ {
+ return m_index;
+ }
+
+ void UpdateTable() override;
+ void StartLiveWindowMode() override;
+ void StopLiveWindowMode() override;
+ virtual std::string GetSmartDashboardType() const override;
+ void InitTable(std::shared_ptr<ITable> subTable) override;
+ std::shared_ptr<ITable> GetTable() const override;
+protected:
+ // TODO: [Not Supported] DigitalSource *m_upSource; ///< What makes the counter count up.
+ // TODO: [Not Supported] DigitalSource *m_downSource; ///< What makes the counter count down.
+ void* m_counter; ///< The FPGA counter object.
+private:
+
+ bool m_allocatedUpSource; ///< Was the upSource allocated locally?
+ bool m_allocatedDownSource; ///< Was the downSource allocated locally?
+ uint32_t m_index; ///< The index of this counter.
+
+ std::shared_ptr<ITable> m_table;
+};
diff --git a/wpilibc/simulation/include/CounterBase.h b/wpilibc/simulation/include/CounterBase.h
new file mode 100644
index 0000000..3352c29
--- /dev/null
+++ b/wpilibc/simulation/include/CounterBase.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+/**
+ * Interface for counting the number of ticks on a digital input channel.
+ * Encoders, Gear tooth sensors, and counters should all subclass this so it can be used to
+ * build more advanced classes for control and driving.
+ *
+ * All counters will immediately start counting - Reset() them if you need them
+ * to be zeroed before use.
+ */
+class CounterBase
+{
+public:
+ enum EncodingType
+ {
+ k1X,
+ k2X,
+ k4X
+ };
+
+ virtual ~CounterBase() = default;
+ virtual int32_t Get() const = 0;
+ virtual void Reset() = 0;
+ virtual double GetPeriod() const = 0;
+ virtual void SetMaxPeriod(double maxPeriod) = 0;
+ virtual bool GetStopped() const = 0;
+ virtual bool GetDirection() const = 0;
+};
diff --git a/wpilibc/simulation/include/DigitalInput.h b/wpilibc/simulation/include/DigitalInput.h
new file mode 100644
index 0000000..1482e9c
--- /dev/null
+++ b/wpilibc/simulation/include/DigitalInput.h
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "simulation/SimDigitalInput.h"
+#include "LiveWindow/LiveWindowSendable.h"
+
+#include <memory>
+
+/**
+ * Class to read a digital input.
+ * This class will read digital inputs and return the current value on the channel. Other devices
+ * such as encoders, gear tooth sensors, etc. that are implemented elsewhere will automatically
+ * allocate digital inputs and outputs as required. This class is only for devices like switches
+ * etc. that aren't implemented anywhere else.
+ */
+class DigitalInput : public LiveWindowSendable {
+public:
+ explicit DigitalInput(uint32_t channel);
+ virtual ~DigitalInput() = default;
+ uint32_t Get() const;
+ uint32_t GetChannel() const;
+
+ void UpdateTable() override;
+ void StartLiveWindowMode() override;
+ void StopLiveWindowMode() override;
+ std::string GetSmartDashboardType() const override;
+ void InitTable(std::shared_ptr<ITable> subTable) override;
+ std::shared_ptr<ITable> GetTable() const override;
+
+private:
+ uint32_t m_channel;
+ bool m_lastValue;
+ SimDigitalInput *m_impl;
+
+ std::shared_ptr<ITable> m_table;
+};
diff --git a/wpilibc/simulation/include/DoubleSolenoid.h b/wpilibc/simulation/include/DoubleSolenoid.h
new file mode 100644
index 0000000..122a439
--- /dev/null
+++ b/wpilibc/simulation/include/DoubleSolenoid.h
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "simulation/SimContinuousOutput.h"
+#include "LiveWindow/LiveWindowSendable.h"
+#include "tables/ITableListener.h"
+
+#include <memory>
+
+/**
+ * DoubleSolenoid class for running 2 channels of high voltage Digital Output
+ * (PCM).
+ *
+ * The DoubleSolenoid class is typically used for pneumatics solenoids that
+ * have two positions controlled by two separate channels.
+ */
+class DoubleSolenoid : public LiveWindowSendable, public ITableListener
+{
+public:
+ enum Value
+ {
+ kOff,
+ kForward,
+ kReverse
+ };
+
+ explicit DoubleSolenoid(uint32_t forwardChannel, uint32_t reverseChannel);
+ DoubleSolenoid(uint8_t moduleNumber, uint32_t forwardChannel, uint32_t reverseChannel);
+ virtual ~DoubleSolenoid();
+ virtual void Set(Value value);
+ virtual Value Get() const;
+
+ void ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) override;
+ void UpdateTable() override;
+ void StartLiveWindowMode() override;
+ void StopLiveWindowMode() override;
+ std::string GetSmartDashboardType() const override;
+ void InitTable(std::shared_ptr<ITable> subTable) override;
+ std::shared_ptr<ITable> GetTable() const override;
+
+private:
+ SimContinuousOutput* m_impl;
+ Value m_value;
+ bool m_reversed;
+
+ std::shared_ptr<ITable> m_table;
+};
diff --git a/wpilibc/simulation/include/DriverStation.h b/wpilibc/simulation/include/DriverStation.h
new file mode 100644
index 0000000..f15985c
--- /dev/null
+++ b/wpilibc/simulation/include/DriverStation.h
@@ -0,0 +1,112 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "SensorBase.h"
+#include "RobotState.h"
+#include "HAL/HAL.hpp"
+#include "HAL/cpp/Semaphore.hpp"
+#include "HAL/cpp/priority_mutex.h"
+#include "HAL/cpp/priority_condition_variable.h"
+#include <condition_variable>
+
+struct HALControlWord;
+class AnalogInput;
+
+/**
+ * Provide access to the network communication data to / from the Driver
+ * Station.
+ */
+class DriverStation : public SensorBase, public RobotStateInterface {
+ public:
+ enum Alliance { kRed, kBlue, kInvalid };
+
+ virtual ~DriverStation();
+ static DriverStation &GetInstance();
+ static void ReportError(std::string error);
+
+ static const uint32_t kJoystickPorts = 6;
+
+ float GetStickAxis(uint32_t stick, uint32_t axis);
+ int GetStickPOV(uint32_t stick, uint32_t pov);
+ uint32_t GetStickButtons(uint32_t stick) const;
+ bool GetStickButton(uint32_t stick, uint8_t button);
+
+ int GetStickAxisCount(uint32_t stick) const;
+ int GetStickPOVCount(uint32_t stick) const;
+ int GetStickButtonCount(uint32_t stick) const;
+
+ bool GetJoystickIsXbox(uint32_t stick) const;
+ int GetJoystickType(uint32_t stick) const;
+ std::string GetJoystickName(uint32_t stick) const;
+ int GetJoystickAxisType(uint32_t stick, uint8_t axis) const;
+
+ bool IsEnabled() const override;
+ bool IsDisabled() const override;
+ bool IsAutonomous() const override;
+ bool IsOperatorControl() const override;
+ bool IsTest() const override;
+ bool IsDSAttached() const;
+ bool IsNewControlData() const;
+ bool IsFMSAttached() const;
+ bool IsSysActive() const;
+ bool IsSysBrownedOut() const;
+
+ Alliance GetAlliance() const;
+ uint32_t GetLocation() const;
+ void WaitForData();
+ double GetMatchTime() const;
+ float GetBatteryVoltage() const;
+
+ /** Only to be used to tell the Driver Station what code you claim to be
+ * executing
+ * for diagnostic purposes only
+ * @param entering If true, starting disabled code; if false, leaving disabled
+ * code */
+ void InDisabled(bool entering) { m_userInDisabled = entering; }
+ /** Only to be used to tell the Driver Station what code you claim to be
+ * executing
+ * for diagnostic purposes only
+ * @param entering If true, starting autonomous code; if false, leaving
+ * autonomous code */
+ void InAutonomous(bool entering) { m_userInAutonomous = entering; }
+ /** Only to be used to tell the Driver Station what code you claim to be
+ * executing
+ * for diagnostic purposes only
+ * @param entering If true, starting teleop code; if false, leaving teleop
+ * code */
+ void InOperatorControl(bool entering) { m_userInTeleop = entering; }
+ /** Only to be used to tell the Driver Station what code you claim to be
+ * executing
+ * for diagnostic purposes only
+ * @param entering If true, starting test code; if false, leaving test code */
+ void InTest(bool entering) { m_userInTest = entering; }
+
+ protected:
+ DriverStation();
+
+ void GetData();
+
+ private:
+ static DriverStation *m_instance;
+ void ReportJoystickUnpluggedError(std::string message);
+ void Run();
+
+ HALJoystickAxes m_joystickAxes[kJoystickPorts];
+ HALJoystickPOVs m_joystickPOVs[kJoystickPorts];
+ HALJoystickButtons m_joystickButtons[kJoystickPorts];
+ HALJoystickDescriptor m_joystickDescriptor[kJoystickPorts];
+ mutable Semaphore m_newControlData{Semaphore::kEmpty};
+ mutable priority_condition_variable m_packetDataAvailableCond;
+ priority_mutex m_packetDataAvailableMutex;
+ std::condition_variable_any m_waitForDataCond;
+ priority_mutex m_waitForDataMutex;
+ bool m_userInDisabled = false;
+ bool m_userInAutonomous = false;
+ bool m_userInTeleop = false;
+ bool m_userInTest = false;
+ double m_nextMessageTime = 0;
+};
diff --git a/wpilibc/simulation/include/Encoder.h b/wpilibc/simulation/include/Encoder.h
new file mode 100644
index 0000000..89b1b3a
--- /dev/null
+++ b/wpilibc/simulation/include/Encoder.h
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "simulation/SimEncoder.h"
+#include "CounterBase.h"
+#include "SensorBase.h"
+#include "Counter.h"
+#include "PIDSource.h"
+#include "LiveWindow/LiveWindowSendable.h"
+
+#include <memory>
+
+/**
+ * Class to read quad encoders.
+ * Quadrature encoders are devices that count shaft rotation and can sense direction. The output of
+ * the QuadEncoder class is an integer that can count either up or down, and can go negative for
+ * reverse direction counting. When creating QuadEncoders, a direction is supplied that changes the
+ * sense of the output to make code more readable if the encoder is mounted such that forward movement
+ * generates negative values. Quadrature encoders have two digital outputs, an A Channel and a B Channel
+ * that are out of phase with each other to allow the FPGA to do direction sensing.
+ *
+ * All encoders will immediately start counting - Reset() them if you need them
+ * to be zeroed before use.
+ */
+class Encoder : public SensorBase, public CounterBase, public PIDSource, public LiveWindowSendable
+{
+public:
+
+ Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection = false,
+ EncodingType encodingType = k4X);
+ // TODO: [Not Supported] Encoder(DigitalSource *aSource, DigitalSource *bSource, bool reverseDirection=false, EncodingType encodingType = k4X);
+ // TODO: [Not Supported] Encoder(DigitalSource &aSource, DigitalSource &bSource, bool reverseDirection=false, EncodingType encodingType = k4X);
+ virtual ~Encoder() = default;
+
+ // CounterBase interface
+ int32_t Get() const override;
+ int32_t GetRaw() const;
+ int32_t GetEncodingScale() const;
+ void Reset() override;
+ double GetPeriod() const override;
+ void SetMaxPeriod(double maxPeriod) override;
+ bool GetStopped() const override;
+ bool GetDirection() const override;
+
+ double GetDistance() const;
+ double GetRate() const;
+ void SetMinRate(double minRate);
+ void SetDistancePerPulse(double distancePerPulse);
+ void SetReverseDirection(bool reverseDirection);
+ void SetSamplesToAverage(int samplesToAverage);
+ int GetSamplesToAverage() const;
+ void SetPIDSourceType(PIDSourceType pidSource);
+ double PIDGet() override;
+
+ void UpdateTable() override;
+ void StartLiveWindowMode() override;
+ void StopLiveWindowMode() override;
+ std::string GetSmartDashboardType() const override;
+ void InitTable(std::shared_ptr<ITable> subTable) override;
+ std::shared_ptr<ITable> GetTable() const override;
+
+ int32_t FPGAEncoderIndex() const
+ {
+ return 0;
+ }
+
+private:
+ void InitEncoder(int channelA, int channelB, bool _reverseDirection, EncodingType encodingType);
+ double DecodingScaleFactor() const;
+
+ // TODO: [Not Supported] DigitalSource *m_aSource; // the A phase of the quad encoder
+ // TODO: [Not Supported] DigitalSource *m_bSource; // the B phase of the quad encoder
+ // TODO: [Not Supported] bool m_allocatedASource; // was the A source allocated locally?
+ // TODO: [Not Supported] bool m_allocatedBSource; // was the B source allocated locally?
+ int channelA, channelB;
+ double m_distancePerPulse; // distance of travel for each encoder tick
+ EncodingType m_encodingType; // Encoding type
+ int32_t m_encodingScale; // 1x, 2x, or 4x, per the encodingType
+ bool m_reverseDirection;
+ SimEncoder* impl;
+
+ std::shared_ptr<ITable> m_table;
+};
diff --git a/wpilibc/simulation/include/Gyro.h b/wpilibc/simulation/include/Gyro.h
new file mode 100644
index 0000000..7e4e40a
--- /dev/null
+++ b/wpilibc/simulation/include/Gyro.h
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "SensorBase.h"
+#include "PIDSource.h"
+#include "LiveWindow/LiveWindowSendable.h"
+#include "simulation/SimGyro.h"
+
+#include <memory>
+
+class AnalogInput;
+class AnalogModule;
+
+/**
+ * Use a rate gyro to return the robots heading relative to a starting position.
+ * The Gyro class tracks the robots heading based on the starting position. As the robot
+ * rotates the new heading is computed by integrating the rate of rotation returned
+ * by the sensor. When the class is instantiated, it does a short calibration routine
+ * where it samples the gyro while at rest to determine the default offset. This is
+ * subtracted from each sample to determine the heading. This gyro class must be used
+ * with a channel that is assigned one of the Analog accumulators from the FPGA. See
+ * AnalogInput for the current accumulator assignments.
+ */
+class Gyro : public SensorBase, public PIDSource, public LiveWindowSendable
+{
+public:
+ static const uint32_t kOversampleBits;
+ static const uint32_t kAverageBits;
+ static const float kSamplesPerSecond;
+ static const float kCalibrationSampleTime;
+ static const float kDefaultVoltsPerDegreePerSecond;
+
+ explicit Gyro(uint32_t channel);
+ virtual ~Gyro() = default;
+ virtual float GetAngle() const;
+ virtual double GetRate() const;
+ virtual void Reset();
+
+ // PIDSource interface
+ void SetPIDSourceType(PIDSourceType pidSource) override;
+ double PIDGet() override;
+
+ void UpdateTable() override;
+ void StartLiveWindowMode() override;
+ void StopLiveWindowMode() override;
+ std::string GetSmartDashboardType() const override;
+ void InitTable(std::shared_ptr<ITable> subTable) override;
+ std::shared_ptr<ITable> GetTable() const override;
+
+private:
+ void InitGyro(int channel);
+
+ SimGyro* impl;
+
+ std::shared_ptr<ITable> m_table;
+};
diff --git a/wpilibc/simulation/include/IterativeRobot.h b/wpilibc/simulation/include/IterativeRobot.h
new file mode 100644
index 0000000..7c9104d
--- /dev/null
+++ b/wpilibc/simulation/include/IterativeRobot.h
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "Timer.h"
+#include "RobotBase.h"
+
+/**
+ * IterativeRobot implements a specific type of Robot Program framework, extending the RobotBase class.
+ *
+ * The IterativeRobot class is intended to be subclassed by a user creating a robot program.
+ *
+ * This class is intended to implement the "old style" default code, by providing
+ * the following functions which are called by the main loop, StartCompetition(), at the appropriate times:
+ *
+ * RobotInit() -- provide for initialization at robot power-on
+ *
+ * Init() functions -- each of the following functions is called once when the
+ * appropriate mode is entered:
+ * - DisabledInit() -- called only when first disabled
+ * - AutonomousInit() -- called each and every time autonomous is entered from another mode
+ * - TeleopInit() -- called each and every time teleop is entered from another mode
+ * - TestInit() -- called each and every time test is entered from another mode
+ *
+ * Periodic() functions -- each of these functions is called iteratively at the
+ * appropriate periodic rate (aka the "slow loop"). The default period of
+ * the iterative robot is synced to the driver station control packets,
+ * giving a periodic frequency of about 50Hz (50 times per second).
+ * - DisabledPeriodic()
+ * - AutonomousPeriodic()
+ * - TeleopPeriodic()
+ * - TestPeriodic()
+ *
+ */
+
+class IterativeRobot : public RobotBase
+{
+public:
+ /*
+ * The default period for the periodic function calls (seconds)
+ * Setting the period to 0.0 will cause the periodic functions to follow
+ * the Driver Station packet rate of about 50Hz.
+ */
+ static const double kDefaultPeriod;
+
+ virtual void StartCompetition();
+
+ virtual void RobotInit();
+ virtual void DisabledInit();
+ virtual void AutonomousInit();
+ virtual void TeleopInit();
+ virtual void TestInit();
+
+ virtual void DisabledPeriodic();
+ virtual void AutonomousPeriodic();
+ virtual void TeleopPeriodic();
+ virtual void TestPeriodic();
+
+ void SetPeriod(double period);
+ double GetPeriod();
+ double GetLoopsPerSec();
+
+protected:
+ virtual ~IterativeRobot() = default;
+ IterativeRobot() = default;
+
+private:
+ bool NextPeriodReady();
+
+ bool m_disabledInitialized = false;
+ bool m_autonomousInitialized = false;
+ bool m_teleopInitialized = false;
+ bool m_testInitialized = false;
+ double m_period = kDefaultPeriod;
+ Timer m_mainLoopTimer;
+};
diff --git a/wpilibc/simulation/include/Jaguar.h b/wpilibc/simulation/include/Jaguar.h
new file mode 100644
index 0000000..1f522d9
--- /dev/null
+++ b/wpilibc/simulation/include/Jaguar.h
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "SafePWM.h"
+#include "SpeedController.h"
+#include "PIDOutput.h"
+
+/**
+ * Luminary Micro Jaguar Speed Control
+ */
+class Jaguar : public SafePWM, public SpeedController
+{
+public:
+ explicit Jaguar(uint32_t channel);
+ virtual ~Jaguar() = default;
+ virtual void Set(float value, uint8_t syncGroup = 0);
+ virtual float Get() const;
+ virtual void Disable();
+
+ virtual void PIDWrite(float output) override;
+};
diff --git a/wpilibc/simulation/include/Joystick.h b/wpilibc/simulation/include/Joystick.h
new file mode 100644
index 0000000..0922359
--- /dev/null
+++ b/wpilibc/simulation/include/Joystick.h
@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef JOYSTICK_H_
+#define JOYSTICK_H_
+
+#include <cstdint>
+#include <memory>
+#include <vector>
+#include "GenericHID.h"
+#include "ErrorBase.h"
+
+class DriverStation;
+
+/**
+ * Handle input from standard Joysticks connected to the Driver Station.
+ * This class handles standard input that comes from the Driver Station. Each
+ * time a value is requested
+ * the most recent value is returned. There is a single class instance for each
+ * joystick and the mapping
+ * of ports to hardware buttons depends on the code in the driver station.
+ */
+class Joystick : public GenericHID, public ErrorBase {
+ public:
+ static const uint32_t kDefaultXAxis = 0;
+ static const uint32_t kDefaultYAxis = 1;
+ static const uint32_t kDefaultZAxis = 2;
+ static const uint32_t kDefaultTwistAxis = 2;
+ static const uint32_t kDefaultThrottleAxis = 3;
+ typedef enum {
+ kXAxis,
+ kYAxis,
+ kZAxis,
+ kTwistAxis,
+ kThrottleAxis,
+ kNumAxisTypes
+ } AxisType;
+ static const uint32_t kDefaultTriggerButton = 1;
+ static const uint32_t kDefaultTopButton = 2;
+ typedef enum { kTriggerButton, kTopButton, kNumButtonTypes } ButtonType;
+ typedef enum { kLeftRumble, kRightRumble } RumbleType;
+ typedef enum {
+ kUnknown = -1,
+ kXInputUnknown = 0,
+ kXInputGamepad = 1,
+ kXInputWheel = 2,
+ kXInputArcadeStick = 3,
+ kXInputFlightStick = 4,
+ kXInputDancePad = 5,
+ kXInputGuitar = 6,
+ kXInputGuitar2 = 7,
+ kXInputDrumKit = 8,
+ kXInputGuitar3 = 11,
+ kXInputArcadePad = 19,
+ kHIDJoystick = 20,
+ kHIDGamepad = 21,
+ kHIDDriving = 22,
+ kHIDFlight = 23,
+ kHID1stPerson = 24
+ } HIDType;
+ explicit Joystick(uint32_t port);
+ Joystick(uint32_t port, uint32_t numAxisTypes, uint32_t numButtonTypes);
+ virtual ~Joystick() = default;
+
+ Joystick(const Joystick&) = delete;
+ Joystick& operator=(const Joystick&) = delete;
+
+ uint32_t GetAxisChannel(AxisType axis) const;
+ void SetAxisChannel(AxisType axis, uint32_t channel);
+
+ virtual float GetX(JoystickHand hand = kRightHand) const override;
+ virtual float GetY(JoystickHand hand = kRightHand) const override;
+ virtual float GetZ() const override;
+ virtual float GetTwist() const override;
+ virtual float GetThrottle() const override;
+ virtual float GetAxis(AxisType axis) const;
+ float GetRawAxis(uint32_t axis) const override;
+
+ virtual bool GetTrigger(JoystickHand hand = kRightHand) const override;
+ virtual bool GetTop(JoystickHand hand = kRightHand) const override;
+ virtual bool GetBumper(JoystickHand hand = kRightHand) const override;
+ virtual bool GetRawButton(uint32_t button) const override;
+ virtual int GetPOV(uint32_t pov = 0) const override;
+ bool GetButton(ButtonType button) const;
+ static Joystick *GetStickForPort(uint32_t port);
+
+ virtual float GetMagnitude() const;
+ virtual float GetDirectionRadians() const;
+ virtual float GetDirectionDegrees() const;
+
+ bool GetIsXbox() const;
+ Joystick::HIDType GetType() const;
+ std::string GetName() const;
+ int GetAxisType(uint8_t axis) const;
+
+ int GetAxisCount() const;
+ int GetButtonCount() const;
+ int GetPOVCount() const;
+
+ void SetRumble(RumbleType type, float value);
+ void SetOutput(uint8_t outputNumber, bool value);
+ void SetOutputs(uint32_t value);
+
+ private:
+ DriverStation &m_ds;
+ uint32_t m_port;
+ ::std::vector<uint32_t> m_axes;
+ ::std::vector<uint32_t> m_buttons;
+ uint32_t m_outputs = 0;
+ uint16_t m_leftRumble = 0;
+ uint16_t m_rightRumble = 0;
+};
+
+#endif
diff --git a/wpilibc/simulation/include/MotorSafety.h b/wpilibc/simulation/include/MotorSafety.h
new file mode 100644
index 0000000..c718806
--- /dev/null
+++ b/wpilibc/simulation/include/MotorSafety.h
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#define DEFAULT_SAFETY_EXPIRATION 0.1
+
+#include <sstream>
+
+class MotorSafety
+{
+public:
+ virtual void SetExpiration(float timeout) = 0;
+ virtual float GetExpiration() const = 0;
+ virtual bool IsAlive() const = 0;
+ virtual void StopMotor() = 0;
+ virtual void SetSafetyEnabled(bool enabled) = 0;
+ virtual bool IsSafetyEnabled() const = 0;
+ virtual void GetDescription(std::ostringstream& desc) const = 0;
+};
diff --git a/wpilibc/simulation/include/MotorSafetyHelper.h b/wpilibc/simulation/include/MotorSafetyHelper.h
new file mode 100644
index 0000000..bbe7658
--- /dev/null
+++ b/wpilibc/simulation/include/MotorSafetyHelper.h
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "ErrorBase.h"
+#include "HAL/cpp/priority_mutex.h"
+
+#include <set>
+
+class MotorSafety;
+
+class MotorSafetyHelper : public ErrorBase {
+ public:
+ MotorSafetyHelper(MotorSafety *safeObject);
+ ~MotorSafetyHelper();
+ void Feed();
+ void SetExpiration(float expirationTime);
+ float GetExpiration() const;
+ bool IsAlive() const;
+ void Check();
+ void SetSafetyEnabled(bool enabled);
+ bool IsSafetyEnabled() const;
+ static void CheckMotors();
+
+ private:
+ double m_expiration; // the expiration time for this object
+ bool m_enabled; // true if motor safety is enabled for this motor
+ double m_stopTime; // the FPGA clock value when this motor has expired
+ mutable priority_recursive_mutex
+ m_syncMutex; // protect accesses to the state for this object
+ MotorSafety *m_safeObject; // the object that is using the helper
+ // List of all existing MotorSafetyHelper objects.
+ static std::set<MotorSafetyHelper*> m_helperList;
+ static priority_recursive_mutex
+ m_listMutex; // protect accesses to the list of helpers
+};
diff --git a/wpilibc/simulation/include/NetworkCommunication/FRCComm.h b/wpilibc/simulation/include/NetworkCommunication/FRCComm.h
new file mode 100644
index 0000000..588c8ca
--- /dev/null
+++ b/wpilibc/simulation/include/NetworkCommunication/FRCComm.h
@@ -0,0 +1,130 @@
+/*************************************************************
+ * NOTICE
+ *
+ * These are the only externally exposed functions to the
+ * NetworkCommunication library
+ *
+ * This is an implementation of FRC Spec for Comm Protocol
+ * Revision 4.5, June 30, 2008
+ *
+ * Copyright (c) National Instruments 2008. All Rights Reserved.
+ *
+ *************************************************************/
+
+#ifndef __FRC_COMM_H__
+#define __FRC_COMM_H__
+
+#ifdef SIMULATION
+#include <vxWorks_compat.h>
+#ifdef USE_THRIFT
+#define EXPORT_FUNC
+#else
+#define EXPORT_FUNC __declspec(dllexport) __cdecl
+#endif
+#else
+#include <stdint.h>
+#include <pthread.h>
+#define EXPORT_FUNC
+#endif
+
+#define ERR_FRCSystem_NetCommNotResponding -44049
+#define ERR_FRCSystem_NoDSConnection -44018
+
+enum AllianceStationID_t {
+ kAllianceStationID_red1,
+ kAllianceStationID_red2,
+ kAllianceStationID_red3,
+ kAllianceStationID_blue1,
+ kAllianceStationID_blue2,
+ kAllianceStationID_blue3,
+};
+
+enum MatchType_t {
+ kMatchType_none,
+ kMatchType_practice,
+ kMatchType_qualification,
+ kMatchType_elimination,
+};
+
+struct ControlWord_t {
+ uint32_t enabled : 1;
+ uint32_t autonomous : 1;
+ uint32_t test : 1;
+ uint32_t eStop : 1;
+ uint32_t fmsAttached : 1;
+ uint32_t dsAttached : 1;
+ uint32_t control_reserved : 26;
+};
+
+struct JoystickAxes_t {
+ uint16_t count;
+ int16_t axes[1];
+};
+
+struct JoystickPOV_t {
+ uint16_t count;
+ int16_t povs[1];
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+int EXPORT_FUNC FRC_NetworkCommunication_Reserve(void *instance);
+#ifndef SIMULATION
+void EXPORT_FUNC
+getFPGAHardwareVersion(uint16_t *fpgaVersion, uint32_t *fpgaRevision);
+#endif
+int EXPORT_FUNC setStatusData(float battery, uint8_t dsDigitalOut,
+ uint8_t updateNumber, const char *userDataHigh,
+ int userDataHighLength, const char *userDataLow,
+ int userDataLowLength, int wait_ms);
+int EXPORT_FUNC setErrorData(const char *errors, int errorsLength, int wait_ms);
+
+#ifdef SIMULATION
+void EXPORT_FUNC setNewDataSem(HANDLE);
+#else
+void EXPORT_FUNC setNewDataSem(pthread_cond_t *);
+#endif
+
+// this uint32_t is really a LVRefNum
+int EXPORT_FUNC setNewDataOccurRef(uint32_t refnum);
+
+int EXPORT_FUNC
+FRC_NetworkCommunication_getControlWord(struct ControlWord_t *controlWord);
+int EXPORT_FUNC FRC_NetworkCommunication_getAllianceStation(
+ enum AllianceStationID_t *allianceStation);
+int EXPORT_FUNC FRC_NetworkCommunication_getMatchTime(float *matchTime);
+int EXPORT_FUNC
+FRC_NetworkCommunication_getJoystickAxes(uint8_t joystickNum,
+ struct JoystickAxes_t *axes,
+ uint8_t maxAxes);
+int EXPORT_FUNC FRC_NetworkCommunication_getJoystickButtons(uint8_t joystickNum,
+ uint32_t *buttons,
+ uint8_t *count);
+int EXPORT_FUNC
+FRC_NetworkCommunication_getJoystickPOVs(uint8_t joystickNum,
+ struct JoystickPOV_t *povs,
+ uint8_t maxPOVs);
+int EXPORT_FUNC
+FRC_NetworkCommunication_setJoystickOutputs(uint8_t joystickNum,
+ uint32_t hidOutputs,
+ uint16_t leftRumble,
+ uint16_t rightRumble);
+int EXPORT_FUNC
+FRC_NetworkCommunication_getJoystickDesc(uint8_t joystickNum, uint8_t *isXBox,
+ uint8_t *type, char *name,
+ uint8_t *axisCount, uint8_t *axisTypes,
+ uint8_t *buttonCount,
+ uint8_t *povCount);
+
+void EXPORT_FUNC FRC_NetworkCommunication_getVersionString(char *version);
+int EXPORT_FUNC FRC_NetworkCommunication_observeUserProgramStarting(void);
+void EXPORT_FUNC FRC_NetworkCommunication_observeUserProgramDisabled(void);
+void EXPORT_FUNC FRC_NetworkCommunication_observeUserProgramAutonomous(void);
+void EXPORT_FUNC FRC_NetworkCommunication_observeUserProgramTeleop(void);
+void EXPORT_FUNC FRC_NetworkCommunication_observeUserProgramTest(void);
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/wpilibc/simulation/include/PWM.h b/wpilibc/simulation/include/PWM.h
new file mode 100644
index 0000000..52007fb
--- /dev/null
+++ b/wpilibc/simulation/include/PWM.h
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "SensorBase.h"
+#include "simulation/SimContinuousOutput.h"
+#include "LiveWindow/LiveWindowSendable.h"
+#include "tables/ITableListener.h"
+
+#include <memory>
+
+/**
+ * Class implements the PWM generation in the FPGA.
+ *
+ * The values supplied as arguments for PWM outputs range from -1.0 to 1.0. They are mapped
+ * to the hardware dependent values, in this case 0-255 for the FPGA.
+ * Changes are immediately sent to the FPGA, and the update occurs at the next
+ * FPGA cycle. There is no delay.
+ *
+ * As of revision 0.1.10 of the FPGA, the FPGA interprets the 0-255 values as follows:
+ * - 255 = full "forward"
+ * - 254 to 129 = linear scaling from "full forward" to "center"
+ * - 128 = center value
+ * - 127 to 2 = linear scaling from "center" to "full reverse"
+ * - 1 = full "reverse"
+ * - 0 = disabled (i.e. PWM output is held low)
+ */
+class PWM : public SensorBase, public ITableListener, public LiveWindowSendable
+{
+public:
+ enum PeriodMultiplier
+ {
+ kPeriodMultiplier_1X = 1,
+ kPeriodMultiplier_2X = 2,
+ kPeriodMultiplier_4X = 4
+ };
+
+ explicit PWM(uint32_t channel);
+ virtual ~PWM();
+ virtual void SetRaw(unsigned short value);
+ void SetPeriodMultiplier(PeriodMultiplier mult);
+ void EnableDeadbandElimination(bool eliminateDeadband);
+ void SetBounds(int32_t max, int32_t deadbandMax, int32_t center, int32_t deadbandMin,
+ int32_t min);
+ void SetBounds(double max, double deadbandMax, double center, double deadbandMin, double min);
+ uint32_t GetChannel() const
+ {
+ return m_channel;
+ }
+
+protected:
+ /**
+ * kDefaultPwmPeriod is in ms
+ *
+ * - 20ms periods (50 Hz) are the "safest" setting in that this works for all devices
+ * - 20ms periods seem to be desirable for Vex Motors
+ * - 20ms periods are the specified period for HS-322HD servos, but work reliably down
+ * to 10.0 ms; starting at about 8.5ms, the servo sometimes hums and get hot;
+ * by 5.0ms the hum is nearly continuous
+ * - 10ms periods work well for Victor 884
+ * - 5ms periods allows higher update rates for Luminary Micro Jaguar speed controllers.
+ * Due to the shipping firmware on the Jaguar, we can't run the update period less
+ * than 5.05 ms.
+ *
+ * kDefaultPwmPeriod is the 1x period (5.05 ms). In hardware, the period scaling is implemented as an
+ * output squelch to get longer periods for old devices.
+ */
+ static const float kDefaultPwmPeriod;
+ /**
+ * kDefaultPwmCenter is the PWM range center in ms
+ */
+ static const float kDefaultPwmCenter;
+ /**
+ * kDefaultPWMStepsDown is the number of PWM steps below the centerpoint
+ */
+ static const int32_t kDefaultPwmStepsDown;
+ static const int32_t kPwmDisabled;
+
+ virtual void SetPosition(float pos);
+ virtual float GetPosition() const;
+ virtual void SetSpeed(float speed);
+ virtual float GetSpeed() const;
+
+ bool m_eliminateDeadband;
+ int32_t m_centerPwm;
+
+ void ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) override;
+ void UpdateTable() override;
+ void StartLiveWindowMode() override;
+ void StopLiveWindowMode() override;
+ std::string GetSmartDashboardType() const override;
+ void InitTable(std::shared_ptr<ITable> subTable) override;
+ std::shared_ptr<ITable> GetTable() const override;
+
+ std::shared_ptr<ITable> m_table;
+
+private:
+ uint32_t m_channel;
+ SimContinuousOutput* impl;
+};
diff --git a/wpilibc/simulation/include/Relay.h b/wpilibc/simulation/include/Relay.h
new file mode 100644
index 0000000..abc9ad5
--- /dev/null
+++ b/wpilibc/simulation/include/Relay.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "MotorSafety.h"
+#include "SensorBase.h"
+#include "simulation/SimContinuousOutput.h"
+#include "tables/ITableListener.h"
+#include "LiveWindow/LiveWindowSendable.h"
+#include "tables/ITable.h"
+
+#include <memory>
+
+class MotorSafetyHelper;
+class DigitalModule;
+
+/**
+ * Class for Spike style relay outputs.
+ * Relays are intended to be connected to spikes or similar relays. The relay channels controls
+ * a pair of pins that are either both off, one on, the other on, or both on. This translates into
+ * two spike outputs at 0v, one at 12v and one at 0v, one at 0v and the other at 12v, or two
+ * spike outputs at 12V. This allows off, full forward, or full reverse control of motors without
+ * variable speed. It also allows the two channels (forward and reverse) to be used independently
+ * for something that does not care about voltage polatiry (like a solenoid).
+ */
+class Relay : public MotorSafety,
+ public SensorBase,
+ public ITableListener,
+ public LiveWindowSendable {
+public:
+ enum Value
+ {
+ kOff,
+ kOn,
+ kForward,
+ kReverse
+ };
+ enum Direction
+ {
+ kBothDirections,
+ kForwardOnly,
+ kReverseOnly
+ };
+
+ Relay(uint32_t channel, Direction direction = kBothDirections);
+ virtual ~Relay();
+
+ void Set(Value value);
+ Value Get() const;
+ uint32_t GetChannel() const;
+
+ void SetExpiration(float timeout) override;
+ float GetExpiration() const override;
+ bool IsAlive() const override;
+ void StopMotor() override;
+ bool IsSafetyEnabled() const override;
+ void SetSafetyEnabled(bool enabled) override;
+ void GetDescription(std::ostringstream& desc) const override;
+
+ void ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) override;
+ void UpdateTable() override;
+ void StartLiveWindowMode() override;
+ void StopLiveWindowMode() override;
+ std::string GetSmartDashboardType() const override;
+ void InitTable(std::shared_ptr<ITable> subTable) override;
+ std::shared_ptr<ITable> GetTable() const override;
+
+ std::shared_ptr<ITable> m_table;
+
+private:
+ uint32_t m_channel;
+ Direction m_direction;
+ std::unique_ptr<MotorSafetyHelper> m_safetyHelper;
+ SimContinuousOutput* impl;
+ bool go_pos, go_neg;
+};
diff --git a/wpilibc/simulation/include/RobotBase.h b/wpilibc/simulation/include/RobotBase.h
new file mode 100644
index 0000000..a88dbb2
--- /dev/null
+++ b/wpilibc/simulation/include/RobotBase.h
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "Base.h"
+#include "DriverStation.h"
+#include "simulation/simTime.h"
+#include "simulation/MainNode.h"
+
+#define START_ROBOT_CLASS(_ClassName_) \
+ int main() \
+ { \
+ (new _ClassName_())->StartCompetition(); \
+ return 0; \
+ }
+
+/**
+ * Implement a Robot Program framework.
+ * The RobotBase class is intended to be subclassed by a user creating a robot program.
+ * Overridden Autonomous() and OperatorControl() methods are called at the appropriate time
+ * as the match proceeds. In the current implementation, the Autonomous code will run to
+ * completion before the OperatorControl code could start. In the future the Autonomous code
+ * might be spawned as a task, then killed at the end of the Autonomous period.
+ */
+class RobotBase
+{
+ friend class RobotDeleter;
+public:
+ static RobotBase &getInstance();
+ static void setInstance(RobotBase* robot);
+
+ bool IsEnabled() const;
+ bool IsDisabled() const;
+ bool IsAutonomous() const;
+ bool IsOperatorControl() const;
+ bool IsTest() const;
+ virtual void StartCompetition() = 0;
+
+protected:
+ RobotBase();
+ virtual ~RobotBase() = default;
+
+ RobotBase(const RobotBase&) = delete;
+ RobotBase& operator=(const RobotBase&) = delete;
+
+ DriverStation &m_ds;
+
+private:
+ static RobotBase *m_instance;
+};
diff --git a/wpilibc/simulation/include/RobotDrive.h b/wpilibc/simulation/include/RobotDrive.h
new file mode 100644
index 0000000..a181e46
--- /dev/null
+++ b/wpilibc/simulation/include/RobotDrive.h
@@ -0,0 +1,112 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "ErrorBase.h"
+#include <stdlib.h>
+#include "MotorSafety.h"
+#include "MotorSafetyHelper.h"
+
+class SpeedController;
+class GenericHID;
+
+/**
+ * Utility class for handling Robot drive based on a definition of the motor configuration.
+ * The robot drive class handles basic driving for a robot. Currently, 2 and 4 motor standard
+ * drive trains are supported. In the future other drive types like swerve and meccanum might
+ * be implemented. Motor channel numbers are passed supplied on creation of the class. Those are
+ * used for either the Drive function (intended for hand created drive code, such as autonomous)
+ * or with the Tank/Arcade functions intended to be used for Operator Control driving.
+ */
+class RobotDrive : public MotorSafety, public ErrorBase
+{
+public:
+ enum MotorType
+ {
+ kFrontLeftMotor = 0,
+ kFrontRightMotor = 1,
+ kRearLeftMotor = 2,
+ kRearRightMotor = 3
+ };
+
+ RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel);
+ RobotDrive(uint32_t frontLeftMotorChannel, uint32_t rearLeftMotorChannel,
+ uint32_t frontRightMotorChannel, uint32_t rearRightMotorChannel);
+ RobotDrive(SpeedController *leftMotor, SpeedController *rightMotor);
+ RobotDrive(SpeedController &leftMotor, SpeedController &rightMotor);
+ RobotDrive(SpeedController *frontLeftMotor, SpeedController *rearLeftMotor,
+ SpeedController *frontRightMotor, SpeedController *rearRightMotor);
+ RobotDrive(SpeedController &frontLeftMotor, SpeedController &rearLeftMotor,
+ SpeedController &frontRightMotor, SpeedController &rearRightMotor);
+ virtual ~RobotDrive();
+
+ RobotDrive(const RobotDrive&) = delete;
+ RobotDrive& operator=(const RobotDrive&) = delete;
+
+ void Drive(float outputMagnitude, float curve);
+ void TankDrive(GenericHID *leftStick, GenericHID *rightStick, bool squaredInputs = true);
+ void TankDrive(GenericHID &leftStick, GenericHID &rightStick, bool squaredInputs = true);
+ void TankDrive(GenericHID *leftStick, uint32_t leftAxis, GenericHID *rightStick,
+ uint32_t rightAxis, bool squaredInputs = true);
+ void TankDrive(GenericHID &leftStick, uint32_t leftAxis, GenericHID &rightStick,
+ uint32_t rightAxis, bool squaredInputs = true);
+ void TankDrive(float leftValue, float rightValue, bool squaredInputs = true);
+ void ArcadeDrive(GenericHID *stick, bool squaredInputs = true);
+ void ArcadeDrive(GenericHID &stick, bool squaredInputs = true);
+ void ArcadeDrive(GenericHID *moveStick, uint32_t moveChannel, GenericHID *rotateStick,
+ uint32_t rotateChannel, bool squaredInputs = true);
+ void ArcadeDrive(GenericHID &moveStick, uint32_t moveChannel, GenericHID &rotateStick,
+ uint32_t rotateChannel, bool squaredInputs = true);
+ void ArcadeDrive(float moveValue, float rotateValue, bool squaredInputs = true);
+ void MecanumDrive_Cartesian(float x, float y, float rotation, float gyroAngle = 0.0);
+ void MecanumDrive_Polar(float magnitude, float direction, float rotation);
+ void HolonomicDrive(float magnitude, float direction, float rotation);
+ virtual void SetLeftRightMotorOutputs(float leftOutput, float rightOutput);
+ void SetInvertedMotor(MotorType motor, bool isInverted);
+ void SetSensitivity(float sensitivity);
+ void SetMaxOutput(double maxOutput);
+
+ void SetExpiration(float timeout) override;
+ float GetExpiration() const override;
+ bool IsAlive() const override;
+ void StopMotor() override;
+ bool IsSafetyEnabled() const override;
+ void SetSafetyEnabled(bool enabled) override;
+ void GetDescription(std::ostringstream& desc) const override;
+
+protected:
+ void InitRobotDrive();
+ float Limit(float num);
+ void Normalize(double *wheelSpeeds);
+ void RotateVector(double &x, double &y, double angle);
+
+ static const int32_t kMaxNumberOfMotors = 4;
+
+ int32_t m_invertedMotors[kMaxNumberOfMotors];
+ float m_sensitivity = 0.5;
+ double m_maxOutput = 1.0;
+ bool m_deleteSpeedControllers;
+ SpeedController *m_frontLeftMotor = nullptr;
+ SpeedController *m_frontRightMotor = nullptr;
+ SpeedController *m_rearLeftMotor = nullptr;
+ SpeedController *m_rearRightMotor = nullptr;
+ // FIXME: MotorSafetyHelper *m_safetyHelper;
+
+private:
+ int32_t GetNumMotors()
+ {
+ int motors = 0;
+ if (m_frontLeftMotor)
+ motors++;
+ if (m_frontRightMotor)
+ motors++;
+ if (m_rearLeftMotor)
+ motors++;
+ if (m_rearRightMotor)
+ motors++;
+ return motors;
+ }
+};
diff --git a/wpilibc/simulation/include/SafePWM.h b/wpilibc/simulation/include/SafePWM.h
new file mode 100644
index 0000000..577d853
--- /dev/null
+++ b/wpilibc/simulation/include/SafePWM.h
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "MotorSafety.h"
+#include "PWM.h"
+#include "MotorSafetyHelper.h"
+#include <memory>
+
+/**
+ * A safe version of the PWM class.
+ * It is safe because it implements the MotorSafety interface that provides timeouts
+ * in the event that the motor value is not updated before the expiration time.
+ * This delegates the actual work to a MotorSafetyHelper object that is used for all
+ * objects that implement MotorSafety.
+ */
+class SafePWM : public PWM, public MotorSafety
+{
+public:
+ explicit SafePWM(uint32_t channel);
+ virtual ~SafePWM() = default;
+
+ void SetExpiration(float timeout);
+ float GetExpiration() const;
+ bool IsAlive() const;
+ void StopMotor();
+ bool IsSafetyEnabled() const;
+ void SetSafetyEnabled(bool enabled);
+ void GetDescription(std::ostringstream& desc) const;
+
+ virtual void SetSpeed(float speed);
+private:
+ std::unique_ptr<MotorSafetyHelper> m_safetyHelper;
+};
diff --git a/wpilibc/simulation/include/SampleRobot.h b/wpilibc/simulation/include/SampleRobot.h
new file mode 100644
index 0000000..38cb37d
--- /dev/null
+++ b/wpilibc/simulation/include/SampleRobot.h
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "RobotBase.h"
+
+class SampleRobot : public RobotBase
+{
+public:
+ SampleRobot();
+ virtual ~SampleRobot() = default;
+ virtual void RobotInit();
+ virtual void Disabled();
+ virtual void Autonomous();
+ virtual void OperatorControl();
+ virtual void Test();
+ virtual void RobotMain();
+ void StartCompetition();
+
+private:
+ bool m_robotMainOverridden;
+};
diff --git a/wpilibc/simulation/include/Solenoid.h b/wpilibc/simulation/include/Solenoid.h
new file mode 100644
index 0000000..b91da6e
--- /dev/null
+++ b/wpilibc/simulation/include/Solenoid.h
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "simulation/SimContinuousOutput.h"
+#include "LiveWindow/LiveWindowSendable.h"
+#include "tables/ITableListener.h"
+
+#include <memory>
+
+/**
+ * Solenoid class for running high voltage Digital Output (PCM).
+ *
+ * The Solenoid class is typically used for pneumatics solenoids, but could be used
+ * for any device within the current spec of the PCM.
+ */
+class Solenoid : public LiveWindowSendable, public ITableListener
+{
+public:
+ explicit Solenoid(uint32_t channel);
+ Solenoid(uint8_t moduleNumber, uint32_t channel);
+ virtual ~Solenoid();
+ virtual void Set(bool on);
+ virtual bool Get() const;
+
+ void ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) override;
+ void UpdateTable() override;
+ void StartLiveWindowMode() override;
+ void StopLiveWindowMode() override;
+ std::string GetSmartDashboardType() const override;
+ void InitTable(std::shared_ptr<ITable> subTable) override;
+ std::shared_ptr<ITable> GetTable() const override;
+
+private:
+ SimContinuousOutput* m_impl;
+ bool m_on;
+
+ std::shared_ptr<ITable> m_table;
+};
diff --git a/wpilibc/simulation/include/SpeedController.h b/wpilibc/simulation/include/SpeedController.h
new file mode 100644
index 0000000..96b6b17
--- /dev/null
+++ b/wpilibc/simulation/include/SpeedController.h
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "PIDOutput.h"
+
+/**
+ * Interface for speed controlling devices.
+ */
+class SpeedController : public PIDOutput
+{
+public:
+ virtual ~SpeedController() = default;
+ /**
+ * Common interface for setting the speed of a speed controller.
+ *
+ * @param speed The speed to set. Value should be between -1.0 and 1.0.
+ * @param syncGroup The update group to add this Set() to, pending UpdateSyncGroup(). If 0, update immediately.
+ */
+ virtual void Set(float speed, uint8_t syncGroup = 0) = 0;
+ /**
+ * Common interface for getting the current set speed of a speed controller.
+ *
+ * @return The current set speed. Value is between -1.0 and 1.0.
+ */
+ virtual float Get() const = 0;
+ /**
+ * Common interface for disabling a motor.
+ */
+ virtual void Disable() = 0;
+};
diff --git a/wpilibc/simulation/include/Talon.h b/wpilibc/simulation/include/Talon.h
new file mode 100644
index 0000000..4b7794a
--- /dev/null
+++ b/wpilibc/simulation/include/Talon.h
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "SafePWM.h"
+#include "SpeedController.h"
+#include "PIDOutput.h"
+
+/**
+ * CTRE Talon Speed Controller
+ */
+class Talon : public SafePWM, public SpeedController
+{
+public:
+ explicit Talon(uint32_t channel);
+ virtual ~Talon() = default;
+ virtual void Set(float value, uint8_t syncGroup = 0);
+ virtual float Get() const;
+ virtual void Disable();
+
+ virtual void PIDWrite(float output) override;
+};
diff --git a/wpilibc/simulation/include/Victor.h b/wpilibc/simulation/include/Victor.h
new file mode 100644
index 0000000..b629bb4
--- /dev/null
+++ b/wpilibc/simulation/include/Victor.h
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "SafePWM.h"
+#include "SpeedController.h"
+#include "PIDOutput.h"
+
+/**
+ * IFI Victor Speed Controller
+ */
+class Victor : public SafePWM, public SpeedController
+{
+public:
+ explicit Victor(uint32_t channel);
+ virtual ~Victor() = default;
+ virtual void Set(float value, uint8_t syncGroup = 0);
+ virtual float Get() const;
+ virtual void Disable();
+
+ virtual void PIDWrite(float output) override;
+};
diff --git a/wpilibc/simulation/include/WPILib.h b/wpilibc/simulation/include/WPILib.h
new file mode 100644
index 0000000..b3477d4
--- /dev/null
+++ b/wpilibc/simulation/include/WPILib.h
@@ -0,0 +1,56 @@
+/*
+ * WPILIb.h
+ *
+ * Created on: May 29, 2014
+ * Author: alex
+ */
+#pragma once
+
+#define SIMULATION "gazebo"
+
+#include "string.h"
+#include <iostream>
+
+#include "Buttons/Trigger.h"
+#include "Buttons/Button.h"
+#include "Buttons/InternalButton.h"
+#include "Buttons/JoystickButton.h"
+#include "Buttons/NetworkButton.h"
+
+#include "Commands/Command.h"
+#include "Commands/CommandGroup.h"
+#include "Commands/PIDCommand.h"
+#include "Commands/PIDSubsystem.h"
+#include "Commands/PrintCommand.h"
+#include "Commands/Scheduler.h"
+#include "Commands/StartCommand.h"
+#include "Commands/Subsystem.h"
+#include "Commands/WaitCommand.h"
+#include "Commands/WaitForChildren.h"
+#include "Commands/WaitUntilCommand.h"
+
+#include "SmartDashboard/SendableChooser.h"
+#include "SmartDashboard/SmartDashboard.h"
+
+#include "RobotBase.h"
+#include "SampleRobot.h"
+#include "IterativeRobot.h"
+#include "SpeedController.h"
+#include "Talon.h"
+#include "Victor.h"
+#include "Jaguar.h"
+#include "Solenoid.h"
+#include "DoubleSolenoid.h"
+#include "interfaces/Potentiometer.h"
+#include "AnalogInput.h"
+#include "AnalogPotentiometer.h"
+#include "Counter.h"
+#include "DigitalInput.h"
+#include "Encoder.h"
+#include "Gyro.h"
+#include "GenericHID.h"
+#include "Joystick.h"
+#include "PIDController.h"
+#include "RobotDrive.h"
+#include "LiveWindow/LiveWindow.h"
+
diff --git a/wpilibc/simulation/include/simulation/MainNode.h b/wpilibc/simulation/include/simulation/MainNode.h
new file mode 100644
index 0000000..a76ef50
--- /dev/null
+++ b/wpilibc/simulation/include/simulation/MainNode.h
@@ -0,0 +1,49 @@
+
+#ifndef _SIM_MAIN_NODE_H
+#define _SIM_MAIN_NODE_H
+
+#include "simulation/gz_msgs/msgs.h"
+#include <gazebo/transport/transport.hh>
+#include <gazebo/gazebo_client.hh>
+
+using namespace gazebo;
+
+class MainNode {
+public:
+ static MainNode* GetInstance() {
+ static MainNode instance;
+ return &instance;
+ }
+
+ template<typename M>
+ static transport::PublisherPtr Advertise(const std::string &topic,
+ unsigned int _queueLimit = 10,
+ bool _latch = false) {
+ return GetInstance()->main->Advertise<M>(topic, _queueLimit, _latch);
+ }
+
+ template<typename M, typename T>
+ static transport::SubscriberPtr Subscribe(const std::string &topic,
+ void(T::*fp)(const boost::shared_ptr<M const> &), T *obj,
+ bool _latching = false) {
+ return GetInstance()->main->Subscribe(topic, fp, obj, _latching);
+ }
+
+ template<typename M>
+ static transport::SubscriberPtr Subscribe(const std::string &topic,
+ void(*fp)(const boost::shared_ptr<M const> &),
+ bool _latching = false) {
+ return GetInstance()->main->Subscribe(topic, fp, _latching);
+ }
+
+ transport::NodePtr main;
+private:
+ MainNode() {
+ gazebo::client::setup();
+ main = transport::NodePtr(new transport::Node());
+ main->Init("frc");
+ gazebo::transport::run();
+ }
+};
+
+#endif
diff --git a/wpilibc/simulation/include/simulation/SimContinuousOutput.h b/wpilibc/simulation/include/simulation/SimContinuousOutput.h
new file mode 100644
index 0000000..06a28e9
--- /dev/null
+++ b/wpilibc/simulation/include/simulation/SimContinuousOutput.h
@@ -0,0 +1,41 @@
+
+
+#ifndef _SIM_SPEED_CONTROLLER_H
+#define _SIM_SPEED_CONTROLLER_H
+
+#ifdef _WIN32
+ // Ensure that Winsock2.h is included before Windows.h, which can get
+ // pulled in by anybody (e.g., Boost).
+ #include <Winsock2.h>
+#endif
+
+#include <gazebo/transport/transport.hh>
+#include "SpeedController.h"
+
+using namespace gazebo;
+
+class SimContinuousOutput {
+private:
+ transport::PublisherPtr pub;
+ float speed;
+
+public:
+ SimContinuousOutput(std::string topic);
+
+ /**
+ * Set the output value.
+ *
+ * The value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value.
+ *
+ * @param value The value between -1.0 and 1.0 to set.
+ */
+ void Set(float value);
+
+ /**
+ * @return The most recently set value.
+ */
+ float Get();
+};
+
+#endif
diff --git a/wpilibc/simulation/include/simulation/SimDigitalInput.h b/wpilibc/simulation/include/simulation/SimDigitalInput.h
new file mode 100644
index 0000000..c85c19f
--- /dev/null
+++ b/wpilibc/simulation/include/simulation/SimDigitalInput.h
@@ -0,0 +1,26 @@
+
+
+#ifndef _SIM_DIGITAL_INPUT_H
+#define _SIM_DIGITAL_INPUT_H
+
+#include "simulation/gz_msgs/msgs.h"
+#include <gazebo/transport/transport.hh>
+
+using namespace gazebo;
+
+class SimDigitalInput {
+public:
+ SimDigitalInput(std::string topic);
+
+ /**
+ * @return The value of the potentiometer.
+ */
+ bool Get();
+
+private:
+ bool value;
+ transport::SubscriberPtr sub;
+ void callback(const msgs::ConstBoolPtr &msg);
+};
+
+#endif
diff --git a/wpilibc/simulation/include/simulation/SimEncoder.h b/wpilibc/simulation/include/simulation/SimEncoder.h
new file mode 100644
index 0000000..9f37723
--- /dev/null
+++ b/wpilibc/simulation/include/simulation/SimEncoder.h
@@ -0,0 +1,32 @@
+
+
+#ifndef _SIM_ENCODER_H
+#define _SIM_ENCODER_H
+
+#include "simulation/gz_msgs/msgs.h"
+#include <gazebo/transport/transport.hh>
+#include <gazebo/common/Time.hh>
+
+using namespace gazebo;
+
+class SimEncoder {
+public:
+ SimEncoder(std::string topic);
+
+ void Reset();
+ void Start();
+ void Stop();
+ double GetPosition();
+ double GetVelocity();
+
+private:
+ void sendCommand(std::string cmd);
+
+ double position, velocity;
+ transport::SubscriberPtr posSub, velSub;
+ transport::PublisherPtr commandPub;
+ void positionCallback(const msgs::ConstFloat64Ptr &msg);
+ void velocityCallback(const msgs::ConstFloat64Ptr &msg);
+};
+
+#endif
diff --git a/wpilibc/simulation/include/simulation/SimFloatInput.h b/wpilibc/simulation/include/simulation/SimFloatInput.h
new file mode 100644
index 0000000..6271b96
--- /dev/null
+++ b/wpilibc/simulation/include/simulation/SimFloatInput.h
@@ -0,0 +1,26 @@
+
+
+#ifndef _SIM_FLOAT_INPUT_H
+#define _SIM_FLOAT_INPUT_H
+
+#include "simulation/gz_msgs/msgs.h"
+#include <gazebo/transport/transport.hh>
+
+using namespace gazebo;
+
+class SimFloatInput {
+public:
+ SimFloatInput(std::string topic);
+
+ /**
+ * @return The value of the potentiometer.
+ */
+ double Get();
+
+private:
+ double value;
+ transport::SubscriberPtr sub;
+ void callback(const msgs::ConstFloat64Ptr &msg);
+};
+
+#endif
diff --git a/wpilibc/simulation/include/simulation/SimGyro.h b/wpilibc/simulation/include/simulation/SimGyro.h
new file mode 100644
index 0000000..fcb81f6
--- /dev/null
+++ b/wpilibc/simulation/include/simulation/SimGyro.h
@@ -0,0 +1,29 @@
+
+
+#ifndef _SIM_GYRO_H
+#define _SIM_GYRO_H
+
+#include "simulation/gz_msgs/msgs.h"
+#include <gazebo/transport/transport.hh>
+
+using namespace gazebo;
+
+class SimGyro {
+public:
+ SimGyro(std::string topic);
+
+ void Reset();
+ double GetAngle();
+ double GetVelocity();
+
+private:
+ void sendCommand(std::string cmd);
+
+ double position, velocity;
+ transport::SubscriberPtr posSub, velSub;
+ transport::PublisherPtr commandPub;
+ void positionCallback(const msgs::ConstFloat64Ptr &msg);
+ void velocityCallback(const msgs::ConstFloat64Ptr &msg);
+};
+
+#endif
diff --git a/wpilibc/simulation/include/simulation/simTime.h b/wpilibc/simulation/include/simulation/simTime.h
new file mode 100644
index 0000000..20fe0c5
--- /dev/null
+++ b/wpilibc/simulation/include/simulation/simTime.h
@@ -0,0 +1,18 @@
+#pragma once
+
+
+#ifdef _WIN32
+ // Ensure that Winsock2.h is included before Windows.h, which can get
+ // pulled in by anybody (e.g., Boost).
+ #include <Winsock2.h>
+#endif
+
+#include "simulation/SimFloatInput.h"
+
+#include <condition_variable>
+#include <mutex>
+
+namespace wpilib { namespace internal {
+ extern double simTime;
+ extern void time_callback(const msgs::ConstFloat64Ptr &msg);
+}}
diff --git a/wpilibc/simulation/src/AnalogInput.cpp b/wpilibc/simulation/src/AnalogInput.cpp
new file mode 100644
index 0000000..71c48c5
--- /dev/null
+++ b/wpilibc/simulation/src/AnalogInput.cpp
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogInput.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Construct an analog input.
+ *
+ * @param channel The channel number to represent.
+ */
+AnalogInput::AnalogInput(uint32_t channel)
+{
+ m_channel = channel;
+ char buffer[50];
+ int n = sprintf(buffer, "analog/%d", channel);
+ m_impl = new SimFloatInput(buffer);
+
+ LiveWindow::GetInstance()->AddSensor("AnalogInput", channel, this);
+}
+
+/**
+ * Get a scaled sample straight from this channel.
+ * The value is scaled to units of Volts using the calibrated scaling data from GetLSBWeight() and GetOffset().
+ * @return A scaled sample straight from this channel.
+ */
+float AnalogInput::GetVoltage() const
+{
+ return m_impl->Get();
+}
+
+/**
+ * Get a scaled sample from the output of the oversample and average engine for this channel.
+ * The value is scaled to units of Volts using the calibrated scaling data from GetLSBWeight() and GetOffset().
+ * Using oversampling will cause this value to be higher resolution, but it will update more slowly.
+ * Using averaging will cause this value to be more stable, but it will update more slowly.
+ * @return A scaled sample from the output of the oversample and average engine for this channel.
+ */
+float AnalogInput::GetAverageVoltage() const
+{
+ return m_impl->Get();
+}
+
+/**
+ * Get the channel number.
+ * @return The channel number.
+ */
+uint32_t AnalogInput::GetChannel() const
+{
+ return m_channel;
+}
+
+/**
+ * Get the Average value for the PID Source base object.
+ *
+ * @return The average voltage.
+ */
+double AnalogInput::PIDGet()
+{
+ return GetAverageVoltage();
+}
+
+void AnalogInput::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", GetAverageVoltage());
+ }
+}
+
+void AnalogInput::StartLiveWindowMode() {
+
+}
+
+void AnalogInput::StopLiveWindowMode() {
+
+}
+
+std::string AnalogInput::GetSmartDashboardType() const {
+ return "Analog Input";
+}
+
+void AnalogInput::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> AnalogInput::GetTable() const {
+ return m_table;
+}
diff --git a/wpilibc/simulation/src/AnalogPotentiometer.cpp b/wpilibc/simulation/src/AnalogPotentiometer.cpp
new file mode 100644
index 0000000..fa312c1
--- /dev/null
+++ b/wpilibc/simulation/src/AnalogPotentiometer.cpp
@@ -0,0 +1,77 @@
+
+#include "AnalogPotentiometer.h"
+
+/**
+ * Common initialization code called by all constructors.
+ */
+void AnalogPotentiometer::initPot(AnalogInput *input, double scale, double offset) {
+ m_scale = scale;
+ m_offset = offset;
+ m_analog_input = input;
+}
+
+AnalogPotentiometer::AnalogPotentiometer(int channel, double scale, double offset) {
+ m_init_analog_input = true;
+ initPot(new AnalogInput(channel), scale, offset);
+}
+
+AnalogPotentiometer::AnalogPotentiometer(AnalogInput *input, double scale, double offset) {
+ m_init_analog_input = false;
+ initPot(input, scale, offset);
+}
+
+AnalogPotentiometer::AnalogPotentiometer(AnalogInput &input, double scale, double offset) {
+ m_init_analog_input = false;
+ initPot(&input, scale, offset);
+}
+
+AnalogPotentiometer::~AnalogPotentiometer() {
+ if(m_init_analog_input){
+ delete m_analog_input;
+ m_init_analog_input = false;
+ }
+}
+
+/**
+ * Get the current reading of the potentiomere.
+ *
+ * @return The current position of the potentiometer.
+ */
+double AnalogPotentiometer::Get() const {
+ return m_analog_input->GetVoltage() * m_scale + m_offset;
+}
+
+/**
+ * Implement the PIDSource interface.
+ *
+ * @return The current reading.
+ */
+double AnalogPotentiometer::PIDGet() {
+ return Get();
+}
+
+
+/**
+ * @return the Smart Dashboard Type
+ */
+std::string AnalogPotentiometer::GetSmartDashboardType() const {
+ return "Analog Input";
+}
+
+/**
+ * Live Window code, only does anything if live window is activated.
+ */
+void AnalogPotentiometer::InitTable(std::shared_ptr<ITable> subtable) {
+ m_table = subtable;
+ UpdateTable();
+}
+
+void AnalogPotentiometer::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", Get());
+ }
+}
+
+std::shared_ptr<ITable> AnalogPotentiometer::GetTable() const {
+ return m_table;
+}
diff --git a/wpilibc/simulation/src/DigitalInput.cpp b/wpilibc/simulation/src/DigitalInput.cpp
new file mode 100644
index 0000000..e7ae36d
--- /dev/null
+++ b/wpilibc/simulation/src/DigitalInput.cpp
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "DigitalInput.h"
+#include "WPIErrors.h"
+
+/**
+ * Create an instance of a Digital Input class.
+ * Creates a digital input given a channel and uses the default module.
+ *
+ * @param channel The digital channel (1..14).
+ */
+DigitalInput::DigitalInput(uint32_t channel)
+{
+ char buf[64];
+ m_channel = channel;
+ int n = sprintf(buf, "dio/%d", channel);
+ m_impl = new SimDigitalInput(buf);
+}
+
+/*
+ * Get the value from a digital input channel.
+ * Retrieve the value of a single digital input channel from the FPGA.
+ */
+uint32_t DigitalInput::Get() const
+{
+ return m_impl->Get();
+}
+
+/**
+ * @return The GPIO channel number that this object represents.
+ */
+uint32_t DigitalInput::GetChannel() const
+{
+ return m_channel;
+}
+
+void DigitalInput::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutBoolean("Value", Get());
+ }
+}
+
+void DigitalInput::StartLiveWindowMode() {
+
+}
+
+void DigitalInput::StopLiveWindowMode() {
+
+}
+
+std::string DigitalInput::GetSmartDashboardType() const {
+ return "DigitalInput";
+}
+
+void DigitalInput::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> DigitalInput::GetTable() const {
+ return m_table;
+}
diff --git a/wpilibc/simulation/src/DoubleSolenoid.cpp b/wpilibc/simulation/src/DoubleSolenoid.cpp
new file mode 100644
index 0000000..d1d0917
--- /dev/null
+++ b/wpilibc/simulation/src/DoubleSolenoid.cpp
@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "DoubleSolenoid.h"
+#include "WPIErrors.h"
+#include <string.h>
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Constructor.
+ *
+ * @param forwardChannel The forward channel on the module to control.
+ * @param reverseChannel The reverse channel on the module to control.
+ */
+DoubleSolenoid::DoubleSolenoid(uint32_t forwardChannel, uint32_t reverseChannel)
+ : DoubleSolenoid(1, forwardChannel, reverseChannel) {}
+
+/**
+ * Constructor.
+ *
+ * @param moduleNumber The solenoid module (1 or 2).
+ * @param forwardChannel The forward channel on the module to control.
+ * @param reverseChannel The reverse channel on the module to control.
+ */
+DoubleSolenoid::DoubleSolenoid(uint8_t moduleNumber, uint32_t forwardChannel, uint32_t reverseChannel)
+{
+ m_reversed = false;
+ if (reverseChannel < forwardChannel) { // Swap ports to get the right address
+ int channel = reverseChannel;
+ reverseChannel = forwardChannel;
+ forwardChannel = channel;
+ m_reversed = true;
+ }
+ char buffer[50];
+ int n = sprintf(buffer, "pneumatic/%d/%d/%d/%d", moduleNumber,
+ forwardChannel, moduleNumber, reverseChannel);
+ m_impl = new SimContinuousOutput(buffer);
+
+ LiveWindow::GetInstance()->AddActuator("DoubleSolenoid", moduleNumber,
+ forwardChannel, this);
+}
+
+DoubleSolenoid::~DoubleSolenoid() {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Set the value of a solenoid.
+ *
+ * @param value Move the solenoid to forward, reverse, or don't move it.
+ */
+void DoubleSolenoid::Set(Value value)
+{
+ m_value = value;
+ switch(value)
+ {
+ case kOff:
+ m_impl->Set(0);
+ break;
+ case kForward:
+ m_impl->Set(m_reversed ? -1 : 1);
+ break;
+ case kReverse:
+ m_impl->Set(m_reversed ? 1 : -1);
+ break;
+ }
+}
+
+/**
+ * Read the current value of the solenoid.
+ *
+ * @return The current value of the solenoid.
+ */
+DoubleSolenoid::Value DoubleSolenoid::Get() const
+{
+ return m_value;
+}
+
+void DoubleSolenoid::ValueChanged(ITable *source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value,
+ bool isNew) {
+ if (!value->IsString()) return;
+ Value lvalue = kOff;
+ if (value->GetString() == "Forward")
+ lvalue = kForward;
+ else if (value->GetString() == "Reverse")
+ lvalue = kReverse;
+ Set(lvalue);
+}
+
+void DoubleSolenoid::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutString("Value", (Get() == kForward ? "Forward" : (Get() == kReverse ? "Reverse" : "Off")));
+ }
+}
+
+void DoubleSolenoid::StartLiveWindowMode() {
+ Set(kOff);
+ if (m_table != nullptr) {
+ m_table->AddTableListener("Value", this, true);
+ }
+}
+
+void DoubleSolenoid::StopLiveWindowMode() {
+ Set(kOff);
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string DoubleSolenoid::GetSmartDashboardType() const {
+ return "Double Solenoid";
+}
+
+void DoubleSolenoid::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> DoubleSolenoid::GetTable() const {
+ return m_table;
+}
diff --git a/wpilibc/simulation/src/DriverStation.cpp b/wpilibc/simulation/src/DriverStation.cpp
new file mode 100644
index 0000000..39802ab
--- /dev/null
+++ b/wpilibc/simulation/src/DriverStation.cpp
@@ -0,0 +1,518 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "DriverStation.h"
+#include "AnalogInput.h"
+#include "Timer.h"
+#include "NetworkCommunication/FRCComm.h"
+#include "MotorSafetyHelper.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+#include <string.h>
+#include "Log.hpp"
+
+// set the logging level
+TLogLevel dsLogLevel = logDEBUG;
+const double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
+
+#define DS_LOG(level) \
+ if (level > dsLogLevel) ; \
+ else Log().Get(level)
+
+const uint32_t DriverStation::kJoystickPorts;
+
+/**
+ * DriverStation constructor.
+ *
+ * This is only called once the first time GetInstance() is called
+ */
+DriverStation::DriverStation() {
+ // All joysticks should default to having zero axes, povs and buttons, so
+ // uninitialized memory doesn't get sent to speed controllers.
+ for (unsigned int i = 0; i < kJoystickPorts; i++) {
+ m_joystickAxes[i].count = 0;
+ m_joystickPOVs[i].count = 0;
+ m_joystickButtons[i].count = 0;
+ m_joystickDescriptor[i].isXbox = 0;
+ m_joystickDescriptor[i].type = -1;
+ m_joystickDescriptor[i].name[0] = '\0';
+ }
+ // Register that semaphore with the network communications task.
+ // It will signal when new packet data is available.
+ HALSetNewDataSem(m_packetDataAvailableCond.native_handle());
+
+ AddToSingletonList();
+
+}
+
+void DriverStation::Run() {
+ int period = 0;
+ while (true) {
+ {
+ std::unique_lock<priority_mutex> lock(m_packetDataAvailableMutex);
+ m_packetDataAvailableCond.wait(lock);
+ }
+ GetData();
+ m_waitForDataCond.notify_all();
+
+ if (++period >= 4) {
+ MotorSafetyHelper::CheckMotors();
+ period = 0;
+ }
+ if (m_userInDisabled) HALNetworkCommunicationObserveUserProgramDisabled();
+ if (m_userInAutonomous) HALNetworkCommunicationObserveUserProgramAutonomous();
+ if (m_userInTeleop) HALNetworkCommunicationObserveUserProgramTeleop();
+ if (m_userInTest) HALNetworkCommunicationObserveUserProgramTest();
+ }
+}
+
+/**
+ * Return a reference to the singleton DriverStation.
+ * @return Pointer to the DS instance
+ */
+DriverStation &DriverStation::GetInstance() {
+ static DriverStation instance;
+ return instance;
+}
+
+/**
+ * Copy data from the DS task for the user.
+ * If no new data exists, it will just be returned, otherwise
+ * the data will be copied from the DS polling loop.
+ */
+void DriverStation::GetData() {
+ // Get the status of all of the joysticks
+ for (uint8_t stick = 0; stick < kJoystickPorts; stick++) {
+ HALGetJoystickAxes(stick, &m_joystickAxes[stick]);
+ HALGetJoystickPOVs(stick, &m_joystickPOVs[stick]);
+ HALGetJoystickButtons(stick, &m_joystickButtons[stick]);
+ HALGetJoystickDescriptor(stick, &m_joystickDescriptor[stick]);
+ }
+ m_newControlData.give();
+}
+
+/**
+ * Read the battery voltage.
+ *
+ * @return The battery voltage in Volts.
+ */
+float DriverStation::GetBatteryVoltage() const {
+ int32_t status = 0;
+ float voltage = getVinVoltage(&status);
+ wpi_setErrorWithContext(status, "getVinVoltage");
+
+ return voltage;
+}
+
+/**
+ * Reports errors related to unplugged joysticks
+ * Throttles the errors so that they don't overwhelm the DS
+ */
+void DriverStation::ReportJoystickUnpluggedError(std::string message) {
+ double currentTime = Timer::GetFPGATimestamp();
+ if (currentTime > m_nextMessageTime) {
+ ReportError(message);
+ m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+ }
+}
+
+/**
+ * Returns the number of axes on a given joystick port
+ *
+ * @param stick The joystick port number
+ * @return The number of axes on the indicated joystick
+ */
+int DriverStation::GetStickAxisCount(uint32_t stick) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0;
+ }
+ HALJoystickAxes joystickAxes;
+ HALGetJoystickAxes(stick, &joystickAxes);
+ return joystickAxes.count;
+}
+
+/**
+ * Returns the name of the joystick at the given port
+ *
+ * @param stick The joystick port number
+ * @return The name of the joystick at the given port
+ */
+std::string DriverStation::GetJoystickName(uint32_t stick) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ }
+ std::string retVal(m_joystickDescriptor[0].name);
+ return retVal;
+}
+
+/**
+ * Returns the type of joystick at a given port
+ *
+ * @param stick The joystick port number
+ * @return The HID type of joystick at the given port
+ */
+int DriverStation::GetJoystickType(uint32_t stick) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return -1;
+ }
+ return (int)m_joystickDescriptor[stick].type;
+}
+
+/**
+ * Returns a boolean indicating if the controller is an xbox controller.
+ *
+ * @param stick The joystick port number
+ * @return A boolean that is true if the controller is an xbox controller.
+ */
+bool DriverStation::GetJoystickIsXbox(uint32_t stick) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return false;
+ }
+ return (bool)m_joystickDescriptor[stick].isXbox;
+}
+
+/**
+ * Returns the types of Axes on a given joystick port
+ *
+ * @param stick The joystick port number and the target axis
+ * @return What type of axis the axis is reporting to be
+ */
+int DriverStation::GetJoystickAxisType(uint32_t stick, uint8_t axis) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return -1;
+ }
+ return m_joystickDescriptor[stick].axisTypes[axis];
+}
+
+/**
+ * Returns the number of POVs on a given joystick port
+ *
+ * @param stick The joystick port number
+ * @return The number of POVs on the indicated joystick
+ */
+int DriverStation::GetStickPOVCount(uint32_t stick) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0;
+ }
+ HALJoystickPOVs joystickPOVs;
+ HALGetJoystickPOVs(stick, &joystickPOVs);
+ return joystickPOVs.count;
+}
+
+/**
+ * Returns the number of buttons on a given joystick port
+ *
+ * @param stick The joystick port number
+ * @return The number of buttons on the indicated joystick
+ */
+int DriverStation::GetStickButtonCount(uint32_t stick) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0;
+ }
+ HALJoystickButtons joystickButtons;
+ HALGetJoystickButtons(stick, &joystickButtons);
+ return joystickButtons.count;
+}
+
+/**
+ * Get the value of the axis on a joystick.
+ * This depends on the mapping of the joystick connected to the specified port.
+ *
+ * @param stick The joystick to read.
+ * @param axis The analog axis value to read from the joystick.
+ * @return The value of the axis on the joystick.
+ */
+float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0;
+ }
+
+ if (axis >= m_joystickAxes[stick].count) {
+ if (axis >= kMaxJoystickAxes) {
+ wpi_setWPIError(BadJoystickAxis);
+ }
+ else {
+ ReportJoystickUnpluggedError(
+ "WARNING: Joystick Axis missing, check if all controllers are "
+ "plugged in\n");
+ }
+ return 0.0f;
+ }
+
+ int8_t value = m_joystickAxes[stick].axes[axis];
+
+ if (value < 0) {
+ return value / 128.0f;
+ } else {
+ return value / 127.0f;
+ }
+}
+
+/**
+ * Get the state of a POV on the joystick.
+ *
+ * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+ */
+int DriverStation::GetStickPOV(uint32_t stick, uint32_t pov) {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return -1;
+ }
+
+ if (pov >= m_joystickPOVs[stick].count) {
+ if (pov >= kMaxJoystickPOVs) {
+ wpi_setWPIError(BadJoystickAxis);
+ }
+ else {
+ ReportJoystickUnpluggedError(
+ "WARNING: Joystick POV missing, check if all controllers are plugged "
+ "in\n");
+ }
+ return -1;
+ }
+
+ return m_joystickPOVs[stick].povs[pov];
+}
+
+/**
+ * The state of the buttons on the joystick.
+ *
+ * @param stick The joystick to read.
+ * @return The state of the buttons on the joystick.
+ */
+uint32_t DriverStation::GetStickButtons(uint32_t stick) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0;
+ }
+
+ return m_joystickButtons[stick].buttons;
+}
+
+/**
+ * The state of one joystick button. Button indexes begin at 1.
+ *
+ * @param stick The joystick to read.
+ * @param button The button index, beginning at 1.
+ * @return The state of the joystick button.
+ */
+bool DriverStation::GetStickButton(uint32_t stick, uint8_t button) {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return false;
+ }
+
+ if (button > m_joystickButtons[stick].count) {
+ ReportJoystickUnpluggedError(
+ "WARNING: Joystick Button missing, check if all controllers are "
+ "plugged in\n");
+ return false;
+ }
+ if (button == 0) {
+ ReportJoystickUnpluggedError(
+ "ERROR: Button indexes begin at 1 in WPILib for C++ and Java");
+ return false;
+ }
+ return ((0x1 << (button - 1)) & m_joystickButtons[stick].buttons) != 0;
+}
+
+/**
+ * Check if the DS has enabled the robot
+ * @return True if the robot is enabled and the DS is connected
+ */
+bool DriverStation::IsEnabled() const {
+ HALControlWord controlWord;
+ memset(&controlWord, 0, sizeof(controlWord));
+ HALGetControlWord(&controlWord);
+ return controlWord.enabled && controlWord.dsAttached;
+}
+
+/**
+ * Check if the robot is disabled
+ * @return True if the robot is explicitly disabled or the DS is not connected
+ */
+bool DriverStation::IsDisabled() const {
+ HALControlWord controlWord;
+ memset(&controlWord, 0, sizeof(controlWord));
+ HALGetControlWord(&controlWord);
+ return !(controlWord.enabled && controlWord.dsAttached);
+}
+
+/**
+ * Check if the DS is commanding autonomous mode
+ * @return True if the robot is being commanded to be in autonomous mode
+ */
+bool DriverStation::IsAutonomous() const {
+ HALControlWord controlWord;
+ memset(&controlWord, 0, sizeof(controlWord));
+ HALGetControlWord(&controlWord);
+ return controlWord.autonomous;
+}
+
+/**
+ * Check if the DS is commanding teleop mode
+ * @return True if the robot is being commanded to be in teleop mode
+ */
+bool DriverStation::IsOperatorControl() const {
+ HALControlWord controlWord;
+ memset(&controlWord, 0, sizeof(controlWord));
+ HALGetControlWord(&controlWord);
+ return !(controlWord.autonomous || controlWord.test);
+}
+
+/**
+ * Check if the DS is commanding test mode
+ * @return True if the robot is being commanded to be in test mode
+ */
+bool DriverStation::IsTest() const {
+ HALControlWord controlWord;
+ HALGetControlWord(&controlWord);
+ return controlWord.test;
+}
+
+/**
+ * Check if the DS is attached
+ * @return True if the DS is connected to the robot
+ */
+bool DriverStation::IsDSAttached() const {
+ HALControlWord controlWord;
+ memset(&controlWord, 0, sizeof(controlWord));
+ HALGetControlWord(&controlWord);
+ return controlWord.dsAttached;
+}
+
+/**
+ * @return always true in simulation
+ */
+bool DriverStation::IsSysActive() const {
+ return true;
+}
+
+/**
+ * @return always false in simulation
+ */
+bool DriverStation::IsSysBrownedOut() const {
+ return false;
+}
+
+/**
+ * Has a new control packet from the driver station arrived since the last time
+ * this function was called?
+ * Warning: If you call this function from more than one place at the same time,
+ * you will not get the get the intended behaviour.
+ * @return True if the control data has been updated since the last call.
+ */
+bool DriverStation::IsNewControlData() const {
+ return m_newControlData.tryTake() == false;
+}
+
+/**
+ * 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 {
+ HALControlWord controlWord;
+ HALGetControlWord(&controlWord);
+ return controlWord.fmsAttached;
+}
+
+/**
+ * Return the alliance that the driver station says it is on.
+ * This could return kRed or kBlue
+ * @return The Alliance enum (kRed, kBlue or kInvalid)
+ */
+DriverStation::Alliance DriverStation::GetAlliance() const {
+ HALAllianceStationID allianceStationID;
+ HALGetAllianceStation(&allianceStationID);
+ switch (allianceStationID) {
+ case kHALAllianceStationID_red1:
+ case kHALAllianceStationID_red2:
+ case kHALAllianceStationID_red3:
+ return kRed;
+ case kHALAllianceStationID_blue1:
+ case kHALAllianceStationID_blue2:
+ case kHALAllianceStationID_blue3:
+ return kBlue;
+ default:
+ return kInvalid;
+ }
+}
+
+/**
+ * Return the driver station location on the field
+ * This could return 1, 2, or 3
+ * @return The location of the driver station (1-3, 0 for invalid)
+ */
+uint32_t DriverStation::GetLocation() const {
+ HALAllianceStationID allianceStationID;
+ HALGetAllianceStation(&allianceStationID);
+ switch (allianceStationID) {
+ case kHALAllianceStationID_red1:
+ case kHALAllianceStationID_blue1:
+ return 1;
+ case kHALAllianceStationID_red2:
+ case kHALAllianceStationID_blue2:
+ return 2;
+ case kHALAllianceStationID_red3:
+ case kHALAllianceStationID_blue3:
+ return 3;
+ default:
+ return 0;
+ }
+}
+
+/**
+ * Wait until a new packet comes from the driver station
+ * This blocks on a semaphore, so the waiting is efficient.
+ * This is a good way to delay processing until there is new driver station data
+ * to act on
+ */
+void DriverStation::WaitForData() {
+ std::unique_lock<priority_mutex> lock(m_waitForDataMutex);
+ m_waitForDataCond.wait(lock);
+}
+
+/**
+ * Return the approximate match time
+ * The FMS does not send an official match time to the robots, but does send an
+ * approximate match time.
+ * The value will count down the time remaining in the current period (auto or
+ * teleop).
+ * Warning: This is not an official time (so it cannot be used to dispute ref
+ * calls or guarantee that a function
+ * will trigger before the match ends)
+ * The Practice Match function of the DS approximates the behaviour seen on the
+ * field.
+ * @return Time remaining in current match period (auto or teleop)
+ */
+double DriverStation::GetMatchTime() const {
+ float matchTime;
+ HALGetMatchTime(&matchTime);
+ return (double)matchTime;
+}
+
+/**
+ * Report an error to the DriverStation messages window.
+ * The error is also printed to the program console.
+ */
+void DriverStation::ReportError(std::string error) {
+ std::cout << error << std::endl;
+
+ HALControlWord controlWord;
+ HALGetControlWord(&controlWord);
+ if (controlWord.dsAttached) {
+ HALSetErrorData(error.c_str(), error.size(), 0);
+ }
+}
diff --git a/wpilibc/simulation/src/Encoder.cpp b/wpilibc/simulation/src/Encoder.cpp
new file mode 100644
index 0000000..790868e
--- /dev/null
+++ b/wpilibc/simulation/src/Encoder.cpp
@@ -0,0 +1,364 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Encoder.h"
+#include "Resource.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Common initialization code for Encoders.
+ * This code allocates resources for Encoders and is common to all constructors.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param reverseDirection If true, counts down instead of up (this is all relative)
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+ * selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder
+ * spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then
+ * a counter object will be used and the returned value will either exactly match the spec'd count
+ * or be double (2x) the spec'd count.
+ */
+void Encoder::InitEncoder(int channelA, int channelB, bool reverseDirection, EncodingType encodingType)
+{
+ m_table = nullptr;
+ this->channelA = channelA;
+ this->channelB = channelB;
+ m_encodingType = encodingType;
+ m_encodingScale = encodingType == k4X ? 4
+ : encodingType == k2X ? 2
+ : 1;
+
+ int32_t index = 0;
+ m_distancePerPulse = 1.0;
+
+ LiveWindow::GetInstance()->AddSensor("Encoder", channelA, this);
+
+ if (channelB < channelA) { // Swap ports
+ int channel = channelB;
+ channelB = channelA;
+ channelA = channel;
+ m_reverseDirection = !reverseDirection;
+ } else {
+ m_reverseDirection = reverseDirection;
+ }
+ char buffer[50];
+ int n = sprintf(buffer, "dio/%d/%d", channelA, channelB);
+ impl = new SimEncoder(buffer);
+ impl->Start();
+}
+
+/**
+ * Encoder constructor.
+ * Construct a Encoder given a and b channels.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param aChannel The a channel digital input channel.
+ * @param bChannel The b channel digital input channel.
+ * @param reverseDirection represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+ * selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder
+ * spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then
+ * a counter object will be used and the returned value will either exactly match the spec'd count
+ * or be double (2x) the spec'd count.
+ */
+Encoder::Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection, EncodingType encodingType)
+{
+ InitEncoder(aChannel, bChannel, reverseDirection, encodingType);
+}
+
+/**
+ * Encoder constructor.
+ * Construct a Encoder given a and b channels as digital inputs. This is used in the case
+ * where the digital inputs are shared. The Encoder class will not allocate the digital inputs
+ * and assume that they already are counted.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param aSource The source that should be used for the a channel.
+ * @param bSource the source that should be used for the b channel.
+ * @param reverseDirection represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+ * selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder
+ * spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then
+ * a counter object will be used and the returned value will either exactly match the spec'd count
+ * or be double (2x) the spec'd count.
+ */
+/* 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 represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+ * selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder
+ * spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then
+ * a counter object will be used and the returned value will either exactly match the spec'd count
+ * or be double (2x) the spec'd count.
+ */
+/*// TODO: [Not Supported] Encoder::Encoder(DigitalSource &aSource, DigitalSource &bSource, bool reverseDirection, EncodingType encodingType) :
+ m_encoder(nullptr),
+ m_counter(nullptr)
+{
+ m_aSource = &aSource;
+ m_bSource = &bSource;
+ m_allocatedASource = false;
+ m_allocatedBSource = false;
+ InitEncoder(reverseDirection, encodingType);
+ }*/
+
+/**
+ * Reset the Encoder distance to zero.
+ * Resets the current count to zero on the encoder.
+ */
+void Encoder::Reset()
+{
+ impl->Reset();
+}
+
+/**
+ * Determine if the encoder is stopped.
+ * Using the MaxPeriod value, a boolean is returned that is true if the encoder is considered
+ * stopped and false if it is still moving. A stopped encoder is one where the most recent pulse
+ * width exceeds the MaxPeriod.
+ * @return True if the encoder is considered stopped.
+ */
+bool Encoder::GetStopped() const
+{
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * The last direction the encoder value changed.
+ * @return The last direction the encoder value changed.
+ */
+bool Encoder::GetDirection() const
+{
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * The scale needed to convert a raw counter value into a number of encoder pulses.
+ */
+double Encoder::DecodingScaleFactor() const
+{
+ switch (m_encodingType)
+ {
+ case k1X:
+ return 1.0;
+ case k2X:
+ return 0.5;
+ case k4X:
+ return 0.25;
+ default:
+ return 0.0;
+ }
+}
+
+/**
+ * The encoding scale factor 1x, 2x, or 4x, per the requested encodingType.
+ * Used to divide raw edge counts down to spec'd counts.
+ */
+int32_t Encoder::GetEncodingScale() const { return m_encodingScale; }
+
+/**
+ * Gets the raw value from the encoder.
+ * The raw value is the actual count unscaled by the 1x, 2x, or 4x scale
+ * factor.
+ * @return Current raw count from the encoder
+ */
+int32_t Encoder::GetRaw() const
+{
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Gets the current count.
+ * Returns the current count on the Encoder.
+ * This method compensates for the decoding type.
+ *
+ * @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale factor.
+ */
+int32_t Encoder::Get() const
+{
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Returns the period of the most recent pulse.
+ * Returns the period of the most recent Encoder pulse in seconds.
+ * This method compenstates for the decoding type.
+ *
+ * @deprecated Use GetRate() in favor of this method. This returns unscaled periods and GetRate() scales using value from SetDistancePerPulse().
+ *
+ * @return Period in seconds of the most recent pulse.
+ */
+double Encoder::GetPeriod() const
+{
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Sets the maximum period for stopped detection.
+ * Sets the value that represents the maximum period of the Encoder before it will assume
+ * that the attached device is stopped. This timeout allows users to determine if the wheels or
+ * other shaft has stopped rotating.
+ * This method compensates for the decoding type.
+ *
+ * @deprecated Use SetMinRate() in favor of this method. This takes unscaled periods and SetMinRate() scales using value from SetDistancePerPulse().
+ *
+ * @param maxPeriod The maximum time between rising and falling edges before the FPGA will
+ * report the device stopped. This is expressed in seconds.
+ */
+void Encoder::SetMaxPeriod(double maxPeriod)
+{
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Get the distance the robot has driven since the last reset.
+ *
+ * @return The distance driven since the last reset as scaled by the value from SetDistancePerPulse().
+ */
+double Encoder::GetDistance() const
+{
+ return m_distancePerPulse * impl->GetPosition();
+}
+
+/**
+ * Get the current rate of the encoder.
+ * Units are distance per second as scaled by the value from SetDistancePerPulse().
+ *
+ * @return The current rate of the encoder.
+ */
+double Encoder::GetRate() const
+{
+ return m_distancePerPulse * impl->GetVelocity();
+}
+
+/**
+ * Set the minimum rate of the device before the hardware reports it stopped.
+ *
+ * @param minRate The minimum rate. The units are in distance per second as scaled by the value from SetDistancePerPulse().
+ */
+void Encoder::SetMinRate(double minRate)
+{
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Set the distance per pulse for this encoder.
+ * This sets the multiplier used to determine the distance driven based on the count value
+ * from the encoder.
+ * Do not include the decoding type in this scale. The library already compensates for the decoding type.
+ * Set this value based on the encoder's rated Pulses per Revolution and
+ * factor in gearing reductions following the encoder shaft.
+ * This distance can be in any units you like, linear or angular.
+ *
+ * @param distancePerPulse The scale factor that will be used to convert pulses to useful units.
+ */
+void Encoder::SetDistancePerPulse(double distancePerPulse)
+{
+ if (m_reverseDirection) {
+ m_distancePerPulse = -distancePerPulse;
+ } else {
+ m_distancePerPulse = distancePerPulse;
+ }
+}
+
+/**
+ * Set the direction sensing for this encoder.
+ * This sets the direction sensing on the encoder so that it could count in the correct
+ * software direction regardless of the mounting.
+ * @param reverseDirection true if the encoder direction should be reversed
+ */
+void Encoder::SetReverseDirection(bool reverseDirection)
+{
+ throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Set which parameter of the encoder you are using as a process control variable.
+ *
+ * @param pidSource An enum to select the parameter.
+ */
+void Encoder::SetPIDSourceType(PIDSourceType pidSource)
+{
+ m_pidSource = pidSource;
+}
+
+/**
+ * Implement the PIDSource interface.
+ *
+ * @return The current value of the selected source parameter.
+ */
+double Encoder::PIDGet()
+{
+ switch (m_pidSource)
+ {
+ case PIDSourceType::kDisplacement:
+ return GetDistance();
+ case PIDSourceType::kRate:
+ return GetRate();
+ default:
+ return 0.0;
+ }
+}
+
+void Encoder::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Speed", GetRate());
+ m_table->PutNumber("Distance", GetDistance());
+ m_table->PutNumber("Distance per Tick", m_reverseDirection ? -m_distancePerPulse : m_distancePerPulse);
+ }
+}
+
+void Encoder::StartLiveWindowMode() {
+
+}
+
+void Encoder::StopLiveWindowMode() {
+
+}
+
+std::string Encoder::GetSmartDashboardType() const {
+ if (m_encodingType == k4X)
+ return "Quadrature Encoder";
+ else
+ return "Encoder";
+}
+
+void Encoder::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> Encoder::GetTable() const {
+ return m_table;
+}
diff --git a/wpilibc/simulation/src/Gyro.cpp b/wpilibc/simulation/src/Gyro.cpp
new file mode 100644
index 0000000..a28bb9b
--- /dev/null
+++ b/wpilibc/simulation/src/Gyro.cpp
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Gyro.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+const uint32_t Gyro::kOversampleBits = 10;
+const uint32_t Gyro::kAverageBits = 0;
+const float Gyro::kSamplesPerSecond = 50.0;
+const float Gyro::kCalibrationSampleTime = 5.0;
+const float Gyro::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 Gyro::InitGyro(int channel)
+{
+ SetPIDSourceType(PIDSourceType::kDisplacement);
+
+ char buffer[50];
+ int n = sprintf(buffer, "analog/%d", channel);
+ impl = new SimGyro(buffer);
+
+ LiveWindow::GetInstance()->AddSensor("Gyro", channel, this);
+}
+
+/**
+ * Gyro constructor with only a channel..
+ *
+ * @param channel The analog channel the gyro is connected to.
+ */
+Gyro::Gyro(uint32_t channel)
+{
+ InitGyro(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 Gyro::Reset()
+{
+ impl->Reset();
+}
+
+/**
+ * Return the actual angle in degrees that the robot is currently facing.
+ *
+ * The angle is based on the current accumulator value corrected by the oversampling rate, the
+ * gyro type and the A/D calibration values.
+ * The angle is continuous, that is can go beyond 360 degrees. This make algorithms that wouldn't
+ * want to see a discontinuity in the gyro output as it sweeps past 0 on the second time around.
+ *
+ * @return the current heading of the robot in degrees. This heading is based on integration
+ * of the returned rate from the gyro.
+ */
+float Gyro::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 Gyro::GetRate() const
+{
+ return impl->GetVelocity();
+}
+
+void Gyro::SetPIDSourceType(PIDSourceType pidSource)
+{
+ m_pidSource = pidSource;
+}
+
+/**
+ * Get the angle in degrees for the PIDSource base object.
+ *
+ * @return The angle in degrees.
+ */
+double Gyro::PIDGet()
+{
+ switch(GetPIDSourceType()){
+ case PIDSourceType::kRate:
+ return GetRate();
+ case PIDSourceType::kDisplacement:
+ return GetAngle();
+ default:
+ return 0;
+ }
+}
+
+void Gyro::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", GetAngle());
+ }
+}
+
+void Gyro::StartLiveWindowMode() {
+
+}
+
+void Gyro::StopLiveWindowMode() {
+
+}
+
+std::string Gyro::GetSmartDashboardType() const {
+ return "Gyro";
+}
+
+void Gyro::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> Gyro::GetTable() const {
+ return m_table;
+}
diff --git a/wpilibc/simulation/src/IterativeRobot.cpp b/wpilibc/simulation/src/IterativeRobot.cpp
new file mode 100644
index 0000000..f2511df
--- /dev/null
+++ b/wpilibc/simulation/src/IterativeRobot.cpp
@@ -0,0 +1,312 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#include "IterativeRobot.h"
+
+#include "DriverStation.h"
+#include "SmartDashboard/SmartDashboard.h"
+#include "LiveWindow/LiveWindow.h"
+#include "networktables/NetworkTable.h"
+
+//not sure what this is used for yet.
+#ifdef _UNIX
+ #include <unistd.h>
+#endif
+
+const double IterativeRobot::kDefaultPeriod = 0;
+
+/**
+ * Set the period for the periodic functions.
+ *
+ * @param period The period of the periodic function calls. 0.0 means sync to driver station control data.
+ */
+void IterativeRobot::SetPeriod(double period)
+{
+ if (period > 0.0)
+ {
+ // Not syncing with the DS, so start the timer for the main loop
+ m_mainLoopTimer.Reset();
+ m_mainLoopTimer.Start();
+ }
+ else
+ {
+ // Syncing with the DS, don't need the timer
+ m_mainLoopTimer.Stop();
+ }
+ m_period = period;
+}
+
+/**
+ * Get the period for the periodic functions.
+ * Returns 0.0 if configured to syncronize with DS control data packets.
+ * @return Period of the periodic function calls
+ */
+double IterativeRobot::GetPeriod()
+{
+ return m_period;
+}
+
+/**
+ * Get the number of loops per second for the IterativeRobot
+ * @return Frequency of the periodic function calls
+ */
+double IterativeRobot::GetLoopsPerSec()
+{
+ // If syncing to the driver station, we don't know the rate,
+ // so guess something close.
+ if (m_period <= 0.0)
+ return 50.0;
+ return 1.0 / m_period;
+}
+
+/**
+ * Provide an alternate "main loop" via StartCompetition().
+ *
+ * This specific StartCompetition() implements "main loop" behavior like that of the FRC
+ * control system in 2008 and earlier, with a primary (slow) loop that is
+ * called periodically, and a "fast loop" (a.k.a. "spin loop") that is
+ * called as fast as possible with no delay between calls.
+ */
+void IterativeRobot::StartCompetition()
+{
+ LiveWindow *lw = LiveWindow::GetInstance();
+ // first and one-time initialization
+ SmartDashboard::init();
+ NetworkTable::GetTable("LiveWindow")->GetSubTable("~STATUS~")->PutBoolean("LW Enabled", false);
+ RobotInit();
+
+ // loop forever, calling the appropriate mode-dependent function
+ lw->SetEnabled(false);
+ while (true)
+ {
+ // Call the appropriate function depending upon the current robot mode
+ if (IsDisabled())
+ {
+ // call DisabledInit() if we are now just entering disabled mode from
+ // either a different mode or from power-on
+ if(!m_disabledInitialized)
+ {
+ lw->SetEnabled(false);
+ DisabledInit();
+ m_disabledInitialized = true;
+ // reset the initialization flags for the other modes
+ m_autonomousInitialized = false;
+ m_teleopInitialized = false;
+ m_testInitialized = false;
+ }
+ if (NextPeriodReady())
+ {
+ // TODO: HALNetworkCommunicationObserveUserProgramDisabled();
+ DisabledPeriodic();
+ }
+ }
+ else if (IsAutonomous())
+ {
+ // call AutonomousInit() if we are now just entering autonomous mode from
+ // either a different mode or from power-on
+ if(!m_autonomousInitialized)
+ {
+ lw->SetEnabled(false);
+ AutonomousInit();
+ m_autonomousInitialized = true;
+ // reset the initialization flags for the other modes
+ m_disabledInitialized = false;
+ m_teleopInitialized = false;
+ m_testInitialized = false;
+ }
+ if (NextPeriodReady())
+ {
+ // TODO: HALNetworkCommunicationObserveUserProgramAutonomous();
+ AutonomousPeriodic();
+ }
+ }
+ else if (IsTest())
+ {
+ // call TestInit() if we are now just entering test mode from
+ // either a different mode or from power-on
+ if(!m_testInitialized)
+ {
+ lw->SetEnabled(true);
+ TestInit();
+ m_testInitialized = true;
+ // reset the initialization flags for the other modes
+ m_disabledInitialized = false;
+ m_autonomousInitialized = false;
+ m_teleopInitialized = false;
+ }
+ if (NextPeriodReady())
+ {
+ // TODO: HALNetworkCommunicationObserveUserProgramTest();
+ TestPeriodic();
+ }
+ }
+ else
+ {
+ // call TeleopInit() if we are now just entering teleop mode from
+ // either a different mode or from power-on
+ if(!m_teleopInitialized)
+ {
+ lw->SetEnabled(false);
+ TeleopInit();
+ m_teleopInitialized = true;
+ // reset the initialization flags for the other modes
+ m_disabledInitialized = false;
+ m_autonomousInitialized = false;
+ m_testInitialized = false;
+ Scheduler::GetInstance()->SetEnabled(true);
+ }
+ if (NextPeriodReady())
+ {
+ // TODO: HALNetworkCommunicationObserveUserProgramTeleop();
+ TeleopPeriodic();
+ }
+ }
+ // wait for driver station data so the loop doesn't hog the CPU
+ m_ds.WaitForData();
+ }
+}
+
+/**
+ * Determine if the periodic functions should be called.
+ *
+ * If m_period > 0.0, call the periodic function every m_period as compared
+ * to Timer.Get(). If m_period == 0.0, call the periodic functions whenever
+ * a packet is received from the Driver Station, or about every 20ms.
+ *
+ * @todo Decide what this should do if it slips more than one cycle.
+ */
+
+bool IterativeRobot::NextPeriodReady()
+{
+ if (m_period > 0.0)
+ {
+ return m_mainLoopTimer.HasPeriodPassed(m_period);
+ }
+ else
+ {
+ // XXX: BROKEN! return m_ds->IsNewControlData();
+ }
+ return true;
+}
+
+/**
+ * Robot-wide initialization code should go here.
+ *
+ * Users should override this method for default Robot-wide initialization which will
+ * be called when the robot is first powered on. It will be called exactly 1 time.
+ */
+void IterativeRobot::RobotInit()
+{
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for disabled mode should go here.
+ *
+ * Users should override this method for initialization code which will be called each time
+ * the robot enters disabled mode.
+ */
+void IterativeRobot::DisabledInit()
+{
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for autonomous mode should go here.
+ *
+ * Users should override this method for initialization code which will be called each time
+ * the robot enters autonomous mode.
+ */
+void IterativeRobot::AutonomousInit()
+{
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for teleop mode should go here.
+ *
+ * Users should override this method for initialization code which will be called each time
+ * the robot enters teleop mode.
+ */
+void IterativeRobot::TeleopInit()
+{
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for test mode should go here.
+ *
+ * Users should override this method for initialization code which will be called each time
+ * the robot enters test mode.
+ */
+void IterativeRobot::TestInit()
+{
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Periodic code for disabled mode should go here.
+ *
+ * Users should override this method for code which will be called periodically at a regular
+ * rate while the robot is in disabled mode.
+ */
+void IterativeRobot::DisabledPeriodic()
+{
+ static bool firstRun = true;
+ if (firstRun)
+ {
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+}
+
+/**
+ * Periodic code for autonomous mode should go here.
+ *
+ * Users should override this method for code which will be called periodically at a regular
+ * rate while the robot is in autonomous mode.
+ */
+void IterativeRobot::AutonomousPeriodic()
+{
+ static bool firstRun = true;
+ if (firstRun)
+ {
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+}
+
+/**
+ * Periodic code for teleop mode should go here.
+ *
+ * Users should override this method for code which will be called periodically at a regular
+ * rate while the robot is in teleop mode.
+ */
+void IterativeRobot::TeleopPeriodic()
+{
+ static bool firstRun = true;
+ if (firstRun)
+ {
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+}
+
+/**
+ * Periodic code for test mode should go here.
+ *
+ * Users should override this method for code which will be called periodically at a regular
+ * rate while the robot is in test mode.
+ */
+void IterativeRobot::TestPeriodic()
+{
+ static bool firstRun = true;
+ if (firstRun)
+ {
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+}
+
diff --git a/wpilibc/simulation/src/Jaguar.cpp b/wpilibc/simulation/src/Jaguar.cpp
new file mode 100644
index 0000000..fab109d
--- /dev/null
+++ b/wpilibc/simulation/src/Jaguar.cpp
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+
+#include "Jaguar.h"
+//#include "NetworkCommunication/UsageReporting.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * @param channel The PWM channel that the Jaguar is attached to.
+ */
+Jaguar::Jaguar(uint32_t channel) : SafePWM(channel)
+{
+ /*
+ * Input profile defined by Luminary Micro.
+ *
+ * Full reverse ranges from 0.671325ms to 0.6972211ms
+ * Proportional reverse ranges from 0.6972211ms to 1.4482078ms
+ * Neutral ranges from 1.4482078ms to 1.5517922ms
+ * Proportional forward ranges from 1.5517922ms to 2.3027789ms
+ * Full forward ranges from 2.3027789ms to 2.328675ms
+ */
+ SetBounds(2.31, 1.55, 1.507, 1.454, .697);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetRaw(m_centerPwm);
+
+ LiveWindow::GetInstance()->AddActuator("Jaguar", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ * @param syncGroup Unused interface.
+ */
+void Jaguar::Set(float speed, uint8_t syncGroup)
+{
+ SetSpeed(speed);
+}
+
+/**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+float Jaguar::Get() const
+{
+ return GetSpeed();
+}
+
+/**
+ * Common interface for disabling a motor.
+ */
+void Jaguar::Disable()
+{
+ SetRaw(kPwmDisabled);
+}
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void Jaguar::PIDWrite(float output)
+{
+ Set(output);
+}
diff --git a/wpilibc/simulation/src/Joystick.cpp b/wpilibc/simulation/src/Joystick.cpp
new file mode 100644
index 0000000..a7a299c
--- /dev/null
+++ b/wpilibc/simulation/src/Joystick.cpp
@@ -0,0 +1,377 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Joystick.h"
+#include "DriverStation.h"
+//#include "NetworkCommunication/UsageReporting.h"
+#include "WPIErrors.h"
+#include <math.h>
+#include <string.h>
+
+const uint32_t Joystick::kDefaultXAxis;
+const uint32_t Joystick::kDefaultYAxis;
+const uint32_t Joystick::kDefaultZAxis;
+const uint32_t Joystick::kDefaultTwistAxis;
+const uint32_t Joystick::kDefaultThrottleAxis;
+const uint32_t Joystick::kDefaultTriggerButton;
+const uint32_t Joystick::kDefaultTopButton;
+static Joystick *joysticks[DriverStation::kJoystickPorts];
+static bool joySticksInitialized = false;
+
+/**
+ * Construct an instance of a joystick.
+ * The joystick index is the usb port on the drivers station.
+ *
+ * @param port The port on the driver station that the joystick is plugged into
+ * (0-5).
+ */
+Joystick::Joystick(uint32_t port)
+ : Joystick(port, kNumAxisTypes, kNumButtonTypes) {
+ m_axes[kXAxis] = kDefaultXAxis;
+ m_axes[kYAxis] = kDefaultYAxis;
+ m_axes[kZAxis] = kDefaultZAxis;
+ m_axes[kTwistAxis] = kDefaultTwistAxis;
+ m_axes[kThrottleAxis] = kDefaultThrottleAxis;
+
+ m_buttons[kTriggerButton] = kDefaultTriggerButton;
+ m_buttons[kTopButton] = kDefaultTopButton;
+
+ HALReport(HALUsageReporting::kResourceType_Joystick, port);
+}
+
+/**
+ * Version of the constructor to be called by sub-classes.
+ *
+ * This constructor allows the subclass to configure the number of constants
+ * for axes and buttons.
+ *
+ * @param port The port on the driver station that the joystick is plugged into.
+ * @param numAxisTypes The number of axis types in the enum.
+ * @param numButtonTypes The number of button types in the enum.
+ */
+Joystick::Joystick(uint32_t port, uint32_t numAxisTypes,
+ uint32_t numButtonTypes)
+ : m_ds(DriverStation::GetInstance()),
+ m_port(port),
+ m_axes(numAxisTypes),
+ m_buttons(numButtonTypes) {
+ if (!joySticksInitialized) {
+ for (auto& joystick : joysticks) joystick = nullptr;
+ joySticksInitialized = true;
+ }
+ if (m_port >= DriverStation::kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ } else {
+ joysticks[m_port] = this;
+ }
+}
+
+Joystick *Joystick::GetStickForPort(uint32_t port) {
+ Joystick *stick = joysticks[port];
+ if (stick == nullptr) {
+ stick = new Joystick(port);
+ joysticks[port] = stick;
+ }
+ return stick;
+}
+
+/**
+ * Get the X value of the joystick.
+ * This depends on the mapping of the joystick connected to the current port.
+ * @param hand This parameter is ignored for the Joystick class and is only here
+ * to complete the GenericHID interface.
+ */
+float Joystick::GetX(JoystickHand hand) const {
+ return GetRawAxis(m_axes[kXAxis]);
+}
+
+/**
+ * Get the Y value of the joystick.
+ * This depends on the mapping of the joystick connected to the current port.
+ * @param hand This parameter is ignored for the Joystick class and is only here
+ * to complete the GenericHID interface.
+ */
+float Joystick::GetY(JoystickHand hand) const {
+ return GetRawAxis(m_axes[kYAxis]);
+}
+
+/**
+ * Get the Z value of the current joystick.
+ * This depends on the mapping of the joystick connected to the current port.
+ */
+float Joystick::GetZ() const { return GetRawAxis(m_axes[kZAxis]); }
+
+/**
+ * Get the twist value of the current joystick.
+ * This depends on the mapping of the joystick connected to the current port.
+ */
+float Joystick::GetTwist() const { return GetRawAxis(m_axes[kTwistAxis]); }
+
+/**
+ * Get the throttle value of the current joystick.
+ * This depends on the mapping of the joystick connected to the current port.
+ */
+float Joystick::GetThrottle() const {
+ return GetRawAxis(m_axes[kThrottleAxis]);
+}
+
+/**
+ * Get the value of the axis.
+ *
+ * @param axis The axis to read, starting at 0.
+ * @return The value of the axis.
+ */
+float Joystick::GetRawAxis(uint32_t axis) const {
+ return m_ds.GetStickAxis(m_port, axis);
+}
+
+/**
+ * For the current joystick, return the axis determined by the argument.
+ *
+ * This is for cases where the joystick axis is returned programatically,
+ * otherwise one of the
+ * previous functions would be preferable (for example GetX()).
+ *
+ * @param axis The axis to read.
+ * @return The value of the axis.
+ */
+float Joystick::GetAxis(AxisType axis) const {
+ switch (axis) {
+ case kXAxis:
+ return this->GetX();
+ case kYAxis:
+ return this->GetY();
+ case kZAxis:
+ return this->GetZ();
+ case kTwistAxis:
+ return this->GetTwist();
+ case kThrottleAxis:
+ return this->GetThrottle();
+ default:
+ wpi_setWPIError(BadJoystickAxis);
+ return 0.0;
+ }
+}
+
+/**
+ * Read the state of the trigger on the joystick.
+ *
+ * Look up which button has been assigned to the trigger and read its state.
+ *
+ * @param hand This parameter is ignored for the Joystick class and is only here
+ * to complete the GenericHID interface.
+ * @return The state of the trigger.
+ */
+bool Joystick::GetTrigger(JoystickHand hand) const {
+ return GetRawButton(m_buttons[kTriggerButton]);
+}
+
+/**
+ * Read the state of the top button on the joystick.
+ *
+ * Look up which button has been assigned to the top and read its state.
+ *
+ * @param hand This parameter is ignored for the Joystick class and is only here
+ * to complete the GenericHID interface.
+ * @return The state of the top button.
+ */
+bool Joystick::GetTop(JoystickHand hand) const {
+ return GetRawButton(m_buttons[kTopButton]);
+}
+
+/**
+ * This is not supported for the Joystick.
+ * This method is only here to complete the GenericHID interface.
+ */
+bool Joystick::GetBumper(JoystickHand hand) const {
+ // Joysticks don't have bumpers.
+ return false;
+}
+
+/**
+ * Get the button value (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 Joystick::GetRawButton(uint32_t button) const {
+ return m_ds.GetStickButton(m_port, button);
+}
+
+/**
+ * Get the state of a POV on the joystick.
+ *
+ * @param pov The index of the POV to read (starting at 0)
+ * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+ */
+int Joystick::GetPOV(uint32_t pov) const {
+ return m_ds.GetStickPOV(m_port, pov);
+}
+
+/**
+ * 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 m_ds.GetStickAxisCount(m_port); }
+
+/**
+ * Get the value of isXbox for the joystick.
+ *
+ * @return A boolean that is true if the joystick is an xbox controller.
+ */
+bool Joystick::GetIsXbox() const { return m_ds.GetJoystickIsXbox(m_port); }
+
+/**
+ * Get the HID type of the controller.
+ *
+ * @return the HID type of the controller.
+ */
+Joystick::HIDType Joystick::GetType() const {
+ return static_cast<HIDType>(m_ds.GetJoystickType(m_port));
+}
+
+/**
+ * Get the name of the joystick.
+ *
+ * @return the name of the controller.
+ */
+std::string Joystick::GetName() const { return m_ds.GetJoystickName(m_port); }
+
+// int Joystick::GetAxisType(uint8_t axis) const
+//{
+// return m_ds.GetJoystickAxisType(m_port, axis);
+//}
+
+/**
+ * Get the number of axis for a joystick
+ *
+* @return the number of buttons on the current joystick
+ */
+int Joystick::GetButtonCount() const {
+ return m_ds.GetStickButtonCount(m_port);
+}
+
+/**
+ * Get the number of axis for a joystick
+ *
+ * @return then umber of POVs for the current joystick
+ */
+int Joystick::GetPOVCount() const { return m_ds.GetStickPOVCount(m_port); }
+
+/**
+ * Get the channel currently associated with the specified axis.
+ *
+ * @param axis The axis to look up the channel for.
+ * @return The channel fr the axis.
+ */
+uint32_t Joystick::GetAxisChannel(AxisType axis) const { return m_axes[axis]; }
+
+/**
+ * Set the channel associated with a specified axis.
+ *
+ * @param axis The axis to set the channel for.
+ * @param channel The channel to set the axis to.
+ */
+void Joystick::SetAxisChannel(AxisType axis, uint32_t channel) {
+ m_axes[axis] = channel;
+}
+
+/**
+ * Get the magnitude of the direction vector formed by the joystick's
+ * current position relative to its origin
+ *
+ * @return The magnitude of the direction vector
+ */
+float Joystick::GetMagnitude() const {
+ return sqrt(pow(GetX(), 2) + pow(GetY(), 2));
+}
+
+/**
+ * Get the direction of the vector formed by the joystick and its origin
+ * in radians
+ *
+ * @return The direction of the vector in radians
+ */
+float Joystick::GetDirectionRadians() const { return atan2(GetX(), -GetY()); }
+
+/**
+ * Get the direction of the vector formed by the joystick and its origin
+ * in degrees
+ *
+ * uses acos(-1) to represent Pi due to absence of readily accessible Pi
+ * constant in C++
+ *
+ * @return The direction of the vector in degrees
+ */
+float Joystick::GetDirectionDegrees() const {
+ return (180 / acos(-1)) * GetDirectionRadians();
+}
+
+/**
+ * Set the rumble output for the joystick. The DS currently supports 2 rumble
+ * values,
+ * left rumble and right rumble
+ * @param type Which rumble value to set
+ * @param value The normalized value (0 to 1) to set the rumble to
+ */
+void Joystick::SetRumble(RumbleType type, float value) {
+ if (value < 0)
+ value = 0;
+ else if (value > 1)
+ value = 1;
+ if (type == kLeftRumble)
+ m_leftRumble = value * 65535;
+ else
+ m_rightRumble = value * 65535;
+ HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}
+
+/**
+ * Set a single HID output value for the joystick.
+ * @param outputNumber The index of the output to set (1-32)
+ * @param value The value to set the output to
+ */
+
+void Joystick::SetOutput(uint8_t outputNumber, bool value) {
+ m_outputs =
+ (m_outputs & ~(1 << (outputNumber - 1))) | (value << (outputNumber - 1));
+
+ HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}
+
+/**
+ * Set all HID output values for the joystick.
+ * @param value The 32 bit output value (1 bit for each output)
+ */
+void Joystick::SetOutputs(uint32_t value) {
+ m_outputs = value;
+ HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}
diff --git a/wpilibc/simulation/src/MotorSafetyHelper.cpp b/wpilibc/simulation/src/MotorSafetyHelper.cpp
new file mode 100644
index 0000000..87f5352
--- /dev/null
+++ b/wpilibc/simulation/src/MotorSafetyHelper.cpp
@@ -0,0 +1,140 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "MotorSafetyHelper.h"
+
+#include "DriverStation.h"
+#include "MotorSafety.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+
+#include <stdio.h>
+#include <sstream>
+
+std::set<MotorSafetyHelper*> MotorSafetyHelper::m_helperList;
+priority_recursive_mutex MotorSafetyHelper::m_listMutex;
+
+/**
+ * The constructor for a MotorSafetyHelper object.
+ * The helper object is constructed for every object that wants to implement the
+ * Motor
+ * Safety protocol. The helper object has the code to actually do the timing and
+ * call the
+ * motors Stop() method when the timeout expires. The motor object is expected
+ * to call the
+ * Feed() method whenever the motors value is updated.
+ * @param safeObject a pointer to the motor object implementing MotorSafety.
+ * This is used
+ * to call the Stop() method on the motor.
+ */
+MotorSafetyHelper::MotorSafetyHelper(MotorSafety *safeObject)
+ : m_safeObject(safeObject) {
+ m_enabled = false;
+ m_expiration = DEFAULT_SAFETY_EXPIRATION;
+ m_stopTime = Timer::GetFPGATimestamp();
+
+ std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
+ m_helperList.insert(this);
+}
+
+MotorSafetyHelper::~MotorSafetyHelper() {
+ std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
+ m_helperList.erase(this);
+}
+
+/**
+ * Feed the motor safety object.
+ * Resets the timer on this object that is used to do the timeouts.
+ */
+void MotorSafetyHelper::Feed() {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
+}
+
+/**
+ * Set the expiration time for the corresponding motor safety object.
+ * @param expirationTime The timeout value in seconds.
+ */
+void MotorSafetyHelper::SetExpiration(float expirationTime) {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ m_expiration = expirationTime;
+}
+
+/**
+ * Retrieve the timeout value for the corresponding motor safety object.
+ * @return the timeout value in seconds.
+ */
+float MotorSafetyHelper::GetExpiration() const {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ return m_expiration;
+}
+
+/**
+ * Determine if the motor is still operating or has timed out.
+ * @return a true value if the motor is still operating normally and hasn't
+ * timed out.
+ */
+bool MotorSafetyHelper::IsAlive() const {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
+}
+
+/**
+ * Check if this motor has exceeded its timeout.
+ * This method is called periodically to determine if this motor has exceeded
+ * its timeout
+ * value. If it has, the stop method is called, and the motor is shut down until
+ * its value is
+ * updated again.
+ */
+void MotorSafetyHelper::Check()
+{
+ DriverStation &ds = DriverStation::GetInstance();
+ if (!m_enabled || ds.IsDisabled() || ds.IsTest()) return;
+
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ if (m_stopTime < Timer::GetFPGATimestamp()) {
+ std::ostringstream desc;
+ m_safeObject->GetDescription(desc);
+ desc << "... Output not updated often enough.";
+ wpi_setWPIErrorWithContext(Timeout, desc.str().c_str());
+ m_safeObject->StopMotor();
+ }
+}
+
+/**
+ * Enable/disable motor safety for this device
+ * Turn on and off the motor safety option for this PWM object.
+ * @param enabled True if motor safety is enforced for this object
+ */
+void MotorSafetyHelper::SetSafetyEnabled(bool enabled) {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ m_enabled = enabled;
+}
+
+/**
+ * Return the state of the motor safety enabled flag
+ * Return if the motor safety is currently enabled for this devicce.
+ * @return True if motor safety is enforced for this device
+ */
+bool MotorSafetyHelper::IsSafetyEnabled() const {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ return m_enabled;
+}
+
+/**
+ * Check the motors to see if any have timed out.
+ * This static method is called periodically to poll all the motors and stop
+ * any that have
+ * timed out.
+ */
+void MotorSafetyHelper::CheckMotors() {
+ std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
+ for (auto elem : m_helperList) {
+ elem->Check();
+ }
+}
diff --git a/wpilibc/simulation/src/Notifier.cpp b/wpilibc/simulation/src/Notifier.cpp
new file mode 100644
index 0000000..25daaf0
--- /dev/null
+++ b/wpilibc/simulation/src/Notifier.cpp
@@ -0,0 +1,261 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Notifier.h"
+#include "Timer.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+
+Notifier *Notifier::timerQueueHead = nullptr;
+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, void *param)
+{
+ if (handler == nullptr)
+ wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
+ m_handler = handler;
+ m_param = param;
+ m_periodic = false;
+ m_expirationTime = 0;
+ m_period = 0;
+ m_nextEvent = nullptr;
+ m_queued = false;
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ // do the first time intialization of static variables
+ if (refcount.fetch_add(1) == 0) {
+ m_task = std::thread(Run);
+ }
+ }
+}
+
+/**
+ * Free the resources for a timer event.
+ * All resources will be freed and the timer event will be removed from the
+ * queue if necessary.
+ */
+Notifier::~Notifier()
+{
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ DeleteFromQueue();
+
+ // Delete the static variables when the last one is going away
+ if (refcount.fetch_sub(1) == 1)
+ {
+ m_stopped = true;
+ m_task.join();
+ }
+ }
+
+ // Acquire the semaphore; this makes certain that the handler is
+ // not being executed by the interrupt manager.
+ std::lock_guard<priority_mutex> lock(m_handlerMutex);
+}
+
+/**
+ * Update the alarm hardware to reflect the current first element in the queue.
+ * Compute the time the next alarm should occur based on the current time and the
+ * period for the first element in the timer queue.
+ * WARNING: this method does not do synchronization! It must be called from somewhere
+ * that is taking care of synchronizing access to the queue.
+ */
+void Notifier::UpdateAlarm()
+{
+}
+
+/**
+ * ProcessQueue is called whenever there is a timer interrupt.
+ * We need to wake up and process the current top item in the timer queue as long
+ * as its scheduled time is after the current time. Then the item is removed or
+ * rescheduled (repetitive events) in the queue.
+ */
+void Notifier::ProcessQueue(uint32_t mask, void *params)
+{
+ Notifier *current;
+ while (true) // keep processing past events until no more
+ {
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ double currentTime = GetClock();
+ current = timerQueueHead;
+ if (current == nullptr || current->m_expirationTime > currentTime)
+ {
+ break; // no more timer events to process
+ }
+ // need to process this entry
+ timerQueueHead = current->m_nextEvent;
+ 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(current->m_param); // 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;
+ }
+ if (timerQueueHead == nullptr || timerQueueHead->m_expirationTime >= this->m_expirationTime)
+ {
+ // the queue is empty or greater than the new entry
+ // the new entry becomes the first element
+ this->m_nextEvent = timerQueueHead;
+ timerQueueHead = this;
+ if (!reschedule)
+ {
+ // since the first element changed, update alarm, unless we already plan to
+ UpdateAlarm();
+ }
+ }
+ else
+ {
+ for (Notifier **npp = &(timerQueueHead->m_nextEvent); ; npp = &(*npp)->m_nextEvent)
+ {
+ Notifier *n = *npp;
+ if (n == nullptr || n->m_expirationTime > this->m_expirationTime)
+ {
+ *npp = this;
+ this->m_nextEvent = n;
+ break;
+ }
+ }
+ }
+ 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(timerQueueHead != nullptr);
+ if (timerQueueHead == this)
+ {
+ // remove the first item in the list - update the alarm
+ timerQueueHead = this->m_nextEvent;
+ UpdateAlarm();
+ }
+ else
+ {
+ for (Notifier *n = timerQueueHead; n != nullptr; n = n->m_nextEvent)
+ {
+ if (n->m_nextEvent == this)
+ {
+ // this element is the next element from *n from the queue
+ n->m_nextEvent = this->m_nextEvent; // point around this one
+ }
+ }
+ }
+ }
+}
+
+/**
+ * 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);
+ if (timerQueueHead != nullptr)
+ {
+ Wait(timerQueueHead->m_expirationTime - GetClock());
+ }
+ else
+ {
+ Wait(0.05);
+ }
+ }
+}
diff --git a/wpilibc/simulation/src/PIDController.cpp b/wpilibc/simulation/src/PIDController.cpp
new file mode 100644
index 0000000..03beb5c
--- /dev/null
+++ b/wpilibc/simulation/src/PIDController.cpp
@@ -0,0 +1,619 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "PIDController.h"
+#include "Notifier.h"
+#include "PIDSource.h"
+#include "PIDOutput.h"
+#include <math.h>
+
+static const std::string kP = "p";
+static const std::string kI = "i";
+static const std::string kD = "d";
+static const std::string kF = "f";
+static const std::string kSetpoint = "setpoint";
+static const std::string kEnabled = "enabled";
+
+
+/**
+ * Allocate a PID object with the given constants for P, I, D
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output value
+ * @param period the loop time for doing calculations. This particularly effects calculations of the
+ * integral and differental terms. The default is 50ms.
+ */
+PIDController::PIDController(float Kp, float Ki, float Kd,
+ PIDSource *source, PIDOutput *output,
+ float period)
+{
+ Initialize(Kp, Ki, Kd, 0.0f, source, output, period);
+}
+
+/**
+ * Allocate a PID object with the given constants for P, I, D
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output value
+ * @param period the loop time for doing calculations. This particularly effects calculations of the
+ * integral and differental terms. The default is 50ms.
+ */
+PIDController::PIDController(float Kp, float Ki, float Kd, float Kf,
+ PIDSource *source, PIDOutput *output,
+ float period)
+{
+ Initialize(Kp, Ki, Kd, Kf, source, output, period);
+}
+
+
+void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
+ PIDSource *source, PIDOutput *output,
+ float period)
+{
+ m_table = nullptr;
+
+ m_P = Kp;
+ m_I = Ki;
+ m_D = Kd;
+ m_F = Kf;
+
+ m_maximumOutput = 1.0;
+ m_minimumOutput = -1.0;
+
+ m_maximumInput = 0;
+ m_minimumInput = 0;
+
+ m_continuous = false;
+ m_enabled = false;
+ m_setpoint = 0;
+
+ m_prevInput = 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::CallCalculate, this);
+ m_controlLoop->StartPeriodic(m_period);
+
+ static int32_t instances = 0;
+ instances++;
+
+ m_toleranceType = kNoTolerance;
+}
+
+PIDController::~PIDController() {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Call the Calculate method as a non-static method. This avoids having to prepend
+ * all local variables in that method with the class pointer. This way the "this"
+ * pointer will be set up and class variables can be called more easily.
+ * This method is static and called by the Notifier class.
+ * @param controller the address of the PID controller object to use in the background loop
+ */
+void PIDController::CallCalculate(void *controller)
+{
+ PIDController *control = (PIDController*) controller;
+ control->Calculate();
+}
+
+ /**
+ * Read the input, calculate the output accordingly, and write to the output.
+ * This should only be called by the Notifier indirectly through CallCalculate
+ * and is created during initialization.
+ */
+void PIDController::Calculate()
+{
+ bool enabled;
+ PIDSource *pidInput;
+
+ {
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ if (m_pidInput == 0) return;
+ if (m_pidOutput == 0) return;
+ enabled = m_enabled;
+ pidInput = m_pidInput;
+ }
+
+ if (enabled)
+ {
+ float input = pidInput->PIDGet();
+ float result;
+ PIDOutput *pidOutput;
+
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_error = m_setpoint - input;
+ if (m_continuous)
+ {
+ if (fabs(m_error) > (m_maximumInput - m_minimumInput) / 2)
+ {
+ if (m_error > 0)
+ {
+ m_error = m_error - m_maximumInput + m_minimumInput;
+ }
+ else
+ {
+ m_error = m_error + m_maximumInput - m_minimumInput;
+ }
+ }
+ }
+
+ if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
+ if (m_P != 0) {
+ double potentialPGain = (m_totalError + m_error) * m_P;
+ if (potentialPGain < m_maximumOutput) {
+ if (potentialPGain > m_minimumOutput) {
+ m_totalError += m_error;
+ }
+ else {
+ m_totalError = m_minimumOutput / m_P;
+ }
+ }
+ else {
+ m_totalError = m_maximumOutput / m_P;
+ }
+ }
+
+ m_result = m_D * m_error + m_P * m_totalError + m_setpoint * m_F;
+ }
+ 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_prevInput - input) + m_setpoint * m_F;
+ }
+ m_prevInput = input;
+
+ 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);
+ }
+}
+
+/**
+ * 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_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_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_mutex> lock(m_mutex);
+ return m_P;
+}
+
+/**
+ * Get the Integral coefficient
+ * @return integral coefficient
+ */
+double PIDController::GetI() const
+{
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ return m_I;
+}
+
+/**
+ * Get the Differential coefficient
+ * @return differential coefficient
+ */
+double PIDController::GetD() const
+{
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ return m_D;
+}
+
+/**
+ * Get the Feed forward coefficient
+ * @return Feed forward coefficient
+ */
+double PIDController::GetF() const
+{
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ return m_F;
+}
+
+/**
+ * Return the current PID result
+ * This is always centered on zero and constrained the the max and min outs
+ * @return the latest calculated output
+ */
+float PIDController::Get() const
+{
+ std::lock_guard<priority_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_mutex> lock(m_mutex);
+ m_continuous = continuous;
+}
+
+/**
+ * Sets the maximum and minimum values expected from the input.
+ *
+ * @param minimumInput the minimum value expected from the input
+ * @param maximumInput the maximum value expected from the output
+ */
+void PIDController::SetInputRange(float minimumInput, float maximumInput)
+{
+ {
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ m_minimumInput = minimumInput;
+ m_maximumInput = maximumInput;
+ }
+
+ SetSetpoint(m_setpoint);
+}
+
+/**
+ * Sets the minimum and maximum values to write.
+ *
+ * @param minimumOutput the minimum value to write to the output
+ * @param maximumOutput the maximum value to write to the output
+ */
+void PIDController::SetOutputRange(float minimumOutput, float maximumOutput)
+{
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ m_minimumOutput = minimumOutput;
+ m_maximumOutput = maximumOutput;
+}
+
+/**
+ * Set the setpoint for the PIDController
+ * @param setpoint the desired setpoint
+ */
+void PIDController::SetSetpoint(float setpoint)
+{
+ {
+ std::lock_guard<priority_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_mutex> lock(m_mutex);
+ return m_setpoint;
+}
+
+/**
+ * Retruns the current difference of the input from the setpoint
+ * @return the current error
+ */
+float PIDController::GetError() const
+{
+ double pidInput;
+ {
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ pidInput = m_pidInput->PIDGet();
+ }
+ return GetSetpoint() - pidInput;
+}
+
+/**
+ * Sets what type of input the PID controller will use
+ */
+void PIDController::SetPIDSourceType(PIDSourceType pidSource) {
+ m_pidInput->SetPIDSourceType(pidSource);
+}
+
+/**
+ * Returns the type of input the PID controller is using
+ * @return the PID controller input type
+ */
+PIDSourceType PIDController::GetPIDSourceType() const {
+ return m_pidInput->GetPIDSourceType();
+}
+
+/**
+ * Returns the current average of the error over the past few iterations.
+ * You can specify the number of iterations to average with SetToleranceBuffer()
+ * (defaults to 1). This is the same value that is used for OnTarget().
+ * @return the average error
+ */
+float PIDController::GetAvgError() const {
+ float avgError = 0;
+ {
+ std::lock_guard<priority_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 percentage error which is tolerable
+ */
+void PIDController::SetTolerance(float percent)
+{
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ m_toleranceType = kPercentTolerance;
+ m_tolerance = percent;
+}
+
+/*
+ * Set the percentage error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDController::SetPercentTolerance(float percent)
+{
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ m_toleranceType = kPercentTolerance;
+ m_tolerance = percent;
+}
+
+/*
+ * Set the absolute error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDController::SetAbsoluteTolerance(float absTolerance)
+{
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ m_toleranceType = kAbsoluteTolerance;
+ m_tolerance = absTolerance;
+}
+
+/*
+ * Set the number of previous error samples to average for tolerancing. When
+ * determining whether a mechanism is on target, the user may want to use a
+ * rolling average of previous measurements instead of a precise position or
+ * velocity. This is useful for noisy sensors which return a few erroneous
+ * measurements when the mechanism is on target. However, the mechanism will
+ * not register as on target for at least the specified bufLength cycles.
+ * @param bufLength Number of previous cycles to average. Defaults to 1.
+ */
+void PIDController::SetToleranceBuffer(unsigned bufLength) {
+ 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
+{
+ double error = GetError();
+
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ switch (m_toleranceType) {
+ case kPercentTolerance:
+ return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput);
+ break;
+ case kAbsoluteTolerance:
+ return fabs(error) < m_tolerance;
+ break;
+ case kNoTolerance: // TODO: this case needs an error
+ return false;
+ }
+ return false;
+}
+
+/**
+ * Begin running the PIDController
+ */
+void PIDController::Enable()
+{
+ {
+ std::lock_guard<priority_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_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_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_mutex> lock(m_mutex);
+ m_prevInput = 0;
+ m_totalError = 0;
+ m_result = 0;
+}
+
+std::string PIDController::GetSmartDashboardType() const {
+ return "PIDController";
+}
+
+void PIDController::InitTable(std::shared_ptr<ITable> table){
+ if(m_table!=nullptr)
+ m_table->RemoveTableListener(this);
+ m_table = table;
+ if(m_table!=nullptr){
+ m_table->PutNumber(kP, GetP());
+ m_table->PutNumber(kI, GetI());
+ m_table->PutNumber(kD, GetD());
+ m_table->PutNumber(kF, GetF());
+ m_table->PutNumber(kSetpoint, GetSetpoint());
+ m_table->PutBoolean(kEnabled, IsEnabled());
+ m_table->AddTableListener(this, false);
+ }
+}
+
+std::shared_ptr<ITable> PIDController::GetTable() const {
+ return m_table;
+}
+
+void PIDController::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (key == kP || key == kI || key == kD || key == kF) {
+ if (m_P != m_table->GetNumber(kP, 0.0) ||
+ m_I != m_table->GetNumber(kI, 0.0) ||
+ m_D != m_table->GetNumber(kD, 0.0) ||
+ m_F != m_table->GetNumber(kF, 0.0)) {
+ SetPID(m_table->GetNumber(kP, 0.0), m_table->GetNumber(kI, 0.0),
+ m_table->GetNumber(kD, 0.0), m_table->GetNumber(kF, 0.0));
+ }
+ } else if (key == kSetpoint && value->IsDouble() &&
+ m_setpoint != value->GetDouble()) {
+ SetSetpoint(value->GetDouble());
+ } else if (key == kEnabled && value->IsBoolean() &&
+ m_enabled != value->GetBoolean()) {
+ if (value->GetBoolean()) {
+ Enable();
+ } else {
+ Disable();
+ }
+ }
+}
+
+void PIDController::UpdateTable() {
+
+}
+
+void PIDController::StartLiveWindowMode() {
+ Disable();
+}
+
+void PIDController::StopLiveWindowMode() {
+
+}
diff --git a/wpilibc/simulation/src/PWM.cpp b/wpilibc/simulation/src/PWM.cpp
new file mode 100644
index 0000000..dfb8bd9
--- /dev/null
+++ b/wpilibc/simulation/src/PWM.cpp
@@ -0,0 +1,261 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "PWM.h"
+
+//#include "NetworkCommunication/UsageReporting.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+
+const float PWM::kDefaultPwmPeriod = 5.05;
+const float PWM::kDefaultPwmCenter = 1.5;
+const int32_t PWM::kDefaultPwmStepsDown = 1000;
+const int32_t PWM::kPwmDisabled = 0;
+
+/**
+ * Allocate a PWM given a channel number.
+ *
+ * Checks channel value range and allocates the appropriate channel.
+ * The allocation is only done to help users ensure that they don't double
+ * assign channels.
+ * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP
+ * port
+ */
+PWM::PWM(uint32_t channel)
+{
+ char buf[64];
+
+ if (!CheckPWMChannel(channel))
+ {
+ snprintf(buf, 64, "PWM Channel %d", channel);
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
+ return;
+ }
+
+ sprintf(buf, "pwm/%d", channel);
+ impl = new SimContinuousOutput(buf);
+ m_channel = channel;
+ m_eliminateDeadband = false;
+
+ m_centerPwm = kPwmDisabled; // In simulation, the same thing.
+}
+
+PWM::~PWM() {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Optionally eliminate the deadband from a speed controller.
+ * @param eliminateDeadband If true, set the motor curve on the Jaguar to eliminate
+ * the deadband in the middle of the range. Otherwise, keep the full range without
+ * modifying any values.
+ */
+void PWM::EnableDeadbandElimination(bool eliminateDeadband)
+{
+ m_eliminateDeadband = eliminateDeadband;
+}
+
+/**
+ * Set the bounds on the PWM values.
+ * This sets the bounds on the PWM values for a particular each type of controller. The values
+ * determine the upper and lower speeds as well as the deadband bracket.
+ * @param max The Minimum pwm value
+ * @param deadbandMax The high end of the deadband range
+ * @param center The center speed (off)
+ * @param deadbandMin The low end of the deadband range
+ * @param min The minimum pwm value
+ */
+void PWM::SetBounds(int32_t max, int32_t deadbandMax, int32_t center, int32_t deadbandMin, int32_t min)
+{
+ // Nothing to do in simulation.
+}
+
+
+/**
+ * Set the bounds on the PWM pulse widths.
+ * This sets the bounds on the PWM values for a particular type of controller. The values
+ * determine the upper and lower speeds as well as the deadband bracket.
+ * @param max The max PWM pulse width in ms
+ * @param deadbandMax The high end of the deadband range pulse width in ms
+ * @param center The center (off) pulse width in ms
+ * @param deadbandMin The low end of the deadband pulse width in ms
+ * @param min The minimum pulse width in ms
+ */
+void PWM::SetBounds(double max, double deadbandMax, double center, double deadbandMin, double min)
+{
+ // Nothing to do in simulation.
+}
+
+/**
+ * Set the PWM value based on a position.
+ *
+ * This is intended to be used by servos.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @param pos The position to set the servo between 0.0 and 1.0.
+ */
+void PWM::SetPosition(float pos)
+{
+ if (pos < 0.0)
+ {
+ pos = 0.0;
+ }
+ else if (pos > 1.0)
+ {
+ pos = 1.0;
+ }
+
+ impl->Set(pos);
+}
+
+/**
+ * Get the PWM value in terms of a position.
+ *
+ * This is intended to be used by servos.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @return The position the servo is set to between 0.0 and 1.0.
+ */
+float PWM::GetPosition() const
+{
+ float value = impl->Get();
+ if (value < 0.0)
+ {
+ return 0.0;
+ }
+ else if (value > 1.0)
+ {
+ return 1.0;
+ }
+ else
+ {
+ return value;
+ }
+}
+
+/**
+ * Set the PWM value based on a speed.
+ *
+ * This is intended to be used by speed controllers.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinPositivePwm() called.
+ * @pre SetCenterPwm() called.
+ * @pre SetMaxNegativePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @param speed The speed to set the speed controller between -1.0 and 1.0.
+ */
+void PWM::SetSpeed(float speed)
+{
+ // clamp speed to be in the range 1.0 >= speed >= -1.0
+ if (speed < -1.0)
+ {
+ speed = -1.0;
+ }
+ else if (speed > 1.0)
+ {
+ speed = 1.0;
+ }
+
+ impl->Set(speed);
+}
+
+/**
+ * Get the PWM value in terms of speed.
+ *
+ * This is intended to be used by speed controllers.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinPositivePwm() called.
+ * @pre SetMaxNegativePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @return The most recently set speed between -1.0 and 1.0.
+ */
+float PWM::GetSpeed() const
+{
+ float value = impl->Get();
+ if (value > 1.0)
+ {
+ return 1.0;
+ }
+ else if (value < -1.0)
+ {
+ return -1.0;
+ }
+ else
+ {
+ return value;
+ }
+}
+
+/**
+ * Set the PWM value directly to the hardware.
+ *
+ * Write a raw value to a PWM channel.
+ *
+ * @param value Raw PWM value.
+ */
+void PWM::SetRaw(unsigned short value)
+{
+ wpi_assert(value == kPwmDisabled);
+ impl->Set(0);
+}
+
+/**
+ * Slow down the PWM signal for old devices.
+ *
+ * @param mult The period multiplier to apply to this channel
+ */
+void PWM::SetPeriodMultiplier(PeriodMultiplier mult)
+{
+ // Do nothing in simulation.
+}
+
+
+void PWM::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (!value->IsDouble()) return;
+ SetSpeed(value->GetDouble());
+}
+
+void PWM::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", GetSpeed());
+ }
+}
+
+void PWM::StartLiveWindowMode() {
+ SetSpeed(0);
+ if (m_table != nullptr) {
+ m_table->AddTableListener("Value", this, true);
+ }
+}
+
+void PWM::StopLiveWindowMode() {
+ SetSpeed(0);
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string PWM::GetSmartDashboardType() const {
+ return "Speed Controller";
+}
+
+void PWM::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> PWM::GetTable() const {
+ return m_table;
+}
diff --git a/wpilibc/simulation/src/Relay.cpp b/wpilibc/simulation/src/Relay.cpp
new file mode 100644
index 0000000..f35d782
--- /dev/null
+++ b/wpilibc/simulation/src/Relay.cpp
@@ -0,0 +1,249 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Relay.h"
+
+#include "MotorSafetyHelper.h"
+//#include "NetworkCommunication/UsageReporting.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Relay constructor given a channel.
+ *
+ * This code initializes the relay and reserves all resources that need to be
+ * locked. Initially the relay is set to both lines at 0v.
+ * @param channel The channel number (0-3).
+ * @param direction The direction that the Relay object will control.
+ */
+Relay::Relay(uint32_t channel, Relay::Direction direction)
+ : m_channel (channel)
+ , m_direction (direction)
+{
+ char buf[64];
+ if (!SensorBase::CheckRelayChannel(m_channel))
+ {
+ snprintf(buf, 64, "Relay Channel %d", m_channel);
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
+ return;
+ }
+
+ m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
+ m_safetyHelper->SetSafetyEnabled(false);
+
+ sprintf(buf, "relay/%d", m_channel);
+ impl = new SimContinuousOutput(buf); // TODO: Allow two different relays (targetting the different halves of a relay) to be combined to control one motor.
+ LiveWindow::GetInstance()->AddActuator("Relay", 1, m_channel, this);
+ go_pos = go_neg = false;
+}
+
+/**
+ * Free the resource associated with a relay.
+ * The relay channels are set to free and the relay output is turned off.
+ */
+Relay::~Relay()
+{
+ impl->Set(0);
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Set the relay state.
+ *
+ * Valid values depend on which directions of the relay are controlled by the object.
+ *
+ * When set to kBothDirections, the relay can be any of the four states:
+ * 0v-0v, 0v-12v, 12v-0v, 12v-12v
+ *
+ * When set to kForwardOnly or kReverseOnly, you can specify the constant for the
+ * direction or you can simply specify kOff and kOn. Using only kOff and kOn is
+ * recommended.
+ *
+ * @param value The state to set the relay.
+ */
+void Relay::Set(Relay::Value value)
+{
+ switch (value)
+ {
+ case kOff:
+ if (m_direction == kBothDirections || m_direction == kForwardOnly)
+ {
+ go_pos = false;
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly)
+ {
+ go_neg = false;
+ }
+ break;
+ case kOn:
+ if (m_direction == kBothDirections || m_direction == kForwardOnly)
+ {
+ go_pos = true;
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly)
+ {
+ go_neg = true;
+ }
+ break;
+ case kForward:
+ if (m_direction == kReverseOnly)
+ {
+ wpi_setWPIError(IncompatibleMode);
+ break;
+ }
+ if (m_direction == kBothDirections || m_direction == kForwardOnly)
+ {
+ go_pos = true;
+ }
+ if (m_direction == kBothDirections)
+ {
+ go_neg = false;
+ }
+ break;
+ case kReverse:
+ if (m_direction == kForwardOnly)
+ {
+ wpi_setWPIError(IncompatibleMode);
+ break;
+ }
+ if (m_direction == kBothDirections)
+ {
+ go_pos = false;
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly)
+ {
+ go_neg = true;
+ }
+ break;
+ }
+ impl->Set((go_pos ? 1 : 0) + (go_neg ? -1 : 0));
+}
+
+/**
+ * Get the Relay State
+ *
+ * Gets the current state of the relay.
+ *
+ * When set to kForwardOnly or kReverseOnly, value is returned as kOn/kOff not
+ * kForward/kReverse (per the recommendation in Set)
+ *
+ * @return The current state of the relay as a Relay::Value
+ */
+Relay::Value Relay::Get() const {
+ // TODO: Don't assume that the go_pos and go_neg fields are correct?
+ if ((go_pos || m_direction == kReverseOnly) && (go_neg || m_direction == kForwardOnly)) {
+ return kOn;
+ } else if (go_pos) {
+ return kForward;
+ } else if (go_neg) {
+ return kReverse;
+ } else {
+ return kOff;
+ }
+}
+
+uint32_t Relay::GetChannel() const {
+ return m_channel;
+}
+
+/**
+ * Set the expiration time for the Relay object
+ * @param timeout The timeout (in seconds) for this relay object
+ */
+void Relay::SetExpiration(float timeout) {
+ m_safetyHelper->SetExpiration(timeout);
+}
+
+/**
+ * Return the expiration time for the relay object.
+ * @return The expiration time value.
+ */
+float Relay::GetExpiration() const { return m_safetyHelper->GetExpiration(); }
+
+/**
+ * Check if the relay object is currently alive or stopped due to a timeout.
+ * @returns 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
+ * @returns 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/simulation/src/RobotBase.cpp b/wpilibc/simulation/src/RobotBase.cpp
new file mode 100644
index 0000000..1fa5ac7
--- /dev/null
+++ b/wpilibc/simulation/src/RobotBase.cpp
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotBase.h"
+#include "RobotState.h"
+#include "Utility.h"
+
+#include <cstring>
+
+RobotBase* RobotBase::m_instance = nullptr;
+
+void RobotBase::setInstance(RobotBase* robot)
+{
+ wpi_assert(m_instance == nullptr);
+ m_instance = robot;
+}
+
+RobotBase &RobotBase::getInstance()
+{
+ return *m_instance;
+}
+
+/**
+ * Constructor for a generic robot program.
+ * User code should be placed in the constuctor that runs before the Autonomous or Operator
+ * Control period starts. The constructor will run to completion before Autonomous is entered.
+ *
+ * This must be used to ensure that the communications code starts. In the future it would be
+ * nice to put this code into it's own task that loads on boot so ensure that it runs.
+ */
+RobotBase::RobotBase() : m_ds(DriverStation::GetInstance())
+{
+ RobotState::SetImplementation(DriverStation::GetInstance());
+ transport::SubscriberPtr time_pub = MainNode::Subscribe("time", &wpilib::internal::time_callback);
+}
+
+/**
+ * Determine if the Robot is currently enabled.
+ * @return True if the Robot is currently enabled by the field controls.
+ */
+bool RobotBase::IsEnabled() const
+{
+ return m_ds.IsEnabled();
+}
+
+/**
+ * Determine if the Robot is currently disabled.
+ * @return True if the Robot is currently disabled by the field controls.
+ */
+bool RobotBase::IsDisabled() const
+{
+ return m_ds.IsDisabled();
+}
+
+/**
+ * Determine if the robot is currently in Autnomous mode.
+ * @return True if the robot is currently operating Autonomously as determined by the field controls.
+ */
+bool RobotBase::IsAutonomous() const
+{
+ return m_ds.IsAutonomous();
+}
+
+/**
+ * Determine if the robot is currently in Operator Control mode.
+ * @return True if the robot is currently operating in Tele-Op mode as determined by the field controls.
+ */
+bool RobotBase::IsOperatorControl() const
+{
+ return m_ds.IsOperatorControl();
+}
+
+/**
+ * Determine if the robot is currently in Test mode.
+ * @return True if the robot is currently running tests as determined by the field controls.
+ */
+bool RobotBase::IsTest() const
+{
+ return m_ds.IsTest();
+}
+
+/**
+ * This class exists for the sole purpose of getting its destructor called when the module unloads.
+ * Before the module is done unloading, we need to delete the RobotBase derived singleton. This should delete
+ * the other remaining singletons that were registered. This should also stop all tasks that are using
+ * the Task class.
+ */
+class RobotDeleter
+{
+public:
+ ~RobotDeleter()
+ {
+ delete &RobotBase::getInstance();
+ }
+};
+static RobotDeleter g_robotDeleter;
diff --git a/wpilibc/simulation/src/RobotDrive.cpp b/wpilibc/simulation/src/RobotDrive.cpp
new file mode 100644
index 0000000..852d0bf
--- /dev/null
+++ b/wpilibc/simulation/src/RobotDrive.cpp
@@ -0,0 +1,734 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotDrive.h"
+
+//#include "CANJaguar.h"
+#include "GenericHID.h"
+#include "Joystick.h"
+#include "Jaguar.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+#include <math.h>
+
+#undef max
+#include <algorithm>
+
+const int32_t RobotDrive::kMaxNumberOfMotors;
+
+/*
+ * 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 Jaguars for controlling the motors.
+ * @param leftMotorChannel The PWM channel number that drives the left motor.
+ * @param rightMotorChannel The PWM channel number that drives the right motor.
+ */
+RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel)
+{
+ InitRobotDrive();
+ m_rearLeftMotor = new Jaguar(leftMotorChannel);
+ m_rearRightMotor = new Jaguar(rightMotorChannel);
+ for (int32_t i=0; i < kMaxNumberOfMotors; i++)
+ {
+ m_invertedMotors[i] = 1;
+ }
+ 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 Jaguars for controlling the motors.
+ * @param frontLeftMotor Front left motor channel number
+ * @param rearLeftMotor Rear Left motor channel number
+ * @param frontRightMotor Front right motor channel number
+ * @param rearRightMotor Rear Right motor channel number
+ */
+RobotDrive::RobotDrive(uint32_t frontLeftMotor, uint32_t rearLeftMotor,
+ uint32_t frontRightMotor, uint32_t rearRightMotor)
+{
+ InitRobotDrive();
+ m_rearLeftMotor = new Jaguar(rearLeftMotor);
+ m_rearRightMotor = new Jaguar(rearRightMotor);
+ m_frontLeftMotor = new Jaguar(frontLeftMotor);
+ m_frontRightMotor = new Jaguar(frontRightMotor);
+ for (int32_t i=0; i < kMaxNumberOfMotors; i++)
+ {
+ m_invertedMotors[i] = 1;
+ }
+ 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 = leftMotor;
+ m_rearRightMotor = rightMotor;
+ for (int32_t i=0; i < kMaxNumberOfMotors; i++)
+ {
+ m_invertedMotors[i] = 1;
+ }
+ m_deleteSpeedControllers = false;
+}
+
+RobotDrive::RobotDrive(SpeedController &leftMotor, SpeedController &rightMotor)
+{
+ InitRobotDrive();
+ m_rearLeftMotor = &leftMotor;
+ m_rearRightMotor = &rightMotor;
+ for (int32_t i=0; i < kMaxNumberOfMotors; i++)
+ {
+ m_invertedMotors[i] = 1;
+ }
+ m_deleteSpeedControllers = false;
+}
+
+/**
+ * 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 = frontLeftMotor;
+ m_rearLeftMotor = rearLeftMotor;
+ m_frontRightMotor = frontRightMotor;
+ m_rearRightMotor = rearRightMotor;
+ for (int32_t i=0; i < kMaxNumberOfMotors; i++)
+ {
+ m_invertedMotors[i] = 1;
+ }
+ m_deleteSpeedControllers = false;
+}
+
+RobotDrive::RobotDrive(SpeedController &frontLeftMotor, SpeedController &rearLeftMotor,
+ SpeedController &frontRightMotor, SpeedController &rearRightMotor)
+{
+ InitRobotDrive();
+ m_frontLeftMotor = &frontLeftMotor;
+ m_rearLeftMotor = &rearLeftMotor;
+ m_frontRightMotor = &frontRightMotor;
+ m_rearRightMotor = &rearRightMotor;
+ for (int32_t i=0; i < kMaxNumberOfMotors; i++)
+ {
+ m_invertedMotors[i] = 1;
+ }
+ m_deleteSpeedControllers = false;
+}
+
+/**
+ * RobotDrive destructor.
+ * Deletes motor objects that were not passed in and created internally only.
+ **/
+RobotDrive::~RobotDrive()
+{
+ if (m_deleteSpeedControllers)
+ {
+ delete m_frontLeftMotor;
+ delete m_rearLeftMotor;
+ delete m_frontRightMotor;
+ delete m_rearRightMotor;
+ }
+ // FIXME: delete m_safetyHelper;
+}
+
+/**
+ * Drive the motors at "outputMagnitude" and "curve".
+ * Both outputMagnitude and curve are -1.0 to +1.0 values, where 0.0 represents
+ * stopped and not turning. curve < 0 will turn left and curve > 0 will turn
+ * right.
+ *
+ * The algorithm for steering provides a constant turn radius for any normal
+ * speed range, both forward and backward. Increasing m_sensitivity causes
+ * sharper turns for fixed values of curve.
+ *
+ * This function will most likely be used in an autonomous routine.
+ *
+ * @param outputMagnitude The speed setting for the outside wheel in a turn,
+ * forward or backwards, +1 to -1.
+ * @param curve The rate of turn, constant for different forward speeds. Set
+ * curve < 0 for left turn or curve > 0 for right turn.
+ * Set curve = e^(-r/w) to get a turn radius r for wheelbase w of your robot.
+ * Conversely, turn radius r = -ln(curve)*w for a given value of curve and
+ * wheelbase w.
+ */
+void RobotDrive::Drive(float outputMagnitude, float curve)
+{
+ float leftOutput, rightOutput;
+ static bool reported = false;
+ if (!reported)
+ {
+ reported = true;
+ }
+
+ if (curve < 0)
+ {
+ float value = log(-curve);
+ float ratio = (value - m_sensitivity)/(value + m_sensitivity);
+ if (ratio == 0) ratio =.0000000001;
+ leftOutput = outputMagnitude / ratio;
+ rightOutput = outputMagnitude;
+ }
+ else if (curve > 0)
+ {
+ float value = log(curve);
+ float ratio = (value - m_sensitivity)/(value + m_sensitivity);
+ if (ratio == 0) ratio =.0000000001;
+ leftOutput = outputMagnitude;
+ rightOutput = outputMagnitude / ratio;
+ }
+ else
+ {
+ leftOutput = outputMagnitude;
+ rightOutput = outputMagnitude;
+ }
+ SetLeftRightMotorOutputs(leftOutput, rightOutput);
+}
+
+/**
+ * Provide tank steering using the stored robot configuration.
+ * Drive the robot using two joystick inputs. The Y-axis will be selected from
+ * each Joystick object.
+ * @param leftStick The joystick to control the left side of the robot.
+ * @param rightStick The joystick to control the right side of the robot.
+ */
+void RobotDrive::TankDrive(GenericHID *leftStick, GenericHID *rightStick, bool squaredInputs)
+{
+ if (leftStick == nullptr || rightStick == nullptr)
+ {
+ wpi_setWPIError(NullParameter);
+ return;
+ }
+ TankDrive(leftStick->GetY(), rightStick->GetY(), squaredInputs);
+}
+
+void RobotDrive::TankDrive(GenericHID &leftStick, GenericHID &rightStick, bool squaredInputs)
+{
+ TankDrive(leftStick.GetY(), rightStick.GetY(), squaredInputs);
+}
+
+/**
+ * Provide tank steering using the stored robot configuration.
+ * This function lets you pick the axis to be used on each Joystick object for the left
+ * and right sides of the robot.
+ * @param leftStick The Joystick object to use for the left side of the robot.
+ * @param leftAxis The axis to select on the left side Joystick object.
+ * @param rightStick The Joystick object to use for the right side of the robot.
+ * @param rightAxis The axis to select on the right side Joystick object.
+ */
+void RobotDrive::TankDrive(GenericHID *leftStick, uint32_t leftAxis,
+ GenericHID *rightStick, uint32_t rightAxis, bool squaredInputs)
+{
+ if (leftStick == nullptr || rightStick == nullptr)
+ {
+ wpi_setWPIError(NullParameter);
+ return;
+ }
+ TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis), squaredInputs);
+}
+
+void RobotDrive::TankDrive(GenericHID &leftStick, uint32_t leftAxis,
+ GenericHID &rightStick, uint32_t rightAxis, bool squaredInputs)
+{
+ TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis), squaredInputs);
+}
+
+
+/**
+ * Provide tank steering using the stored robot configuration.
+ * This function lets you directly provide joystick values from any source.
+ * @param leftValue The value of the left stick.
+ * @param rightValue The value of the right stick.
+ */
+void RobotDrive::TankDrive(float leftValue, float rightValue, bool squaredInputs)
+{
+ static bool reported = false;
+ if (!reported)
+ {
+ reported = true;
+ }
+
+ // square the inputs (while preserving the sign) to increase fine control while permitting full power
+ leftValue = Limit(leftValue);
+ rightValue = Limit(rightValue);
+ if(squaredInputs)
+ {
+ if (leftValue >= 0.0)
+ {
+ leftValue = (leftValue * leftValue);
+ }
+ else
+ {
+ leftValue = -(leftValue * leftValue);
+ }
+ if (rightValue >= 0.0)
+ {
+ rightValue = (rightValue * rightValue);
+ }
+ else
+ {
+ rightValue = -(rightValue * rightValue);
+ }
+ }
+
+ SetLeftRightMotorOutputs(leftValue, rightValue);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ * Given a single Joystick, the class assumes the Y axis for the move value and the X axis
+ * for the rotate value.
+ * (Should add more information here regarding the way that arcade drive works.)
+ * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
+ * for forwards/backwards and the X-axis will be selected for rotation rate.
+ * @param squaredInputs If true, the sensitivity will be increased for small values
+ */
+void RobotDrive::ArcadeDrive(GenericHID *stick, bool squaredInputs)
+{
+ // simply call the full-featured ArcadeDrive with the appropriate values
+ ArcadeDrive(stick->GetY(), stick->GetX(), squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ * Given a single Joystick, the class assumes the Y axis for the move value and the X axis
+ * for the rotate value.
+ * (Should add more information here regarding the way that arcade drive works.)
+ * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
+ * for forwards/backwards and the X-axis will be selected for rotation rate.
+ * @param squaredInputs If true, the sensitivity will be increased for small values
+ */
+void RobotDrive::ArcadeDrive(GenericHID &stick, bool squaredInputs)
+{
+ // simply call the full-featured ArcadeDrive with the appropriate values
+ ArcadeDrive(stick.GetY(), stick.GetX(), squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ * Given two joystick instances and two axis, compute the values to send to either two
+ * or four motors.
+ * @param moveStick The Joystick object that represents the forward/backward direction
+ * @param moveAxis The axis on the moveStick object to use for fowards/backwards (typically Y_AXIS)
+ * @param rotateStick The Joystick object that represents the rotation value
+ * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
+ * @param squaredInputs Setting this parameter to true increases the sensitivity at lower speeds
+ */
+void RobotDrive::ArcadeDrive(GenericHID* moveStick, uint32_t moveAxis,
+ GenericHID* rotateStick, uint32_t rotateAxis,
+ bool squaredInputs)
+{
+ float moveValue = moveStick->GetRawAxis(moveAxis);
+ float rotateValue = rotateStick->GetRawAxis(rotateAxis);
+
+ ArcadeDrive(moveValue, rotateValue, squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ * Given two joystick instances and two axis, compute the values to send to either two
+ * or four motors.
+ * @param moveStick The Joystick object that represents the forward/backward direction
+ * @param moveAxis The axis on the moveStick object to use for fowards/backwards (typically Y_AXIS)
+ * @param rotateStick The Joystick object that represents the rotation value
+ * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
+ * @param squaredInputs Setting this parameter to true increases the sensitivity at lower speeds
+ */
+
+void RobotDrive::ArcadeDrive(GenericHID &moveStick, uint32_t moveAxis,
+ GenericHID &rotateStick, uint32_t rotateAxis,
+ bool squaredInputs)
+{
+ float moveValue = moveStick.GetRawAxis(moveAxis);
+ float rotateValue = rotateStick.GetRawAxis(rotateAxis);
+
+ ArcadeDrive(moveValue, rotateValue, squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ * This function lets you directly provide joystick values from any source.
+ * @param moveValue The value to use for fowards/backwards
+ * @param rotateValue The value to use for the rotate right/left
+ * @param squaredInputs If set, increases the sensitivity at low speeds
+ */
+void RobotDrive::ArcadeDrive(float moveValue, float rotateValue, bool squaredInputs)
+{
+ static bool reported = false;
+ if (!reported)
+ {
+ reported = true;
+ }
+
+ // local variables to hold the computed PWM values for the motors
+ float leftMotorOutput;
+ float rightMotorOutput;
+
+ moveValue = Limit(moveValue);
+ rotateValue = Limit(rotateValue);
+
+ if (squaredInputs)
+ {
+ // square the inputs (while preserving the sign) to increase fine control while permitting full power
+ if (moveValue >= 0.0)
+ {
+ moveValue = (moveValue * moveValue);
+ }
+ else
+ {
+ moveValue = -(moveValue * moveValue);
+ }
+ if (rotateValue >= 0.0)
+ {
+ rotateValue = (rotateValue * rotateValue);
+ }
+ else
+ {
+ rotateValue = -(rotateValue * rotateValue);
+ }
+ }
+
+ if (moveValue > 0.0)
+ {
+ if (rotateValue > 0.0)
+ {
+ leftMotorOutput = moveValue - rotateValue;
+ rightMotorOutput = std::max(moveValue, rotateValue);
+ }
+ else
+ {
+ leftMotorOutput = std::max(moveValue, -rotateValue);
+ rightMotorOutput = moveValue + rotateValue;
+ }
+ }
+ else
+ {
+ if (rotateValue > 0.0)
+ {
+ leftMotorOutput = - std::max(-moveValue, rotateValue);
+ rightMotorOutput = moveValue + rotateValue;
+ }
+ else
+ {
+ leftMotorOutput = moveValue - rotateValue;
+ rightMotorOutput = - std::max(-moveValue, -rotateValue);
+ }
+ }
+ SetLeftRightMotorOutputs(leftMotorOutput, rightMotorOutput);
+}
+
+/**
+ * Drive method for Mecanum wheeled robots.
+ *
+ * A method for driving with Mecanum wheeled robots. There are 4 wheels
+ * on the robot, arranged so that the front and back wheels are toed in 45 degrees.
+ * When looking at the wheels from the top, the roller axles should form an X across the robot.
+ *
+ * This is designed to be directly driven by joystick axes.
+ *
+ * @param x The speed that the robot should drive in the X direction. [-1.0..1.0]
+ * @param y The speed that the robot should drive in the Y direction.
+ * This input is inverted to match the forward == -1.0 that joysticks produce. [-1.0..1.0]
+ * @param rotation The rate of rotation for the robot that is completely independent of
+ * the translation. [-1.0..1.0]
+ * @param gyroAngle The current angle reading from the gyro. Use this to implement field-oriented controls.
+ */
+void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation, float gyroAngle)
+{
+ static bool reported = false;
+ if (!reported)
+ {
+ reported = true;
+ }
+
+ double xIn = x;
+ double yIn = y;
+ // Negate y for the joystick.
+ yIn = -yIn;
+ // Compenstate for gyro angle.
+ RotateVector(xIn, yIn, gyroAngle);
+
+ double wheelSpeeds[kMaxNumberOfMotors];
+ wheelSpeeds[kFrontLeftMotor] = xIn + yIn + rotation;
+ wheelSpeeds[kFrontRightMotor] = -xIn + yIn - rotation;
+ wheelSpeeds[kRearLeftMotor] = -xIn + yIn + rotation;
+ wheelSpeeds[kRearRightMotor] = xIn + yIn - rotation;
+
+ Normalize(wheelSpeeds);
+
+ uint8_t syncGroup = 0x80;
+
+ m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
+ m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
+ m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
+ m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
+
+ // CANJaguar::UpdateSyncGroup(syncGroup);
+
+ // FIXME: m_safetyHelper->Feed();
+}
+
+/**
+ * Drive method for Mecanum wheeled robots.
+ *
+ * A method for driving with Mecanum wheeled robots. There are 4 wheels
+ * on the robot, arranged so that the front and back wheels are toed in 45 degrees.
+ * When looking at the wheels from the top, the roller axles should form an X across the robot.
+ *
+ * @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
+ * @param direction The direction the robot should drive in degrees. The direction and maginitute are
+ * independent of the rotation rate.
+ * @param rotation The rate of rotation for the robot that is completely independent of
+ * the magnitute or direction. [-1.0..1.0]
+ */
+void RobotDrive::MecanumDrive_Polar(float magnitude, float direction, float rotation)
+{
+ static bool reported = false;
+ if (!reported)
+ {
+ reported = true;
+ }
+
+ // Normalized for full power along the Cartesian axes.
+ magnitude = Limit(magnitude) * sqrt(2.0);
+ // The rollers are at 45 degree angles.
+ double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
+ double cosD = cos(dirInRad);
+ double sinD = sin(dirInRad);
+
+ double wheelSpeeds[kMaxNumberOfMotors];
+ wheelSpeeds[kFrontLeftMotor] = sinD * magnitude + rotation;
+ wheelSpeeds[kFrontRightMotor] = cosD * magnitude - rotation;
+ wheelSpeeds[kRearLeftMotor] = cosD * magnitude + rotation;
+ wheelSpeeds[kRearRightMotor] = sinD * magnitude - rotation;
+
+ Normalize(wheelSpeeds);
+
+ uint8_t syncGroup = 0x80;
+
+ m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
+ m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
+ m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
+ m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
+
+ // CANJaguar::UpdateSyncGroup(syncGroup);
+
+ // FIXME: m_safetyHelper->Feed();
+}
+
+/**
+ * Holonomic Drive method for Mecanum wheeled robots.
+ *
+ * This is an alias to MecanumDrive_Polar() for backward compatability
+ *
+ * @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
+ * @param direction The direction the robot should drive. The direction and 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::HolonomicDrive(float magnitude, float direction, float rotation)
+{
+ MecanumDrive_Polar(magnitude, direction, rotation);
+}
+
+/** Set the speed of the right and left motors.
+ * This is used once an appropriate drive setup function is called such as
+ * TwoWheelDrive(). The motors are set to "leftOutput" and "rightOutput"
+ * and includes flipping the direction of one side for opposing motors.
+ * @param leftOutput The speed to send to the left side of the robot.
+ * @param rightOutput The speed to send to the right side of the robot.
+ */
+void RobotDrive::SetLeftRightMotorOutputs(float leftOutput, float rightOutput)
+{
+ wpi_assert(m_rearLeftMotor != nullptr && m_rearRightMotor != nullptr);
+
+ uint8_t syncGroup = 0x80;
+
+ if (m_frontLeftMotor != nullptr)
+ m_frontLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
+ m_rearLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
+
+ if (m_frontRightMotor != nullptr)
+ m_frontRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
+ m_rearRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
+
+ // CANJaguar::UpdateSyncGroup(syncGroup);
+
+ // FIXME: m_safetyHelper->Feed();
+}
+
+/**
+ * Limit motor values to the -1.0 to +1.0 range.
+ */
+float RobotDrive::Limit(float num)
+{
+ if (num > 1.0)
+ {
+ return 1.0;
+ }
+ if (num < -1.0)
+ {
+ return -1.0;
+ }
+ return num;
+}
+
+/**
+ * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
+ */
+void RobotDrive::Normalize(double *wheelSpeeds)
+{
+ double maxMagnitude = fabs(wheelSpeeds[0]);
+ int32_t i;
+ for (i=1; i<kMaxNumberOfMotors; i++)
+ {
+ double temp = fabs(wheelSpeeds[i]);
+ if (maxMagnitude < temp) maxMagnitude = temp;
+ }
+ if (maxMagnitude > 1.0)
+ {
+ for (i=0; i<kMaxNumberOfMotors; i++)
+ {
+ wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
+ }
+ }
+}
+
+/**
+ * Rotate a vector in Cartesian space.
+ */
+void RobotDrive::RotateVector(double &x, double &y, double angle)
+{
+ double cosA = cos(angle * (3.14159 / 180.0));
+ double sinA = sin(angle * (3.14159 / 180.0));
+ double xOut = x * cosA - y * sinA;
+ double yOut = x * sinA + y * cosA;
+ x = xOut;
+ y = yOut;
+}
+
+/*
+ * Invert a motor direction.
+ * This is used when a motor should run in the opposite direction as the drive
+ * code would normally run it. Motors that are direct drive would be inverted, the
+ * Drive code assumes that the motors are geared with one reversal.
+ * @param motor The motor index to invert.
+ * @param isInverted True if the motor should be inverted when operated.
+ */
+void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted)
+{
+ if (motor < 0 || motor > 3)
+ {
+ wpi_setWPIError(InvalidMotorIndex);
+ return;
+ }
+ m_invertedMotors[motor] = isInverted ? -1 : 1;
+}
+
+/**
+ * Set the turning sensitivity.
+ *
+ * This only impacts the Drive() entry-point.
+ * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
+ */
+void RobotDrive::SetSensitivity(float sensitivity)
+{
+ m_sensitivity = sensitivity;
+}
+
+/**
+ * Configure the scaling factor for using RobotDrive with motor controllers in a mode other than PercentVbus.
+ * @param maxOutput Multiplied with the output percentage computed by the drive functions.
+ */
+void RobotDrive::SetMaxOutput(double maxOutput)
+{
+ m_maxOutput = maxOutput;
+}
+
+
+
+void RobotDrive::SetExpiration(float timeout)
+{
+ // FIXME: m_safetyHelper->SetExpiration(timeout);
+}
+
+float RobotDrive::GetExpiration() const
+{
+ return -1; // FIXME: return m_safetyHelper->GetExpiration();
+}
+
+bool RobotDrive::IsAlive() const
+{
+ return true; // FIXME: m_safetyHelper->IsAlive();
+}
+
+bool RobotDrive::IsSafetyEnabled() const
+{
+ return false; // FIXME: return m_safetyHelper->IsSafetyEnabled();
+}
+
+void RobotDrive::SetSafetyEnabled(bool enabled)
+{
+ // FIXME: m_safetyHelper->SetSafetyEnabled(enabled);
+}
+
+void RobotDrive::GetDescription(std::ostringstream& desc) const
+{
+ desc << "RobotDrive";
+}
+
+void RobotDrive::StopMotor()
+{
+ if (m_frontLeftMotor != nullptr) m_frontLeftMotor->Disable();
+ if (m_frontRightMotor != nullptr) m_frontRightMotor->Disable();
+ if (m_rearLeftMotor != nullptr) m_rearLeftMotor->Disable();
+ if (m_rearRightMotor != nullptr) m_rearRightMotor->Disable();
+}
diff --git a/wpilibc/simulation/src/SafePWM.cpp b/wpilibc/simulation/src/SafePWM.cpp
new file mode 100644
index 0000000..8ba0b68
--- /dev/null
+++ b/wpilibc/simulation/src/SafePWM.cpp
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "SafePWM.h"
+#include <sstream>
+#include <memory>
+
+/**
+ * Constructor for a SafePWM object taking a channel number.
+ * @param channel The PWM channel number (0..19).
+ */
+SafePWM::SafePWM(uint32_t channel): PWM(channel)
+{
+ m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
+ m_safetyHelper->SetSafetyEnabled(false);
+}
+
+/*
+ * Set the expiration time for the PWM object
+ * @param timeout The timeout (in seconds) for this motor object
+ */
+void SafePWM::SetExpiration(float timeout)
+{
+ m_safetyHelper->SetExpiration(timeout);
+}
+
+/**
+ * Return the expiration time for the PWM object.
+ * @returns The expiration time value.
+ */
+float SafePWM::GetExpiration() const
+{
+ return m_safetyHelper->GetExpiration();
+}
+
+/**
+ * Check if the PWM object is currently alive or stopped due to a timeout.
+ * @returns a bool value that is true if the motor has NOT timed out and should still
+ * be running.
+ */
+bool SafePWM::IsAlive() const
+{
+ return m_safetyHelper->IsAlive();
+}
+
+/**
+ * Stop the motor associated with this PWM object.
+ * This is called by the MotorSafetyHelper object when it has a timeout for this PWM and needs to
+ * stop it from running.
+ */
+void SafePWM::StopMotor()
+{
+ SetRaw(kPwmDisabled);
+}
+
+/**
+ * Enable/disable motor safety for this device
+ * Turn on and off the motor safety option for this PWM object.
+ * @param enabled True if motor safety is enforced for this object
+ */
+void SafePWM::SetSafetyEnabled(bool enabled)
+{
+ m_safetyHelper->SetSafetyEnabled(enabled);
+}
+
+/**
+ * Check if motor safety is enabled for this object
+ * @returns True if motor safety is enforced for this object
+ */
+bool SafePWM::IsSafetyEnabled() const
+{
+ return m_safetyHelper->IsSafetyEnabled();
+}
+
+void SafePWM::GetDescription(std::ostringstream& desc) const
+{
+ desc << "PWM " << GetChannel();
+}
+
+/**
+ * Feed the MotorSafety timer when setting the speed.
+ * This method is called by the subclass motor whenever it updates its speed, thereby reseting
+ * the timeout value.
+ * @param speed Value to pass to the PWM class
+ */
+void SafePWM::SetSpeed(float speed)
+{
+ PWM::SetSpeed(speed);
+ m_safetyHelper->Feed();
+}
diff --git a/wpilibc/simulation/src/SampleRobot.cpp b/wpilibc/simulation/src/SampleRobot.cpp
new file mode 100644
index 0000000..b8f3181
--- /dev/null
+++ b/wpilibc/simulation/src/SampleRobot.cpp
@@ -0,0 +1,157 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "SampleRobot.h"
+
+#include <stdio.h>
+#include "Timer.h"
+#include "SmartDashboard/SmartDashboard.h"
+#include "LiveWindow/LiveWindow.h"
+#include "networktables/NetworkTable.h"
+
+#if defined(_UNIX)
+ #include <unistd.h>
+#elif defined(_WIN32)
+ #include <windows.h>
+ void sleep(unsigned milliseconds)
+ {
+ Sleep(milliseconds);
+ }
+#endif
+
+
+SampleRobot::SampleRobot()
+ : m_robotMainOverridden (true)
+{
+}
+
+/**
+ * Robot-wide initialization code should go here.
+ *
+ * Programmers should override this method for default Robot-wide initialization which will
+ * be called each time the robot enters the disabled state.
+ */
+void SampleRobot::RobotInit()
+{
+ printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Disabled should go here.
+ * Programmers should override this method to run code that should run while the field is
+ * disabled.
+ */
+void SampleRobot::Disabled()
+{
+ printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Autonomous should go here.
+ * Programmers should override this method to run code that should run while the field is
+ * in the autonomous period. This will be called once each time the robot enters the
+ * autonomous state.
+ */
+void SampleRobot::Autonomous()
+{
+ printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Operator control (tele-operated) code should go here.
+ * Programmers should override this method to run code that should run while the field is
+ * in the Operator Control (tele-operated) period. This is called once each time the robot
+ * enters the teleop state.
+ */
+void SampleRobot::OperatorControl()
+{
+ printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Test program should go here.
+ * Programmers should override this method to run code that executes while the robot is
+ * in test mode. This will be called once whenever the robot enters test mode
+ */
+void SampleRobot::Test()
+{
+ printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Robot main program for free-form programs.
+ *
+ * This should be overridden by user subclasses if the intent is to not use the Autonomous() and
+ * OperatorControl() methods. In that case, the program is responsible for sensing when to run
+ * the autonomous and operator control functions in their program.
+ *
+ * This method will be called immediately after the constructor is called. If it has not been
+ * overridden by a user subclass (i.e. the default version runs), then the Autonomous() and
+ * OperatorControl() methods will be called.
+ */
+void SampleRobot::RobotMain()
+{
+ m_robotMainOverridden = false;
+}
+
+/**
+ * Start a competition.
+ * This code needs to track the order of the field starting to ensure that everything happens
+ * in the right order. Repeatedly run the correct method, either Autonomous or OperatorControl
+ * or Test when the robot is enabled. After running the correct method, wait for some state to
+ * change, either the other mode starts or the robot is disabled. Then go back and wait for the
+ * robot to be enabled again.
+ */
+void SampleRobot::StartCompetition()
+{
+ LiveWindow *lw = LiveWindow::GetInstance();
+
+ SmartDashboard::init();
+ NetworkTable::GetTable("LiveWindow")->GetSubTable("~STATUS~")->PutBoolean("LW Enabled", false);
+
+ RobotMain();
+
+ if (!m_robotMainOverridden)
+ {
+ // first and one-time initialization
+ lw->SetEnabled(false);
+ RobotInit();
+
+ while (true)
+ {
+ if (IsDisabled())
+ {
+ m_ds.InDisabled(true);
+ Disabled();
+ m_ds.InDisabled(false);
+ while (IsDisabled()) sleep(1); //m_ds.WaitForData();
+ }
+ else if (IsAutonomous())
+ {
+ m_ds.InAutonomous(true);
+ Autonomous();
+ m_ds.InAutonomous(false);
+ while (IsAutonomous() && IsEnabled()) sleep(1); //m_ds.WaitForData();
+ }
+ else if (IsTest())
+ {
+ lw->SetEnabled(true);
+ m_ds.InTest(true);
+ Test();
+ m_ds.InTest(false);
+ while (IsTest() && IsEnabled()) sleep(1); //m_ds.WaitForData();
+ lw->SetEnabled(false);
+ }
+ else
+ {
+ m_ds.InOperatorControl(true);
+ OperatorControl();
+ m_ds.InOperatorControl(false);
+ while (IsOperatorControl() && IsEnabled()) sleep(1); //m_ds.WaitForData();
+ }
+ }
+ }
+}
diff --git a/wpilibc/simulation/src/SensorBase.cpp b/wpilibc/simulation/src/SensorBase.cpp
new file mode 100644
index 0000000..fa3086f
--- /dev/null
+++ b/wpilibc/simulation/src/SensorBase.cpp
@@ -0,0 +1,161 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "SensorBase.h"
+
+#include "WPIErrors.h"
+
+const uint32_t SensorBase::kDigitalChannels;
+const uint32_t SensorBase::kAnalogInputs;
+const uint32_t SensorBase::kSolenoidChannels;
+const uint32_t SensorBase::kSolenoidModules;
+const uint32_t SensorBase::kPwmChannels;
+const uint32_t SensorBase::kRelayChannels;
+const uint32_t SensorBase::kPDPChannels;
+const uint32_t SensorBase::kChassisSlots;
+SensorBase *SensorBase::m_singletonList = nullptr;
+
+/**
+ * Creates an instance of the sensor base and gets an FPGA handle
+ */
+SensorBase::SensorBase()
+{
+}
+
+/**
+ * Add sensor to the singleton list.
+ * Add this sensor to the list of singletons that need to be deleted when
+ * the robot program exits. Each of the sensors on this list are singletons,
+ * that is they aren't allocated directly with new, but instead are allocated
+ * by the static GetInstance method. As a result, they are never deleted when
+ * the program exits. Consequently these sensors may still be holding onto
+ * resources and need to have their destructors called at the end of the program.
+ */
+void SensorBase::AddToSingletonList()
+{
+ m_nextSingleton = m_singletonList;
+ m_singletonList = this;
+}
+
+/**
+ * Delete all the singleton classes on the list.
+ * All the classes that were allocated as singletons need to be deleted so
+ * their resources can be freed.
+ */
+void SensorBase::DeleteSingletons()
+{
+ for (SensorBase *next = m_singletonList; next != nullptr;)
+ {
+ SensorBase *tmp = next;
+ next = next->m_nextSingleton;
+ delete tmp;
+ }
+ m_singletonList = nullptr;
+}
+
+/**
+ * Check that the solenoid module number is valid.
+ *
+ * @return Solenoid module is valid and present
+ */
+bool SensorBase::CheckSolenoidModule(uint8_t moduleNumber)
+{
+ return 1 <= moduleNumber && moduleNumber <= 2; // TODO: Fix for Athena
+}
+
+/**
+ * Check that the digital channel number is valid.
+ * Verify that the channel number is one of the legal channel numbers. Channel numbers are
+ * 1-based.
+ *
+ * @return Digital channel is valid
+ */
+bool SensorBase::CheckDigitalChannel(uint32_t channel)
+{
+ if (channel > 0 && channel <= kDigitalChannels)
+ return true;
+ return false;
+}
+
+/**
+ * Check that the digital channel number is valid.
+ * Verify that the channel number is one of the legal channel numbers. Channel numbers are
+ * 1-based.
+ *
+ * @return Relay channel is valid
+ */
+bool SensorBase::CheckRelayChannel(uint32_t channel)
+{
+ if (channel > 0 && channel <= kRelayChannels)
+ return true;
+ return false;
+}
+
+/**
+ * Check that the digital channel number is valid.
+ * Verify that the channel number is one of the legal channel numbers. Channel numbers are
+ * 1-based.
+ *
+ * @return PWM channel is valid
+ */
+bool SensorBase::CheckPWMChannel(uint32_t channel)
+{
+ if (channel > 0 && channel <= kPwmChannels)
+ return true;
+ return false;
+}
+
+/**
+ * Check that the analog input number is valid.
+ * Verify that the analog input number is one of the legal channel numbers. Channel numbers
+ * are 1-based.
+ *
+ * @return Analog channel is valid
+ */
+bool SensorBase::CheckAnalogInput(uint32_t channel)
+{
+ if (channel > 0 && channel <= kAnalogInputs)
+ return true;
+ return false;
+}
+
+/**
+ * Check that the analog output number is valid.
+ * Verify that the analog output number is one of the legal channel numbers. Channel numbers
+ * are 1-based.
+ *
+ * @return Analog channel is valid
+ */
+bool SensorBase::CheckAnalogOutput(uint32_t channel)
+{
+ if (channel > 0 && channel <= kAnalogOutputs)
+ return true;
+ return false;
+}
+
+/**
+ * Verify that the solenoid channel number is within limits.
+ *
+ * @return Solenoid channel is valid
+ */
+bool SensorBase::CheckSolenoidChannel(uint32_t channel)
+{
+ if (channel > 0 && channel <= kSolenoidChannels)
+ return true;
+ return false;
+}
+
+/**
+ * Verify that the power distribution channel number is within limits.
+ *
+ * @return PDP channel is valid
+ */
+bool SensorBase::CheckPDPChannel(uint32_t channel)
+{
+ if (channel > 0 && channel <= kPDPChannels)
+ return true;
+ return false;
+}
diff --git a/wpilibc/simulation/src/Solenoid.cpp b/wpilibc/simulation/src/Solenoid.cpp
new file mode 100644
index 0000000..387194d
--- /dev/null
+++ b/wpilibc/simulation/src/Solenoid.cpp
@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Solenoid.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+#include "simulation/simTime.h"
+
+/**
+ * Constructor.
+ *
+ * @param channel The channel on the solenoid module to control (1..8).
+ */
+Solenoid::Solenoid(uint32_t channel) : Solenoid(1, channel) {}
+
+/**
+ * Constructor.
+ *
+ * @param moduleNumber The solenoid module (1 or 2).
+ * @param channel The channel on the solenoid module to control (1..8).
+ */
+Solenoid::Solenoid(uint8_t moduleNumber, uint32_t channel)
+{
+ char buffer[50];
+ int n = sprintf(buffer, "pneumatic/%d/%d", moduleNumber, channel);
+ m_impl = new SimContinuousOutput(buffer);
+
+ LiveWindow::GetInstance()->AddActuator("Solenoid", moduleNumber, channel,
+ this);
+}
+
+Solenoid::~Solenoid() {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Set the value of a solenoid.
+ *
+ * @param on Turn the solenoid output off or on.
+ */
+void Solenoid::Set(bool on)
+{
+ m_on = on;
+ m_impl->Set(on ? 1 : -1);
+}
+
+/**
+ * Read the current value of the solenoid.
+ *
+ * @return The current value of the solenoid.
+ */
+bool Solenoid::Get() const
+{
+ return m_on;
+}
+
+
+void Solenoid::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (!value->IsBoolean()) return;
+ Set(value->GetBoolean());
+}
+
+void Solenoid::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutBoolean("Value", Get());
+ }
+}
+
+void Solenoid::StartLiveWindowMode() {
+ Set(false);
+ if (m_table != nullptr) {
+ m_table->AddTableListener("Value", this, true);
+ }
+}
+
+void Solenoid::StopLiveWindowMode() {
+ Set(false);
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string Solenoid::GetSmartDashboardType() const {
+ return "Solenoid";
+}
+
+void Solenoid::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> Solenoid::GetTable() const {
+ return m_table;
+}
diff --git a/wpilibc/simulation/src/Talon.cpp b/wpilibc/simulation/src/Talon.cpp
new file mode 100644
index 0000000..0e9505d
--- /dev/null
+++ b/wpilibc/simulation/src/Talon.cpp
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Talon.h"
+
+//#include "NetworkCommunication/UsageReporting.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * @param channel The PWM channel that the Talon is attached to.
+ */
+Talon::Talon(uint32_t channel) : SafePWM(channel)
+{
+ /* Note that the Talon uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the Talon User Manual available
+ * from CTRE.
+ *
+ * - 211 = full "forward"
+ * - 133 = the "high end" of the deadband range
+ * - 129 = center of the deadband range (off)
+ * - 125 = the "low end" of the deadband range
+ * - 49 = full "reverse"
+ */
+ SetBounds(2.037, 1.539, 1.513, 1.487, .989);
+ SetPeriodMultiplier(kPeriodMultiplier_2X);
+ SetRaw(m_centerPwm);
+
+ LiveWindow::GetInstance()->AddActuator("Talon", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ * @param syncGroup Unused interface.
+ */
+void Talon::Set(float speed, uint8_t syncGroup)
+{
+ SetSpeed(speed);
+}
+
+/**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+float Talon::Get() const
+{
+ return GetSpeed();
+}
+
+/**
+ * Common interface for disabling a motor.
+ */
+void Talon::Disable()
+{
+ SetRaw(kPwmDisabled);
+}
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void Talon::PIDWrite(float output)
+{
+ Set(output);
+}
diff --git a/wpilibc/simulation/src/Timer.cpp b/wpilibc/simulation/src/Timer.cpp
new file mode 100644
index 0000000..b4734db
--- /dev/null
+++ b/wpilibc/simulation/src/Timer.cpp
@@ -0,0 +1,208 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Timer.h"
+
+#include <time.h>
+
+#include "simulation/simTime.h"
+#include "Utility.h"
+
+
+// Internal stuff
+#include "simulation/SimFloatInput.h"
+#include "simulation/MainNode.h"
+namespace wpilib { namespace internal {
+ double simTime = 0;
+ std::condition_variable time_wait;
+ std::mutex time_wait_mutex;
+
+ void time_callback(const msgs::ConstFloat64Ptr &msg) {
+ simTime = msg->data();
+ time_wait.notify_all();
+ }
+}}
+
+/**
+ * Pause the task for a specified time.
+ *
+ * Pause the execution of the program for a specified period of time given in seconds.
+ * Motors will continue to run at their last assigned values, and sensors will continue to
+ * update. Only the task containing the wait will pause until the wait time is expired.
+ *
+ * @param seconds Length of time to pause, in seconds.
+ */
+void Wait(double seconds)
+{
+ if (seconds < 0.0) return;
+
+ double start = wpilib::internal::simTime;
+
+ std::unique_lock<std::mutex> lock(wpilib::internal::time_wait_mutex);
+ while ((wpilib::internal::simTime - start) < seconds) {
+ wpilib::internal::time_wait.wait(lock);
+ }
+}
+
+/*
+ * Return the FPGA system clock time in seconds.
+ * This is deprecated and just forwards to Timer::GetFPGATimestamp().
+ * @returns Robot running time in seconds.
+ */
+double GetClock()
+{
+ return Timer::GetFPGATimestamp();
+}
+
+/**
+ * @brief Gives real-time clock system time with nanosecond resolution
+ * @return The time, just in case you want the robot to start autonomous at 8pm on Saturday (except in simulation).
+*/
+double GetTime()
+{
+ return Timer::GetFPGATimestamp(); // The epoch starts when Gazebo starts
+}
+
+//for compatibility with msvc12--see C2864
+const double Timer::kRolloverTime = (1ll << 32) / 1e6;
+/**
+ * Create a new timer object.
+ *
+ * Create a new timer object and reset the time to zero. The timer is initially not running and
+ * must be started.
+ */
+Timer::Timer()
+ : m_startTime (0.0)
+ , m_accumulatedTime (0.0)
+ , m_running (false)
+{
+ //Creates a semaphore to control access to critical regions.
+ //Initially 'open'
+ Reset();
+}
+
+/**
+ * Get the current time from the timer. If the clock is running it is derived from
+ * the current system clock the start time stored in the timer class. If the clock
+ * is not running, then return the time when it was last stopped.
+ *
+ * @return unsigned Current time value for this timer in seconds
+ */
+double Timer::Get() const
+{
+ double result;
+ double currentTime = GetFPGATimestamp();
+
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ if(m_running)
+ {
+ // This math won't work if the timer rolled over (71 minutes after boot).
+ // TODO: Check for it and compensate.
+ result = (currentTime - m_startTime) + m_accumulatedTime;
+ }
+ else
+ {
+ result = m_accumulatedTime;
+ }
+
+ return result;
+}
+
+/**
+ * Reset the timer by setting the time to 0.
+ *
+ * Make the timer startTime the current time so new requests will be relative to now
+ */
+void Timer::Reset()
+{
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_accumulatedTime = 0;
+ m_startTime = GetFPGATimestamp();
+}
+
+/**
+ * Start the timer running.
+ * Just set the running flag to true indicating that all time requests should be
+ * relative to the system clock.
+ */
+void Timer::Start()
+{
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ if (!m_running)
+ {
+ m_startTime = GetFPGATimestamp();
+ m_running = true;
+ }
+}
+
+/**
+ * Stop the timer.
+ * This computes the time as of now and clears the running flag, causing all
+ * subsequent time requests to be read from the accumulated time rather than
+ * looking at the system clock.
+ */
+void Timer::Stop()
+{
+ double temp = Get();
+
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ if (m_running)
+ {
+ m_accumulatedTime = temp;
+ m_running = false;
+ }
+}
+
+/**
+ * Check if the period specified has passed and if it has, advance the start
+ * time by that period. This is useful to decide if it's time to do periodic
+ * work without drifting later by the time it took to get around to checking.
+ *
+ * @param period The period to check for (in seconds).
+ * @return If the period has passed.
+ */
+bool Timer::HasPeriodPassed(double period)
+{
+ if (Get() > period)
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ // Advance the start time by the period.
+ // Don't set it to the current time... we want to avoid drift.
+ m_startTime += period;
+ return true;
+ }
+ return false;
+}
+
+/*
+ * Return the FPGA system clock time in seconds.
+ *
+ * Return the time from the FPGA hardware clock in seconds since the FPGA
+ * started.
+ * Rolls over after 71 minutes.
+ * @returns Robot running time in seconds.
+ */
+double Timer::GetFPGATimestamp()
+{
+ // FPGA returns the timestamp in microseconds
+ // Call the helper GetFPGATime() in Utility.cpp
+ return wpilib::internal::simTime;
+}
+
+/*
+ * Not in a match.
+ */
+double Timer::GetMatchTime()
+{
+ return Timer::GetFPGATimestamp();
+}
+
+// Internal function that reads the PPC timestamp counter.
+extern "C"
+{
+ uint32_t niTimestamp32(void);
+ uint64_t niTimestamp64(void);
+}
diff --git a/wpilibc/simulation/src/Utility.cpp b/wpilibc/simulation/src/Utility.cpp
new file mode 100644
index 0000000..b0710f1
--- /dev/null
+++ b/wpilibc/simulation/src/Utility.cpp
@@ -0,0 +1,235 @@
+
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Utility.h"
+
+#include "Timer.h"
+#include "simulation/simTime.h"
+#include <stdio.h>
+#include <string.h>
+
+#include <iostream>
+#include <sstream>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#if defined(UNIX)
+ #include <execinfo.h>
+ #include <cxxabi.h>
+#endif
+
+static bool stackTraceEnabled = false;
+static bool suspendOnAssertEnabled = false;
+
+/**
+ * Enable Stack trace after asserts.
+ */
+void wpi_stackTraceOnAssertEnable(bool enabled)
+{
+ stackTraceEnabled = enabled;
+}
+
+/**
+ * Enable suspend on asssert.
+ * If enabled, the user task will be suspended whenever an assert fails. This
+ * will allow the user to attach to the task with the debugger and examine variables
+ * around the failure.
+ */
+void wpi_suspendOnAssertEnabled(bool enabled)
+{
+ suspendOnAssertEnabled = enabled;
+}
+
+static void wpi_handleTracing()
+{
+ // if (stackTraceEnabled)
+ // {
+ // printf("\n-----------<Stack Trace>----------------\n");
+ // printCurrentStackTrace();
+ // }
+ printf("\n");
+}
+
+/**
+ * Assert implementation.
+ * This allows breakpoints to be set on an assert.
+ * The users don't call this, but instead use the wpi_assert macros in Utility.h.
+ */
+bool wpi_assert_impl(bool conditionValue, const std::string &conditionText,
+ const std::string &message, const std::string &fileName,
+ uint32_t lineNumber, const std::string &funcName) {
+ if (!conditionValue) {
+ // Error string buffer
+ std::stringstream error;
+
+ // If an error message was specified, include it
+ // Build error string
+ if (message.size()) {
+ error << "Assertion failed: \"" << message << "\", \"" << conditionText
+ << "\" failed in " << funcName + "() in " << fileName << " at line "
+ << lineNumber;
+ } else {
+ error << "Assertion failed: \"" << conditionText << "\" in " << funcName
+ << "() in " << fileName << " at line " << lineNumber;
+ }
+
+ // Print to console and send to remote dashboard
+ std::cout << "\n\n>>>>" << error.str();
+
+ wpi_handleTracing();
+ }
+ return conditionValue;
+}
+
+/**
+ * Common error routines for wpi_assertEqual_impl and wpi_assertNotEqual_impl
+ * This should not be called directly; it should only be used by wpi_assertEqual_impl
+ * and wpi_assertNotEqual_impl.
+ */
+void wpi_assertEqual_common_impl(int valueA, int valueB,
+ const std::string &equalityType,
+ const std::string &message,
+ const std::string &fileName,
+ uint32_t lineNumber,
+ const std::string &funcName) {
+ // Error string buffer
+ std::stringstream error;
+
+ // If an error message was specified, include it
+ // Build error string
+ if (message.size() > 0) {
+ error << "Assertion failed: \"" << message << "\", \"" << valueA << "\" "
+ << equalityType << " \"" << valueB << "\" in " << funcName << "() in "
+ << fileName << " at line " << lineNumber << "\n";
+ } else {
+ error << "Assertion failed: \"" << valueA << "\" " << equalityType << " \""
+ << valueB << "\" in " << funcName << "() in " << fileName
+ << " at line " << lineNumber << "\n";
+ }
+
+ // Print to console and send to remote dashboard
+ std::cout << "\n\n>>>>" << error.str();
+
+ wpi_handleTracing();
+}
+
+/**
+ * Assert equal implementation.
+ * This determines whether the two given integers are equal. If not,
+ * the value of each is printed along with an optional message string.
+ * The users don't call this, but instead use the wpi_assertEqual macros in Utility.h.
+ */
+bool wpi_assertEqual_impl(int valueA,
+ int valueB,
+ const std::string &message,
+ const std::string &fileName,
+ uint32_t lineNumber,
+ const std::string &funcName)
+{
+ if(!(valueA == valueB))
+ {
+ wpi_assertEqual_common_impl(valueA, valueB, "!=", message, fileName, lineNumber, funcName);
+ }
+ return valueA == valueB;
+}
+
+/**
+ * Assert not equal implementation.
+ * This determines whether the two given integers are equal. If so,
+ * the value of each is printed along with an optional message string.
+ * The users don't call this, but instead use the wpi_assertNotEqual macros in Utility.h.
+ */
+bool wpi_assertNotEqual_impl(int valueA,
+ int valueB,
+ const std::string &message,
+ const std::string &fileName,
+ uint32_t lineNumber,
+ const std::string &funcName)
+{
+ if(!(valueA != valueB))
+ {
+ wpi_assertEqual_common_impl(valueA, valueB, "==", message, fileName, lineNumber, funcName);
+ }
+ return valueA != valueB;
+}
+
+/**
+ * Read the microsecond-resolution timer on the FPGA.
+ *
+ * @return The current time in microseconds according to the FPGA (since FPGA reset).
+ */
+uint32_t GetFPGATime()
+{
+ return wpilib::internal::simTime * 1e6;
+}
+
+//TODO: implement symbol demangling and backtrace on windows
+#if defined(UNIX)
+
+/**
+ * Demangle a C++ symbol, used for printing stack traces.
+ */
+static std::string demangle(char const *mangledSymbol)
+{
+ char buffer[256];
+ size_t length;
+ int status;
+
+ if(sscanf(mangledSymbol, "%*[^(]%*[^_]%255[^)+]", buffer))
+ {
+ char *symbol = abi::__cxa_demangle(buffer, nullptr, &length, &status);
+
+ if(status == 0)
+ {
+ return symbol;
+ }
+ else
+ {
+ // If the symbol couldn't be demangled, it's probably a C function,
+ // so just return it as-is.
+ return buffer;
+ }
+ }
+
+
+ // If everything else failed, just return the mangled symbol
+ return mangledSymbol;
+}
+
+/**
+ * Get a stack trace, ignoring the first "offset" symbols.
+ */
+std::string GetStackTrace(uint32_t offset)
+{
+ void *stackTrace[128];
+ int stackSize = backtrace(stackTrace, 128);
+ char **mangledSymbols = backtrace_symbols(stackTrace, stackSize);
+ std::stringstream trace;
+
+ for(int i = offset; i < stackSize; i++)
+ {
+ // Only print recursive functions once in a row.
+ if(i == 0 ||stackTrace[i] != stackTrace[i - 1])
+ {
+ trace << "\tat " << demangle(mangledSymbols[i]) << std::endl;
+ }
+ }
+
+ free(mangledSymbols);
+
+ return trace.str();
+}
+
+#else
+static std::string demangle(char const *mangledSymbol)
+{
+ return "no demangling on windows";
+}
+std::string GetStackTrace(uint32_t offset)
+{
+ return "no stack trace on windows";
+}
+#endif
diff --git a/wpilibc/simulation/src/Victor.cpp b/wpilibc/simulation/src/Victor.cpp
new file mode 100644
index 0000000..54ba61f
--- /dev/null
+++ b/wpilibc/simulation/src/Victor.cpp
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Victor.h"
+
+//#include "NetworkCommunication/UsageReporting.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * @param channel The PWM channel that the Victor is attached to.
+ */
+Victor::Victor(uint32_t channel) : SafePWM(channel)
+{
+ /* Note that the Victor uses the following bounds for PWM values. These values
+ * were determined empirically and optimized for the Victor 888. These values
+ * should work reasonably well for Victor 884 controllers as well but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Victor 884 User
+ * Manual available from IFI.
+ *
+ * - 206 = full "forward"
+ * - 131 = the "high end" of the deadband range
+ * - 128 = center of the deadband range (off)
+ * - 125 = the "low end" of the deadband range
+ * - 56 = full "reverse"
+ */
+
+ SetBounds(2.027, 1.525, 1.507, 1.49, 1.026);
+ SetPeriodMultiplier(kPeriodMultiplier_2X);
+ SetRaw(m_centerPwm);
+
+ LiveWindow::GetInstance()->AddActuator("Victor", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ * @param syncGroup Unused interface.
+ */
+void Victor::Set(float speed, uint8_t syncGroup)
+{
+ SetSpeed(speed);
+}
+
+/**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+float Victor::Get() const
+{
+ return GetSpeed();
+}
+
+/**
+ * Common interface for disabling a motor.
+ */
+void Victor::Disable()
+{
+ SetRaw(kPwmDisabled);
+}
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void Victor::PIDWrite(float output)
+{
+ Set(output);
+}
diff --git a/wpilibc/simulation/src/simulation/SimContinuousOutput.cpp b/wpilibc/simulation/src/simulation/SimContinuousOutput.cpp
new file mode 100644
index 0000000..39dbf70
--- /dev/null
+++ b/wpilibc/simulation/src/simulation/SimContinuousOutput.cpp
@@ -0,0 +1,24 @@
+/*
+ * SimContinuousOutput.cpp
+ *
+ * Created on: May 28, 2014
+ * Author: alex
+ */
+
+#include "simulation/SimContinuousOutput.h"
+#include "simulation/MainNode.h"
+
+SimContinuousOutput::SimContinuousOutput(std::string topic) {
+ pub = MainNode::Advertise<msgs::Float64>("~/simulator/"+topic);
+ std::cout << "Initialized ~/simulator/"+topic << std::endl;
+}
+
+void SimContinuousOutput::Set(float speed) {
+ msgs::Float64 msg;
+ msg.set_data(speed);
+ pub->Publish(msg);
+}
+
+float SimContinuousOutput::Get() {
+ return speed;
+}
diff --git a/wpilibc/simulation/src/simulation/SimDigitalInput.cpp b/wpilibc/simulation/src/simulation/SimDigitalInput.cpp
new file mode 100644
index 0000000..b39f2ae
--- /dev/null
+++ b/wpilibc/simulation/src/simulation/SimDigitalInput.cpp
@@ -0,0 +1,22 @@
+/*
+ * SimDigitalInput.cpp
+ *
+ * Created on: May 28, 2014
+ * Author: alex
+ */
+
+#include "simulation/SimDigitalInput.h"
+#include "simulation/MainNode.h"
+
+SimDigitalInput::SimDigitalInput(std::string topic) {
+ sub = MainNode::Subscribe("~/simulator/"+topic, &SimDigitalInput::callback, this);
+ std::cout << "Initialized ~/simulator/"+topic << std::endl;
+}
+
+bool SimDigitalInput::Get() {
+ return value;
+}
+
+void SimDigitalInput::callback(const msgs::ConstBoolPtr &msg) {
+ value = msg->data();
+}
diff --git a/wpilibc/simulation/src/simulation/SimEncoder.cpp b/wpilibc/simulation/src/simulation/SimEncoder.cpp
new file mode 100644
index 0000000..b72b8f1
--- /dev/null
+++ b/wpilibc/simulation/src/simulation/SimEncoder.cpp
@@ -0,0 +1,54 @@
+
+#include "simulation/SimEncoder.h"
+#include "simulation/MainNode.h"
+
+SimEncoder::SimEncoder(std::string topic) {
+ commandPub = MainNode::Advertise<msgs::GzString>("~/simulator/"+topic+"/control");
+
+ posSub = MainNode::Subscribe("~/simulator/"+topic+"/position",
+ &SimEncoder::positionCallback, this);
+ velSub = MainNode::Subscribe("~/simulator/"+topic+"/velocity",
+ &SimEncoder::velocityCallback, this);
+
+ if (commandPub->WaitForConnection(gazebo::common::Time(5.0))) { // Wait up to five seconds.
+ std::cout << "Initialized ~/simulator/" + topic << std::endl;
+ } else {
+ std::cerr << "Failed to initialize ~/simulator/" + topic + ": does the encoder exist?" << std::endl;
+ }
+}
+
+void SimEncoder::Reset() {
+ sendCommand("reset");
+}
+
+void SimEncoder::Start() {
+ sendCommand("start");
+}
+
+void SimEncoder::Stop() {
+ sendCommand("stop");
+}
+
+double SimEncoder::GetPosition() {
+ return position;
+}
+
+double SimEncoder::GetVelocity() {
+ return velocity;
+}
+
+
+void SimEncoder::sendCommand(std::string cmd) {
+ msgs::GzString msg;
+ msg.set_data(cmd);
+ commandPub->Publish(msg);
+}
+
+
+void SimEncoder::positionCallback(const msgs::ConstFloat64Ptr &msg) {
+ position = msg->data();
+}
+
+void SimEncoder::velocityCallback(const msgs::ConstFloat64Ptr &msg) {
+ velocity = msg->data();
+}
diff --git a/wpilibc/simulation/src/simulation/SimFloatInput.cpp b/wpilibc/simulation/src/simulation/SimFloatInput.cpp
new file mode 100644
index 0000000..1fa9337
--- /dev/null
+++ b/wpilibc/simulation/src/simulation/SimFloatInput.cpp
@@ -0,0 +1,22 @@
+/*
+ * SimFloatInput.cpp
+ *
+ * Created on: May 28, 2014
+ * Author: alex
+ */
+
+#include "simulation/SimFloatInput.h"
+#include "simulation/MainNode.h"
+
+SimFloatInput::SimFloatInput(std::string topic) {
+ sub = MainNode::Subscribe("~/simulator/"+topic, &SimFloatInput::callback, this);
+ std::cout << "Initialized ~/simulator/"+topic << std::endl;
+}
+
+double SimFloatInput::Get() {
+ return value;
+}
+
+void SimFloatInput::callback(const msgs::ConstFloat64Ptr &msg) {
+ value = msg->data();
+}
diff --git a/wpilibc/simulation/src/simulation/SimGyro.cpp b/wpilibc/simulation/src/simulation/SimGyro.cpp
new file mode 100644
index 0000000..c173c05
--- /dev/null
+++ b/wpilibc/simulation/src/simulation/SimGyro.cpp
@@ -0,0 +1,46 @@
+
+#include "simulation/SimGyro.h"
+#include "simulation/MainNode.h"
+
+SimGyro::SimGyro(std::string topic) {
+ commandPub = MainNode::Advertise<msgs::GzString>("~/simulator/"+topic+"/control");
+
+ posSub = MainNode::Subscribe("~/simulator/"+topic+"/position",
+ &SimGyro::positionCallback, this);
+ velSub = MainNode::Subscribe("~/simulator/"+topic+"/velocity",
+ &SimGyro::velocityCallback, this);
+
+ if (commandPub->WaitForConnection(gazebo::common::Time(5.0))) { // Wait up to five seconds.
+ std::cout << "Initialized ~/simulator/" + topic << std::endl;
+ } else {
+ std::cerr << "Failed to initialize ~/simulator/" + topic + ": does the gyro exist?" << std::endl;
+ }
+}
+
+void SimGyro::Reset() {
+ sendCommand("reset");
+}
+
+double SimGyro::GetAngle() {
+ return position;
+}
+
+double SimGyro::GetVelocity() {
+ return velocity;
+}
+
+
+void SimGyro::sendCommand(std::string cmd) {
+ msgs::GzString msg;
+ msg.set_data(cmd);
+ commandPub->Publish(msg);
+}
+
+
+void SimGyro::positionCallback(const msgs::ConstFloat64Ptr &msg) {
+ position = msg->data();
+}
+
+void SimGyro::velocityCallback(const msgs::ConstFloat64Ptr &msg) {
+ velocity = msg->data();
+}