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/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);
+}}