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/shared/include/Base.h b/wpilibc/shared/include/Base.h
new file mode 100644
index 0000000..b1b806f
--- /dev/null
+++ b/wpilibc/shared/include/Base.h
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* 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
+
+// MSVC 2013 doesn't allow "= default" on move constructors, but since we are
+// (currently) only actually using the move constructors in non-MSVC situations
+// (ie, wpilibC++Devices), we can just ignore it in MSVC.
+#if !defined(_MSC_VER)
+#define DEFAULT_MOVE_CONSTRUCTOR(ClassName) \
+ClassName(ClassName &&) = default
+#else
+#define DEFAULT_MOVE_CONSTRUCTOR(ClassName)
+#endif
+
+#if (__cplusplus < 201103L)
+ #if !defined(_MSC_VER)
+ #define nullptr NULL
+ #endif
+ #define constexpr const
+#endif
+
+#if defined(_MSC_VER)
+ #define noexcept throw()
+#endif
+
+// [[deprecated(msg)]] is a C++14 feature not supported by MSVC or GCC < 4.9.
+// We provide an equivalent warning implementation for those compilers here.
+#if defined(_MSC_VER)
+ #define DEPRECATED(msg) __declspec(deprecated(msg))
+#elif defined(__GNUC__)
+ #if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 8)
+ #define DEPRECATED(msg) [[deprecated(msg)]]
+ #else
+ #define DEPRECATED(msg) __attribute__((deprecated(msg)))
+ #endif
+#elif __cplusplus > 201103L
+ #define DEPRECATED(msg) [[deprecated(msg)]]
+#else
+ #define DEPRECATED(msg) /*nothing*/
+#endif
+
+// Provide std::decay_t when using GCC < 4.9
+#if defined(__GNUC__)
+ #if __GNUC__ == 4 && __GNUC_MINOR__ < 9
+ #include <type_traits>
+ namespace std {
+ template <class T> using decay_t = typename decay<T>::type;
+ }
+ #endif
+#endif
+
+// A struct to use as a deleter when a std::shared_ptr must wrap a raw pointer
+// that is being deleted by someone else.
+template<class T>
+struct
+NullDeleter {
+ void operator()(T *) const noexcept {};
+};
+
+#include <atomic>
+// Use this for determining whether the default move constructor has been
+// called on a containing object. This serves the purpose of allowing us to
+// use the default move constructor of an object for moving all the data around
+// while being able to use this to, for instance, chose not to de-allocate
+// a PWM port in a destructor.
+struct HasBeenMoved {
+ HasBeenMoved(HasBeenMoved&& other) {
+ other.moved = true;
+ moved = false;
+ }
+ HasBeenMoved() = default;
+ std::atomic<bool> moved{false};
+ operator bool() const { return moved; }
+};
+
+// Define make_unique for C++11-only compilers
+#if __cplusplus == 201103L
+#include <cstddef>
+#include <memory>
+#include <type_traits>
+#include <utility>
+namespace std {
+template <class T>
+struct _Unique_if {
+ typedef unique_ptr<T> _Single_object;
+};
+
+template <class T>
+struct _Unique_if<T[]> {
+ typedef unique_ptr<T[]> _Unknown_bound;
+};
+
+template <class T, size_t N>
+struct _Unique_if<T[N]> {
+ typedef void _Known_bound;
+};
+
+template <class T, class... Args>
+typename _Unique_if<T>::_Single_object make_unique(Args &&... args) {
+ return unique_ptr<T>(new T(std::forward<Args>(args)...));
+}
+
+template <class T>
+typename _Unique_if<T>::_Unknown_bound make_unique(size_t n) {
+ typedef typename remove_extent<T>::type U;
+ return unique_ptr<T>(new U[n]());
+}
+
+template <class T, class... Args>
+typename _Unique_if<T>::_Known_bound make_unique(Args &&...) = delete;
+} // namespace std
+#endif
diff --git a/wpilibc/shared/include/Buttons/Button.h b/wpilibc/shared/include/Buttons/Button.h
new file mode 100644
index 0000000..8897569
--- /dev/null
+++ b/wpilibc/shared/include/Buttons/Button.h
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __BUTTON_H__
+#define __BUTTON_H__
+
+#include "Buttons/Trigger.h"
+#include "Commands/Command.h"
+
+/**
+ * This class provides an easy way to link commands to OI inputs.
+ *
+ * It is very easy to link a button to a command. For instance, you could
+ * link the trigger button of a joystick to a "score" command.
+ *
+ * This class represents a subclass of Trigger that is specifically aimed at
+ * buttons on an operator interface as a common use case of the more generalized
+ * Trigger objects. This is a simple wrapper around Trigger with the method
+ * names
+ * renamed to fit the Button object use.
+ *
+ * @author brad
+ */
+class Button : public Trigger {
+ public:
+ virtual void WhenPressed(Command *command);
+ virtual void WhileHeld(Command *command);
+ virtual void WhenReleased(Command *command);
+ virtual void CancelWhenPressed(Command *command);
+ virtual void ToggleWhenPressed(Command *command);
+};
+
+#endif
diff --git a/wpilibc/shared/include/Buttons/ButtonScheduler.h b/wpilibc/shared/include/Buttons/ButtonScheduler.h
new file mode 100644
index 0000000..6222da7
--- /dev/null
+++ b/wpilibc/shared/include/Buttons/ButtonScheduler.h
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __BUTTON_SCHEDULER_H__
+#define __BUTTON_SCHEDULER_H__
+
+class Trigger;
+class Command;
+
+class ButtonScheduler {
+ public:
+ ButtonScheduler(bool last, Trigger *button, Command *orders);
+ virtual ~ButtonScheduler() = default;
+ virtual void Execute() = 0;
+ void Start();
+
+ protected:
+ bool m_pressedLast;
+ Trigger *m_button;
+ Command *m_command;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Buttons/CancelButtonScheduler.h b/wpilibc/shared/include/Buttons/CancelButtonScheduler.h
new file mode 100644
index 0000000..ba3ba24
--- /dev/null
+++ b/wpilibc/shared/include/Buttons/CancelButtonScheduler.h
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __CANCEL_BUTTON_SCHEDULER_H__
+#define __CANCEL_BUTTON_SCHEDULER_H__
+
+#include "Buttons/ButtonScheduler.h"
+
+class Trigger;
+class Command;
+
+class CancelButtonScheduler : public ButtonScheduler {
+ public:
+ CancelButtonScheduler(bool last, Trigger *button, Command *orders);
+ virtual ~CancelButtonScheduler() = default;
+ virtual void Execute();
+
+ private:
+ bool pressedLast;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Buttons/HeldButtonScheduler.h b/wpilibc/shared/include/Buttons/HeldButtonScheduler.h
new file mode 100644
index 0000000..6dcd7a3
--- /dev/null
+++ b/wpilibc/shared/include/Buttons/HeldButtonScheduler.h
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __HELD_BUTTON_SCHEDULER_H__
+#define __HELD_BUTTON_SCHEDULER_H__
+
+#include "Buttons/ButtonScheduler.h"
+
+class Trigger;
+class Command;
+
+class HeldButtonScheduler : public ButtonScheduler {
+ public:
+ HeldButtonScheduler(bool last, Trigger *button, Command *orders);
+ virtual ~HeldButtonScheduler() = default;
+ virtual void Execute();
+};
+
+#endif
diff --git a/wpilibc/shared/include/Buttons/InternalButton.h b/wpilibc/shared/include/Buttons/InternalButton.h
new file mode 100644
index 0000000..fdad6f9
--- /dev/null
+++ b/wpilibc/shared/include/Buttons/InternalButton.h
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __INTERNAL_BUTTON_H__
+#define __INTERNAL_BUTTON_H__
+
+#include "Buttons/Button.h"
+
+class InternalButton : public Button {
+ public:
+ InternalButton() = default;
+ InternalButton(bool inverted);
+ virtual ~InternalButton() = default;
+
+ void SetInverted(bool inverted);
+ void SetPressed(bool pressed);
+
+ virtual bool Get();
+
+ private:
+ bool m_pressed = false;
+ bool m_inverted = false;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Buttons/JoystickButton.h b/wpilibc/shared/include/Buttons/JoystickButton.h
new file mode 100644
index 0000000..028efea
--- /dev/null
+++ b/wpilibc/shared/include/Buttons/JoystickButton.h
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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_BUTTON_H__
+#define __JOYSTICK_BUTTON_H__
+
+#include "GenericHID.h"
+#include "Buttons/Button.h"
+
+class JoystickButton : public Button {
+ public:
+ JoystickButton(GenericHID *joystick, int buttonNumber);
+ virtual ~JoystickButton() = default;
+
+ virtual bool Get();
+
+ private:
+ GenericHID *m_joystick;
+ int m_buttonNumber;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Buttons/NetworkButton.h b/wpilibc/shared/include/Buttons/NetworkButton.h
new file mode 100644
index 0000000..9dcba58
--- /dev/null
+++ b/wpilibc/shared/include/Buttons/NetworkButton.h
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __NETWORK_BUTTON_H__
+#define __NETWORK_BUTTON_H__
+
+#include "Buttons/Button.h"
+#include <string>
+#include <memory>
+
+class NetworkButton : public Button {
+ public:
+ NetworkButton(const std::string &tableName, const std::string &field);
+ NetworkButton(std::shared_ptr<ITable> table, const std::string &field);
+ virtual ~NetworkButton() = default;
+
+ virtual bool Get();
+
+ private:
+ std::shared_ptr<ITable> m_netTable;
+ std::string m_field;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Buttons/PressedButtonScheduler.h b/wpilibc/shared/include/Buttons/PressedButtonScheduler.h
new file mode 100644
index 0000000..62ff7a8
--- /dev/null
+++ b/wpilibc/shared/include/Buttons/PressedButtonScheduler.h
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __PRESSED_BUTTON_SCHEDULER_H__
+#define __PRESSED_BUTTON_SCHEDULER_H__
+
+#include "Buttons/ButtonScheduler.h"
+
+class Trigger;
+class Command;
+
+class PressedButtonScheduler : public ButtonScheduler {
+ public:
+ PressedButtonScheduler(bool last, Trigger *button, Command *orders);
+ virtual ~PressedButtonScheduler() = default;
+ virtual void Execute();
+};
+
+#endif
diff --git a/wpilibc/shared/include/Buttons/ReleasedButtonScheduler.h b/wpilibc/shared/include/Buttons/ReleasedButtonScheduler.h
new file mode 100644
index 0000000..2a29981
--- /dev/null
+++ b/wpilibc/shared/include/Buttons/ReleasedButtonScheduler.h
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __RELEASED_BUTTON_SCHEDULER_H__
+#define __RELEASED_BUTTON_SCHEDULER_H__
+
+#include "Buttons/ButtonScheduler.h"
+
+class Trigger;
+class Command;
+
+class ReleasedButtonScheduler : public ButtonScheduler {
+ public:
+ ReleasedButtonScheduler(bool last, Trigger *button, Command *orders);
+ virtual ~ReleasedButtonScheduler() = default;
+ virtual void Execute();
+};
+
+#endif
diff --git a/wpilibc/shared/include/Buttons/ToggleButtonScheduler.h b/wpilibc/shared/include/Buttons/ToggleButtonScheduler.h
new file mode 100644
index 0000000..d79b456
--- /dev/null
+++ b/wpilibc/shared/include/Buttons/ToggleButtonScheduler.h
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __TOGGLE_BUTTON_SCHEDULER_H__
+#define __TOGGLE_BUTTON_SCHEDULER_H__
+
+#include "Buttons/ButtonScheduler.h"
+
+class Trigger;
+class Command;
+
+class ToggleButtonScheduler : public ButtonScheduler {
+ public:
+ ToggleButtonScheduler(bool last, Trigger *button, Command *orders);
+ virtual ~ToggleButtonScheduler() = default;
+ virtual void Execute();
+
+ private:
+ bool pressedLast;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Buttons/Trigger.h b/wpilibc/shared/include/Buttons/Trigger.h
new file mode 100644
index 0000000..3a05995
--- /dev/null
+++ b/wpilibc/shared/include/Buttons/Trigger.h
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __TRIGGER_H__
+#define __TRIGGER_H__
+
+#include "SmartDashboard/Sendable.h"
+#include <memory>
+
+class Command;
+
+/**
+ * This class provides an easy way to link commands to inputs.
+ *
+ * It is very easy to link a polled input to a command. For instance, you could
+ * link the trigger button of a joystick to a "score" command or an encoder
+ * reaching
+ * a particular value.
+ *
+ * It is encouraged that teams write a subclass of Trigger if they want to have
+ * something unusual (for instance, if they want to react to the user holding
+ * a button while the robot is reading a certain sensor input). For this, they
+ * only have to write the {@link Trigger#Get()} method to get the full
+ * functionality
+ * of the Trigger class.
+ *
+ * @author Brad Miller, Joe Grinstead
+ */
+class Trigger : public Sendable {
+ public:
+ Trigger() = default;
+ virtual ~Trigger() = default;
+ bool Grab();
+ virtual bool Get() = 0;
+ void WhenActive(Command *command);
+ void WhileActive(Command *command);
+ void WhenInactive(Command *command);
+ void CancelWhenActive(Command *command);
+ void ToggleWhenActive(Command *command);
+
+ virtual void InitTable(std::shared_ptr<ITable> table);
+ virtual std::shared_ptr<ITable> GetTable() const;
+ virtual std::string GetSmartDashboardType() const;
+
+ protected:
+ std::shared_ptr<ITable> m_table;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Commands/Command.h b/wpilibc/shared/include/Commands/Command.h
new file mode 100644
index 0000000..6ec5512
--- /dev/null
+++ b/wpilibc/shared/include/Commands/Command.h
@@ -0,0 +1,190 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __COMMAND_H__
+#define __COMMAND_H__
+
+#include "ErrorBase.h"
+#include "SmartDashboard/NamedSendable.h"
+#include "tables/ITableListener.h"
+#include <set>
+#include <string>
+#include <memory>
+
+class CommandGroup;
+class Subsystem;
+
+/**
+ * The Command class is at the very core of the entire command framework.
+ * Every command can be started with a call to {@link Command#Start() Start()}.
+ * Once a command is started it will call {@link Command#Initialize()
+ * Initialize()}, and then
+ * will repeatedly call {@link Command#Execute() Execute()} until the {@link
+ *Command#IsFinished() IsFinished()}
+ * returns true. Once it does, {@link Command#End() End()} will be called.
+ *
+ * <p>However, if at any point while it is running {@link Command#Cancel()
+ * Cancel()} is called, then
+ * the command will be stopped and {@link Command#Interrupted() Interrupted()}
+ * will be called.</p>
+ *
+ * <p>If a command uses a {@link Subsystem}, then it should specify that it does
+ * so by
+ * calling the {@link Command#Requires(Subsystem) Requires(...)} method
+ * in its constructor. Note that a Command may have multiple requirements, and
+ * {@link Command#Requires(Subsystem) Requires(...)} should be
+ * called for each one.</p>
+ *
+ * <p>If a command is running and a new command with shared requirements is
+ * started,
+ * then one of two things will happen. If the active command is interruptible,
+ * then {@link Command#Cancel() Cancel()} will be called and the command will be
+ * removed
+ * to make way for the new one. If the active command is not interruptible, the
+ * other one will not even be started, and the active one will continue
+ * functioning.</p>
+ *
+ * @see CommandGroup
+ * @see Subsystem
+ */
+class Command : public ErrorBase, public NamedSendable, public ITableListener {
+ friend class CommandGroup;
+ friend class Scheduler;
+
+ public:
+ Command();
+ Command(const std::string &name);
+ Command(double timeout);
+ Command(const std::string &name, double timeout);
+ virtual ~Command();
+ double TimeSinceInitialized() const;
+ void Requires(Subsystem *s);
+ bool IsCanceled() const;
+ void Start();
+ bool Run();
+ void Cancel();
+ bool IsRunning() const;
+ bool IsInterruptible() const;
+ void SetInterruptible(bool interruptible);
+ bool DoesRequire(Subsystem *subsystem) const;
+ typedef std::set<Subsystem *> SubsystemSet;
+ SubsystemSet GetRequirements() const;
+ CommandGroup *GetGroup() const;
+ void SetRunWhenDisabled(bool run);
+ bool WillRunWhenDisabled() const;
+ int GetID() const;
+
+ protected:
+ void SetTimeout(double timeout);
+ bool IsTimedOut() const;
+ bool AssertUnlocked(const std::string &message);
+ void SetParent(CommandGroup *parent);
+ /**
+ * The initialize method is called the first time this Command is run after
+ * being started.
+ */
+ virtual void Initialize() = 0;
+ /**
+ * The execute method is called repeatedly until this Command either finishes
+ * or is canceled.
+ */
+ virtual void Execute() = 0;
+ /**
+ * Returns whether this command is finished.
+ * If it is, then the command will be removed
+ * and {@link Command#end() end()} will be called.
+ *
+ * <p>It may be useful for a team to reference the {@link Command#isTimedOut()
+ * isTimedOut()} method
+ * for time-sensitive commands.</p>
+ * @return whether this command is finished.
+ * @see Command#isTimedOut() isTimedOut()
+ */
+ virtual bool IsFinished() = 0;
+ /**
+ * Called when the command ended peacefully. This is where you may want
+ * to wrap up loose ends, like shutting off a motor that was being used
+ * in the command.
+ */
+ virtual void End() = 0;
+ /**
+ * Called when the command ends because somebody called {@link
+ *Command#cancel() cancel()}
+ * or another command shared the same requirements as this one, and booted
+ * it out.
+ *
+ * <p>This is where you may want
+ * to wrap up loose ends, like shutting off a motor that was being used
+ * in the command.</p>
+ *
+ * <p>Generally, it is useful to simply call the {@link Command#end() end()}
+ * method
+ * within this method</p>
+ */
+ virtual void Interrupted() = 0;
+ virtual void _Initialize();
+ virtual void _Interrupted();
+ virtual void _Execute();
+ virtual void _End();
+ virtual void _Cancel();
+
+ private:
+ void LockChanges();
+ /*synchronized*/ void Removed();
+ void StartRunning();
+ void StartTiming();
+
+ /** The name of this command */
+ std::string m_name;
+
+ /** The time since this command was initialized */
+ double m_startTime = -1;
+
+ /** The time (in seconds) before this command "times out" (or -1 if no
+ * timeout) */
+ double m_timeout;
+
+ /** Whether or not this command has been initialized */
+ bool m_initialized = false;
+
+ /** The requirements (or null if no requirements) */
+ SubsystemSet m_requirements;
+
+ /** Whether or not it is running */
+ bool m_running = false;
+
+ /** Whether or not it is interruptible*/
+ bool m_interruptible = true;
+
+ /** Whether or not it has been canceled */
+ bool m_canceled = false;
+
+ /** Whether or not it has been locked */
+ bool m_locked = false;
+
+ /** Whether this command should run when the robot is disabled */
+ bool m_runWhenDisabled = false;
+
+ /** The {@link CommandGroup} this is in */
+ CommandGroup *m_parent = nullptr;
+
+ int m_commandID = m_commandCounter++;
+ static int m_commandCounter;
+
+ public:
+ virtual std::string GetName() const;
+ virtual void InitTable(std::shared_ptr<ITable> table);
+ virtual std::shared_ptr<ITable> GetTable() const;
+ virtual std::string GetSmartDashboardType() const;
+ virtual void ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew);
+
+ protected:
+ std::shared_ptr<ITable> m_table;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Commands/CommandGroup.h b/wpilibc/shared/include/Commands/CommandGroup.h
new file mode 100644
index 0000000..7f289ca
--- /dev/null
+++ b/wpilibc/shared/include/Commands/CommandGroup.h
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __COMMAND_GROUP_H__
+#define __COMMAND_GROUP_H__
+
+#include "Commands/Command.h"
+#include "Commands/CommandGroupEntry.h"
+#include <list>
+#include <vector>
+
+/**
+ * A {@link CommandGroup} is a list of commands which are executed in sequence.
+ *
+ * <p>Commands in a {@link CommandGroup} are added using the {@link
+ * CommandGroup#AddSequential(Command) AddSequential(...)} method
+ * and are called sequentially.
+ * {@link CommandGroup CommandGroups} are themselves {@link Command Commands}
+ * and can be given to other {@link CommandGroup CommandGroups}.</p>
+ *
+ * <p>{@link CommandGroup CommandGroups} will carry all of the requirements of
+ * their {@link Command subcommands}. Additional
+ * requirements can be specified by calling {@link
+ *CommandGroup#Requires(Subsystem) Requires(...)}
+ * normally in the constructor.</P>
+ *
+ * <p>CommandGroups can also execute commands in parallel, simply by adding them
+ * using {@link CommandGroup#AddParallel(Command) AddParallel(...)}.</p>
+ *
+ * @see Command
+ * @see Subsystem
+ */
+class CommandGroup : public Command {
+ public:
+ CommandGroup() = default;
+ CommandGroup(const std::string &name);
+ virtual ~CommandGroup() = default;
+
+ void AddSequential(Command *command);
+ void AddSequential(Command *command, double timeout);
+ void AddParallel(Command *command);
+ void AddParallel(Command *command, double timeout);
+ bool IsInterruptible() const;
+ int GetSize() const;
+
+ protected:
+ virtual void Initialize();
+ virtual void Execute();
+ virtual bool IsFinished();
+ virtual void End();
+ virtual void Interrupted();
+ virtual void _Initialize();
+ virtual void _Interrupted();
+ virtual void _Execute();
+ virtual void _End();
+
+ private:
+ void CancelConflicts(Command *command);
+
+ /** The commands in this group (stored in entries) */
+ std::vector<CommandGroupEntry> m_commands;
+
+ /** The active children in this group (stored in entries) */
+ std::list<CommandGroupEntry> m_children;
+
+ /** The current command, -1 signifies that none have been run */
+ int m_currentCommandIndex = -1;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Commands/CommandGroupEntry.h b/wpilibc/shared/include/Commands/CommandGroupEntry.h
new file mode 100644
index 0000000..442a02c
--- /dev/null
+++ b/wpilibc/shared/include/Commands/CommandGroupEntry.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __COMMAND_GROUP_ENTRY_H__
+#define __COMMAND_GROUP_ENTRY_H__
+
+class Command;
+
+class CommandGroupEntry {
+ public:
+ typedef enum {
+ kSequence_InSequence,
+ kSequence_BranchPeer,
+ kSequence_BranchChild
+ } Sequence;
+
+ CommandGroupEntry() = default;
+ CommandGroupEntry(Command *command, Sequence state, double timeout = -1.0);
+ bool IsTimedOut() const;
+
+ double m_timeout = -1.0;
+ Command *m_command = nullptr;
+ Sequence m_state = kSequence_InSequence;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Commands/PIDCommand.h b/wpilibc/shared/include/Commands/PIDCommand.h
new file mode 100644
index 0000000..d67c063
--- /dev/null
+++ b/wpilibc/shared/include/Commands/PIDCommand.h
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __PID_COMMAND_H__
+#define __PID_COMMAND_H__
+
+#include "Commands/Command.h"
+#include "PIDController.h"
+#include "PIDSource.h"
+#include "PIDOutput.h"
+
+#include <memory>
+
+class PIDCommand : public Command, public PIDOutput, public PIDSource {
+ public:
+ PIDCommand(const std::string &name, double p, double i, double d);
+ PIDCommand(const std::string &name, double p, double i, double d, double period);
+ PIDCommand(const std::string &name, double p, double i, double d, double f,
+ double period);
+ PIDCommand(double p, double i, double d);
+ PIDCommand(double p, double i, double d, double period);
+ PIDCommand(double p, double i, double d, double f, double period);
+ virtual ~PIDCommand() = default;
+
+ void SetSetpointRelative(double deltaSetpoint);
+
+ // PIDOutput interface
+ virtual void PIDWrite(float output);
+
+ // PIDSource interface
+ virtual double PIDGet();
+
+ protected:
+ std::shared_ptr<PIDController> GetPIDController() const;
+ virtual void _Initialize();
+ virtual void _Interrupted();
+ virtual void _End();
+ void SetSetpoint(double setpoint);
+ double GetSetpoint() const;
+ double GetPosition();
+
+ virtual double ReturnPIDInput() = 0;
+ virtual void UsePIDOutput(double output) = 0;
+
+ private:
+ /** The internal {@link PIDController} */
+ std::shared_ptr<PIDController> m_controller;
+
+ public:
+ virtual void InitTable(std::shared_ptr<ITable> table);
+ virtual std::string GetSmartDashboardType() const;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Commands/PIDSubsystem.h b/wpilibc/shared/include/Commands/PIDSubsystem.h
new file mode 100644
index 0000000..ff05514
--- /dev/null
+++ b/wpilibc/shared/include/Commands/PIDSubsystem.h
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __PID_SUBSYSTEM_H__
+#define __PID_SUBSYSTEM_H__
+
+#include "Commands/Subsystem.h"
+#include "PIDController.h"
+#include "PIDSource.h"
+#include "PIDOutput.h"
+
+#include <memory>
+
+/**
+ * This class is designed to handle the case where there is a {@link Subsystem}
+ * which uses a single {@link PIDController} almost constantly (for instance,
+ * an elevator which attempts to stay at a constant height).
+ *
+ * <p>It provides some convenience methods to run an internal {@link
+ * PIDController}.
+ * It also allows access to the internal {@link PIDController} in order to give
+ * total control
+ * to the programmer.</p>
+ *
+ */
+class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource {
+ public:
+ PIDSubsystem(const std::string &name, double p, double i, double d);
+ PIDSubsystem(const std::string &name, double p, double i, double d, double f);
+ PIDSubsystem(const std::string &name, double p, double i, double d, double f,
+ double period);
+ PIDSubsystem(double p, double i, double d);
+ PIDSubsystem(double p, double i, double d, double f);
+ PIDSubsystem(double p, double i, double d, double f, double period);
+ virtual ~PIDSubsystem() = default;
+
+ void Enable();
+ void Disable();
+
+ // PIDOutput interface
+ virtual void PIDWrite(float output);
+
+ // PIDSource interface
+ virtual double PIDGet();
+ void SetSetpoint(double setpoint);
+ void SetSetpointRelative(double deltaSetpoint);
+ void SetInputRange(float minimumInput, float maximumInput);
+ void SetOutputRange(float minimumOutput, float maximumOutput);
+ double GetSetpoint();
+ double GetPosition();
+ double GetRate();
+
+ virtual void SetAbsoluteTolerance(float absValue);
+ virtual void SetPercentTolerance(float percent);
+ virtual bool OnTarget() const;
+
+ protected:
+ std::shared_ptr<PIDController> GetPIDController();
+
+ virtual double ReturnPIDInput() = 0;
+ virtual void UsePIDOutput(double output) = 0;
+
+ private:
+ /** The internal {@link PIDController} */
+ std::shared_ptr<PIDController> m_controller;
+
+ public:
+ virtual void InitTable(std::shared_ptr<ITable> table);
+ virtual std::string GetSmartDashboardType() const;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Commands/PrintCommand.h b/wpilibc/shared/include/Commands/PrintCommand.h
new file mode 100644
index 0000000..cbb3e5e
--- /dev/null
+++ b/wpilibc/shared/include/Commands/PrintCommand.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __PRINT_COMMAND_H__
+#define __PRINT_COMMAND_H__
+
+#include "Commands/Command.h"
+#include <string>
+
+class PrintCommand : public Command {
+ public:
+ PrintCommand(const std::string &message);
+ virtual ~PrintCommand() = default;
+
+ protected:
+ virtual void Initialize();
+ virtual void Execute();
+ virtual bool IsFinished();
+ virtual void End();
+ virtual void Interrupted();
+
+ private:
+ std::string m_message;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Commands/Scheduler.h b/wpilibc/shared/include/Commands/Scheduler.h
new file mode 100644
index 0000000..065fbad
--- /dev/null
+++ b/wpilibc/shared/include/Commands/Scheduler.h
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __SCHEDULER_H__
+#define __SCHEDULER_H__
+
+#include "Commands/Command.h"
+#include "ErrorBase.h"
+#include "SmartDashboard/NamedSendable.h"
+#include "networktables/NetworkTable.h"
+#include "SmartDashboard/SmartDashboard.h"
+#include <list>
+#include <map>
+#include <memory>
+#include <set>
+#include <string>
+#include <vector>
+#include "HAL/cpp/priority_mutex.h"
+
+class ButtonScheduler;
+class Subsystem;
+
+class Scheduler : public ErrorBase, public NamedSendable {
+ public:
+ static Scheduler *GetInstance();
+
+ void AddCommand(Command *command);
+ void AddButton(ButtonScheduler *button);
+ void RegisterSubsystem(Subsystem *subsystem);
+ void Run();
+ void Remove(Command *command);
+ void RemoveAll();
+ void ResetAll();
+ void SetEnabled(bool enabled);
+
+ void UpdateTable();
+ std::string GetSmartDashboardType() const;
+ void InitTable(std::shared_ptr<ITable> subTable);
+ std::shared_ptr<ITable> GetTable() const;
+ std::string GetName() const;
+ std::string GetType() const;
+
+ private:
+ Scheduler();
+ virtual ~Scheduler() = default;
+
+ void ProcessCommandAddition(Command *command);
+
+ Command::SubsystemSet m_subsystems;
+ priority_mutex m_buttonsLock;
+ typedef std::vector<ButtonScheduler *> ButtonVector;
+ ButtonVector m_buttons;
+ typedef std::vector<Command *> CommandVector;
+ priority_mutex m_additionsLock;
+ CommandVector m_additions;
+ typedef std::set<Command *> CommandSet;
+ CommandSet m_commands;
+ bool m_adding = false;
+ bool m_enabled = true;
+ std::vector<std::string> commands;
+ std::vector<double> ids;
+ std::vector<double> toCancel;
+ std::shared_ptr<ITable> m_table;
+ bool m_runningCommandsChanged = false;
+};
+#endif
diff --git a/wpilibc/shared/include/Commands/StartCommand.h b/wpilibc/shared/include/Commands/StartCommand.h
new file mode 100644
index 0000000..8f53d14
--- /dev/null
+++ b/wpilibc/shared/include/Commands/StartCommand.h
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __START_COMMAND_H__
+#define __START_COMMAND_H__
+
+#include "Commands/Command.h"
+
+class StartCommand : public Command {
+ public:
+ StartCommand(Command *commandToStart);
+ virtual ~StartCommand() = default;
+
+ protected:
+ virtual void Initialize();
+ virtual void Execute();
+ virtual bool IsFinished();
+ virtual void End();
+ virtual void Interrupted();
+
+ private:
+ Command *m_commandToFork;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Commands/Subsystem.h b/wpilibc/shared/include/Commands/Subsystem.h
new file mode 100644
index 0000000..8762ec8
--- /dev/null
+++ b/wpilibc/shared/include/Commands/Subsystem.h
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __SUBSYSTEM_H__
+#define __SUBSYSTEM_H__
+
+#include "ErrorBase.h"
+#include "SmartDashboard/NamedSendable.h"
+#include <string>
+#include <memory>
+
+class Command;
+
+class Subsystem : public ErrorBase, public NamedSendable {
+ friend class Scheduler;
+
+ public:
+ Subsystem(const std::string &name);
+ virtual ~Subsystem() = default;
+
+ void SetDefaultCommand(Command *command);
+ Command *GetDefaultCommand();
+ void SetCurrentCommand(Command *command);
+ Command *GetCurrentCommand() const;
+ virtual void InitDefaultCommand();
+
+ private:
+ void ConfirmCommand();
+
+ Command *m_currentCommand = nullptr;
+ bool m_currentCommandChanged = true;
+ Command *m_defaultCommand = nullptr;
+ std::string m_name;
+ bool m_initializedDefaultCommand = false;
+
+ public:
+ virtual std::string GetName() const;
+ virtual void InitTable(std::shared_ptr<ITable> table);
+ virtual std::shared_ptr<ITable> GetTable() const;
+ virtual std::string GetSmartDashboardType() const;
+
+ protected:
+ std::shared_ptr<ITable> m_table;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Commands/WaitCommand.h b/wpilibc/shared/include/Commands/WaitCommand.h
new file mode 100644
index 0000000..6db6dac
--- /dev/null
+++ b/wpilibc/shared/include/Commands/WaitCommand.h
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __WAIT_COMMAND_H__
+#define __WAIT_COMMAND_H__
+
+#include "Commands/Command.h"
+
+class WaitCommand : public Command {
+ public:
+ WaitCommand(double timeout);
+ WaitCommand(const std::string &name, double timeout);
+ virtual ~WaitCommand() = default;
+
+ protected:
+ virtual void Initialize();
+ virtual void Execute();
+ virtual bool IsFinished();
+ virtual void End();
+ virtual void Interrupted();
+};
+
+#endif
diff --git a/wpilibc/shared/include/Commands/WaitForChildren.h b/wpilibc/shared/include/Commands/WaitForChildren.h
new file mode 100644
index 0000000..858d243
--- /dev/null
+++ b/wpilibc/shared/include/Commands/WaitForChildren.h
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __WAIT_FOR_CHILDREN_H__
+#define __WAIT_FOR_CHILDREN_H__
+
+#include "Commands/Command.h"
+
+class WaitForChildren : public Command {
+ public:
+ WaitForChildren(double timeout);
+ WaitForChildren(const std::string &name, double timeout);
+ virtual ~WaitForChildren() = default;
+
+ protected:
+ virtual void Initialize();
+ virtual void Execute();
+ virtual bool IsFinished();
+ virtual void End();
+ virtual void Interrupted();
+};
+
+#endif
diff --git a/wpilibc/shared/include/Commands/WaitUntilCommand.h b/wpilibc/shared/include/Commands/WaitUntilCommand.h
new file mode 100644
index 0000000..fd77f8e
--- /dev/null
+++ b/wpilibc/shared/include/Commands/WaitUntilCommand.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __WAIT_UNTIL_COMMAND_H__
+#define __WAIT_UNTIL_COMMAND_H__
+
+#include "Commands/Command.h"
+
+class WaitUntilCommand : public Command {
+ public:
+ WaitUntilCommand(double time);
+ WaitUntilCommand(const std::string &name, double time);
+ virtual ~WaitUntilCommand() = default;
+
+ protected:
+ virtual void Initialize();
+ virtual void Execute();
+ virtual bool IsFinished();
+ virtual void End();
+ virtual void Interrupted();
+
+ private:
+ double m_time;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Controller.h b/wpilibc/shared/include/Controller.h
new file mode 100644
index 0000000..d852307
--- /dev/null
+++ b/wpilibc/shared/include/Controller.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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 Controllers
+ * Common interface for controllers. Controllers run control loops, the most
+ * common
+ * are PID controllers and their variants, but this includes anything that is
+ * controlling
+ * an actuator in a separate thread.
+ */
+class Controller {
+ public:
+ virtual ~Controller() = default;
+
+ /**
+ * Allows the control loop to run
+ */
+ virtual void Enable() = 0;
+
+ /**
+ * Stops the control loop from running until explicitly re-enabled by calling
+ * enable()
+ */
+ virtual void Disable() = 0;
+};
diff --git a/wpilibc/shared/include/Error.h b/wpilibc/shared/include/Error.h
new file mode 100644
index 0000000..f9e301d
--- /dev/null
+++ b/wpilibc/shared/include/Error.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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"
+
+#ifdef _WIN32
+ #include <Windows.h>
+ //Windows.h defines #define GetMessage GetMessageW, which is stupid and we don't want it.
+ #undef GetMessage
+#endif
+
+#include <string>
+#include <stdint.h>
+#include "llvm/StringRef.h"
+
+// Forward declarations
+class ErrorBase;
+
+/**
+ * Error object represents a library error.
+ */
+class Error {
+ public:
+ typedef int32_t Code;
+
+ Error() = default;
+
+ Error(const Error&) = delete;
+ Error& operator=(const Error&) = delete;
+
+ void Clone(const Error& error);
+ Code GetCode() const;
+ std::string GetMessage() const;
+ std::string GetFilename() const;
+ std::string GetFunction() const;
+ uint32_t GetLineNumber() const;
+ const ErrorBase* GetOriginatingObject() const;
+ double GetTimestamp() const;
+ void Clear();
+ void Set(Code code, llvm::StringRef contextMessage,
+ llvm::StringRef filename, llvm::StringRef function,
+ uint32_t lineNumber, const ErrorBase* originatingObject);
+
+ private:
+ void Report();
+
+ Code m_code = 0;
+ std::string m_message;
+ std::string m_filename;
+ std::string m_function;
+ uint32_t m_lineNumber = 0;
+ const ErrorBase* m_originatingObject = nullptr;
+ double m_timestamp = 0.0;
+};
diff --git a/wpilibc/shared/include/ErrorBase.h b/wpilibc/shared/include/ErrorBase.h
new file mode 100644
index 0000000..4730a29
--- /dev/null
+++ b/wpilibc/shared/include/ErrorBase.h
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "Error.h"
+
+#include "HAL/cpp/priority_mutex.h"
+#include "llvm/StringRef.h"
+
+#define wpi_setErrnoErrorWithContext(context) \
+ this->SetErrnoError((context), __FILE__, __FUNCTION__, __LINE__)
+#define wpi_setErrnoError() wpi_setErrnoErrorWithContext("")
+#define wpi_setImaqErrorWithContext(code, context) \
+ do { \
+ if ((code) != 0) \
+ this->SetImaqError((code), (context), __FILE__, __FUNCTION__, __LINE__); \
+ } while (0)
+#define wpi_setErrorWithContext(code, context) \
+ do { \
+ if ((code) != 0) \
+ this->SetError((code), (context), __FILE__, __FUNCTION__, __LINE__); \
+ } while (0)
+#define wpi_setError(code) wpi_setErrorWithContext(code, "")
+#define wpi_setStaticErrorWithContext(object, code, context) \
+ do { \
+ if ((code) != 0) \
+ object->SetError((code), (context), __FILE__, __FUNCTION__, __LINE__); \
+ } while (0)
+#define wpi_setStaticError(object, code) \
+ wpi_setStaticErrorWithContext(object, code, "")
+#define wpi_setGlobalErrorWithContext(code, context) \
+ do { \
+ if ((code) != 0) \
+ ErrorBase::SetGlobalError((code), (context), __FILE__, __FUNCTION__, \
+ __LINE__); \
+ } while (0)
+#define wpi_setGlobalError(code) wpi_setGlobalErrorWithContext(code, "")
+#define wpi_setWPIErrorWithContext(error, context) \
+ this->SetWPIError((wpi_error_s_##error), (wpi_error_value_##error), \
+ (context), __FILE__, __FUNCTION__, __LINE__)
+#define wpi_setWPIError(error) (wpi_setWPIErrorWithContext(error, ""))
+#define wpi_setStaticWPIErrorWithContext(object, error, context) \
+ object->SetWPIError((wpi_error_s_##error), (context), __FILE__, \
+ __FUNCTION__, __LINE__)
+#define wpi_setStaticWPIError(object, error) \
+ wpi_setStaticWPIErrorWithContext(object, error, "")
+#define wpi_setGlobalWPIErrorWithContext(error, context) \
+ ErrorBase::SetGlobalWPIError((wpi_error_s_##error), (context), __FILE__, \
+ __FUNCTION__, __LINE__)
+#define wpi_setGlobalWPIError(error) \
+ wpi_setGlobalWPIErrorWithContext(error, "")
+
+/**
+ * Base class for most objects.
+ * ErrorBase is the base class for most objects since it holds the generated
+ * error
+ * for that object. In addition, there is a single instance of a global error
+ * object
+ */
+class ErrorBase {
+ // TODO: Consider initializing instance variables and cleanup in destructor
+ public:
+ ErrorBase() = default;
+ virtual ~ErrorBase() = default;
+
+ ErrorBase(const ErrorBase&) = delete;
+ ErrorBase& operator=(const ErrorBase&) = delete;
+
+ virtual Error& GetError();
+ virtual const Error& GetError() const;
+ virtual void SetErrnoError(llvm::StringRef contextMessage,
+ llvm::StringRef filename, llvm::StringRef function,
+ uint32_t lineNumber) const;
+ virtual void SetImaqError(int success, llvm::StringRef contextMessage,
+ llvm::StringRef filename, llvm::StringRef function,
+ uint32_t lineNumber) const;
+ virtual void SetError(Error::Code code, llvm::StringRef contextMessage,
+ llvm::StringRef filename, llvm::StringRef function,
+ uint32_t lineNumber) const;
+ virtual void SetWPIError(llvm::StringRef errorMessage, Error::Code code,
+ llvm::StringRef contextMessage,
+ llvm::StringRef filename, llvm::StringRef function,
+ uint32_t lineNumber) const;
+ virtual void CloneError(const ErrorBase& rhs) const;
+ virtual void ClearError() const;
+ virtual bool StatusIsFatal() const;
+ static void SetGlobalError(Error::Code code, llvm::StringRef contextMessage,
+ llvm::StringRef filename, llvm::StringRef function,
+ uint32_t lineNumber);
+ static void SetGlobalWPIError(llvm::StringRef errorMessage,
+ llvm::StringRef contextMessage,
+ llvm::StringRef filename,
+ llvm::StringRef function, uint32_t lineNumber);
+ static Error& GetGlobalError();
+
+ protected:
+ mutable Error m_error;
+ // TODO: Replace globalError with a global list of all errors.
+ static priority_mutex _globalErrorMutex;
+ static Error _globalError;
+};
diff --git a/wpilibc/shared/include/GenericHID.h b/wpilibc/shared/include/GenericHID.h
new file mode 100644
index 0000000..bf1fe8e
--- /dev/null
+++ b/wpilibc/shared/include/GenericHID.h
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <stdint.h>
+
+/** GenericHID Interface
+ */
+class GenericHID {
+ public:
+ enum JoystickHand { kLeftHand = 0, kRightHand = 1 };
+
+ virtual ~GenericHID() = default;
+
+ virtual float GetX(JoystickHand hand = kRightHand) const = 0;
+ virtual float GetY(JoystickHand hand = kRightHand) const = 0;
+ virtual float GetZ() const = 0;
+ virtual float GetTwist() const = 0;
+ virtual float GetThrottle() const = 0;
+ virtual float GetRawAxis(uint32_t axis) const = 0;
+
+ virtual bool GetTrigger(JoystickHand hand = kRightHand) const = 0;
+ virtual bool GetTop(JoystickHand hand = kRightHand) const = 0;
+ virtual bool GetBumper(JoystickHand hand = kRightHand) const = 0;
+ virtual bool GetRawButton(uint32_t button) const = 0;
+
+ virtual int GetPOV(uint32_t pov = 0) const = 0;
+};
diff --git a/wpilibc/shared/include/HLUsageReporting.h b/wpilibc/shared/include/HLUsageReporting.h
new file mode 100644
index 0000000..5d8d34f
--- /dev/null
+++ b/wpilibc/shared/include/HLUsageReporting.h
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* 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
+
+class HLUsageReportingInterface {
+ public:
+ virtual ~HLUsageReportingInterface() = default;
+ virtual void ReportScheduler() = 0;
+ virtual void ReportSmartDashboard() = 0;
+};
+
+class HLUsageReporting {
+ private:
+ static HLUsageReportingInterface* impl;
+
+ public:
+ static void SetImplementation(HLUsageReportingInterface* i);
+ static void ReportScheduler();
+ static void ReportSmartDashboard();
+};
diff --git a/wpilibc/shared/include/LiveWindow/LiveWindow.h b/wpilibc/shared/include/LiveWindow/LiveWindow.h
new file mode 100644
index 0000000..7b40f67
--- /dev/null
+++ b/wpilibc/shared/include/LiveWindow/LiveWindow.h
@@ -0,0 +1,78 @@
+#ifndef _LIVE_WINDOW_H
+#define _LIVE_WINDOW_H
+
+#include "LiveWindow/LiveWindowSendable.h"
+#include "tables/ITable.h"
+#include "Commands/Scheduler.h"
+#include <vector>
+#include <map>
+#include <memory>
+
+struct LiveWindowComponent {
+ std::string subsystem;
+ std::string name;
+ bool isSensor = false;
+
+ LiveWindowComponent() = default;
+ LiveWindowComponent(std::string subsystem, std::string name, bool isSensor) {
+ this->subsystem = subsystem;
+ this->name = name;
+ this->isSensor = isSensor;
+ }
+};
+
+/**
+ * The LiveWindow class is the public interface for putting sensors and
+ * actuators
+ * on the LiveWindow.
+ *
+ * @author Brad Miller
+ */
+class LiveWindow {
+ public:
+ static LiveWindow *GetInstance();
+ void Run();
+ void AddSensor(const std::string &subsystem, const std::string &name,
+ LiveWindowSendable *component);
+ void AddSensor(const std::string &subsystem, const std::string &name,
+ LiveWindowSendable &component);
+ void AddSensor(const std::string &subsystem, const std::string &name,
+ std::shared_ptr<LiveWindowSendable> component);
+ void AddActuator(const std::string &subsystem, const std::string &name,
+ LiveWindowSendable *component);
+ void AddActuator(const std::string &subsystem, const std::string &name,
+ LiveWindowSendable &component);
+ void AddActuator(const std::string &subsystem, const std::string &name,
+ std::shared_ptr<LiveWindowSendable> component);
+
+ void AddSensor(std::string type, int channel, LiveWindowSendable *component);
+ void AddActuator(std::string type, int channel,
+ LiveWindowSendable *component);
+ void AddActuator(std::string type, int module, int channel,
+ LiveWindowSendable *component);
+
+ bool IsEnabled() const { return m_enabled; }
+ void SetEnabled(bool enabled);
+
+ protected:
+ LiveWindow();
+ virtual ~LiveWindow() = default;
+
+ private:
+ void UpdateValues();
+ void Initialize();
+ void InitializeLiveWindowComponents();
+
+ std::vector<std::shared_ptr<LiveWindowSendable>> m_sensors;
+ std::map<std::shared_ptr<LiveWindowSendable>, LiveWindowComponent> m_components;
+
+ std::shared_ptr<ITable> m_liveWindowTable;
+ std::shared_ptr<ITable> m_statusTable;
+
+ Scheduler *m_scheduler;
+
+ bool m_enabled = false;
+ bool m_firstTime = true;
+};
+
+#endif
diff --git a/wpilibc/shared/include/LiveWindow/LiveWindowSendable.h b/wpilibc/shared/include/LiveWindow/LiveWindowSendable.h
new file mode 100644
index 0000000..4f2023a
--- /dev/null
+++ b/wpilibc/shared/include/LiveWindow/LiveWindowSendable.h
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) Patrick Plenefisch 2012. 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 LIVEWINDOWSENDABLE_H_
+#define LIVEWINDOWSENDABLE_H_
+
+#include "SmartDashboard/Sendable.h"
+
+/**
+ * Live Window Sendable is a special type of object sendable to the live window.
+ *
+ * @author Patrick Plenefisch
+ */
+class LiveWindowSendable : public Sendable {
+ public:
+ /**
+ * Update the table for this sendable object with the latest
+ * values.
+ */
+ virtual void UpdateTable() = 0;
+
+ /**
+ * Start having this sendable object automatically respond to
+ * value changes reflect the value on the table.
+ */
+ virtual void StartLiveWindowMode() = 0;
+
+ /**
+ * Stop having this sendable object automatically respond to value
+ * changes.
+ */
+ virtual void StopLiveWindowMode() = 0;
+};
+
+#endif /* LIVEWINDOWSENDABLE_H_ */
diff --git a/wpilibc/shared/include/LiveWindow/LiveWindowStatusListener.h b/wpilibc/shared/include/LiveWindow/LiveWindowStatusListener.h
new file mode 100644
index 0000000..4fa3eb7
--- /dev/null
+++ b/wpilibc/shared/include/LiveWindow/LiveWindowStatusListener.h
@@ -0,0 +1,13 @@
+#ifndef _LIVE_WINDOW_STATUS_LISTENER_H
+#define _LIVE_WINDOW_STATUS_LISTENER_H
+
+#include "tables/ITable.h"
+#include "tables/ITableListener.h"
+
+class LiveWindowStatusListener : public ITableListener {
+ public:
+ virtual void ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew);
+};
+
+#endif
diff --git a/wpilibc/shared/include/Notifier.h b/wpilibc/shared/include/Notifier.h
new file mode 100644
index 0000000..056ed0a
--- /dev/null
+++ b/wpilibc/shared/include/Notifier.h
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <thread>
+#include <atomic>
+
+typedef void (*TimerEventHandler)(void *param);
+
+class Notifier : public ErrorBase {
+ public:
+ Notifier(TimerEventHandler handler, void *param = nullptr);
+ virtual ~Notifier();
+
+ Notifier(const Notifier&) = delete;
+ Notifier& operator=(const Notifier&) = delete;
+
+ void StartSingle(double delay);
+ void StartPeriodic(double period);
+ void Stop();
+
+ private:
+ static Notifier *timerQueueHead;
+ static priority_recursive_mutex queueMutex;
+ static priority_mutex halMutex;
+ static void *m_notifier;
+ static std::atomic<int> refcount;
+
+ static void ProcessQueue(
+ uint32_t mask, void *params); // process the timer queue on a timer event
+ static void
+ UpdateAlarm(); // update the FPGA alarm since the queue has changed
+ void InsertInQueue(
+ bool reschedule); // insert this Notifier in the timer queue
+ void DeleteFromQueue(); // delete this Notifier from the timer queue
+ TimerEventHandler m_handler; // address of the handler
+ void *m_param; // a parameter to pass to the handler
+ double m_period = 0; // the relative time (either periodic or single)
+ double m_expirationTime = 0; // absolute expiration time for the current event
+ Notifier *m_nextEvent = nullptr; // next Nofifier event
+ bool m_periodic = false; // true if this is a periodic event
+ bool m_queued = false; // indicates if this entry is queued
+ priority_mutex m_handlerMutex; // held by interrupt manager task while
+ // handler call is in progress
+
+#ifdef FRC_SIMULATOR
+ static std::thread m_task;
+ static std::atomic<bool> m_stopped;
+#endif
+ static void Run();
+};
diff --git a/wpilibc/shared/include/PIDController.h b/wpilibc/shared/include/PIDController.h
new file mode 100644
index 0000000..301356c
--- /dev/null
+++ b/wpilibc/shared/include/PIDController.h
@@ -0,0 +1,132 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "Controller.h"
+#include "LiveWindow/LiveWindow.h"
+#include "PIDInterface.h"
+#include "PIDSource.h"
+#include "Notifier.h"
+#include "HAL/cpp/priority_mutex.h"
+
+#include <memory>
+
+#include <atomic>
+#include <queue>
+
+class PIDOutput;
+
+/**
+ * Class implements a PID Control Loop.
+ *
+ * Creates a separate thread which reads the given PIDSource and takes
+ * care of the integral calculations, as well as writing the given
+ * PIDOutput
+ */
+class PIDController : public LiveWindowSendable,
+ public PIDInterface,
+ public ITableListener {
+ public:
+ PIDController(float p, float i, float d, PIDSource *source, PIDOutput *output,
+ float period = 0.05);
+ PIDController(float p, float i, float d, float f, PIDSource *source,
+ PIDOutput *output, float period = 0.05);
+ virtual ~PIDController();
+
+ PIDController(const PIDController&) = delete;
+ PIDController& operator=(const PIDController) = delete;
+
+ virtual float Get() const;
+ virtual void SetContinuous(bool continuous = true);
+ virtual void SetInputRange(float minimumInput, float maximumInput);
+ virtual void SetOutputRange(float minimumOutput, float maximumOutput);
+ virtual void SetPID(double p, double i, double d) override;
+ virtual void SetPID(double p, double i, double d, double f);
+ virtual double GetP() const override;
+ virtual double GetI() const override;
+ virtual double GetD() const override;
+ virtual double GetF() const;
+
+ virtual void SetSetpoint(float setpoint) override;
+ virtual double GetSetpoint() const override;
+
+ virtual float GetError() const;
+ virtual float GetAvgError() const;
+
+ virtual void SetPIDSourceType(PIDSourceType pidSource);
+ virtual PIDSourceType GetPIDSourceType() const;
+
+ virtual void SetTolerance(float percent);
+ virtual void SetAbsoluteTolerance(float absValue);
+ virtual void SetPercentTolerance(float percentValue);
+ virtual void SetToleranceBuffer(unsigned buf = 1);
+ virtual bool OnTarget() const;
+
+ virtual void Enable() override;
+ virtual void Disable() override;
+ virtual bool IsEnabled() const override;
+
+ virtual void Reset() override;
+
+ virtual void InitTable(std::shared_ptr<ITable> table) override;
+
+ protected:
+ PIDSource *m_pidInput;
+ PIDOutput *m_pidOutput;
+
+ std::shared_ptr<ITable> m_table;
+ virtual void Calculate();
+
+ private:
+ float m_P; // factor for "proportional" control
+ float m_I; // factor for "integral" control
+ float m_D; // factor for "derivative" control
+ float m_F; // factor for "feed forward" control
+ float m_maximumOutput = 1.0; // |maximum output|
+ float m_minimumOutput = -1.0; // |minimum output|
+ float m_maximumInput = 0; // maximum input - limit setpoint to this
+ float m_minimumInput = 0; // minimum input - limit setpoint to this
+ bool m_continuous = false; // do the endpoints wrap around? eg. Absolute encoder
+ bool m_enabled = false; // is the pid controller enabled
+ float m_prevInput = 0; // the prior sensor input (used to compute velocity)
+ double m_totalError = 0; // the sum of the errors for use in the integral calc
+ enum {
+ kAbsoluteTolerance,
+ kPercentTolerance,
+ kNoTolerance
+ } m_toleranceType = kNoTolerance;
+
+ // the percetage or absolute error that is considered on target.
+ float m_tolerance = 0.05;
+ float m_setpoint = 0;
+ float m_error = 0;
+ float m_result = 0;
+ float m_period;
+
+ // Length of buffer for averaging for tolerances.
+ std::atomic<unsigned> m_bufLength{1};
+ std::queue<double> m_buf;
+ double m_bufTotal = 0;
+
+ mutable priority_mutex m_mutex;
+
+ std::unique_ptr<Notifier> m_controlLoop;
+
+ void Initialize(float p, float i, float d, float f, PIDSource *source,
+ PIDOutput *output, float period = 0.05);
+ static void CallCalculate(void *controller);
+
+ virtual std::shared_ptr<ITable> GetTable() const override;
+ virtual std::string GetSmartDashboardType() const override;
+ virtual void ValueChanged(ITable *source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value,
+ bool isNew) override;
+ virtual void UpdateTable() override;
+ virtual void StartLiveWindowMode() override;
+ virtual void StopLiveWindowMode() override;
+};
diff --git a/wpilibc/shared/include/PIDInterface.h b/wpilibc/shared/include/PIDInterface.h
new file mode 100644
index 0000000..5d50199
--- /dev/null
+++ b/wpilibc/shared/include/PIDInterface.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include "Base.h"
+#include "Controller.h"
+#include "LiveWindow/LiveWindow.h"
+
+class PIDInterface : public Controller {
+ virtual void SetPID(double p, double i, double d) = 0;
+ virtual double GetP() const = 0;
+ virtual double GetI() const = 0;
+ virtual double GetD() const = 0;
+
+ virtual void SetSetpoint(float setpoint) = 0;
+ virtual double GetSetpoint() const = 0;
+
+ virtual void Enable() = 0;
+ virtual void Disable() = 0;
+ virtual bool IsEnabled() const = 0;
+
+ virtual void Reset() = 0;
+};
diff --git a/wpilibc/shared/include/PIDOutput.h b/wpilibc/shared/include/PIDOutput.h
new file mode 100644
index 0000000..40421d0
--- /dev/null
+++ b/wpilibc/shared/include/PIDOutput.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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"
+
+/**
+ * PIDOutput interface is a generic output for the PID class.
+ * PWMs use this class.
+ * Users implement this interface to allow for a PIDController to
+ * read directly from the inputs
+ */
+class PIDOutput {
+ public:
+ virtual void PIDWrite(float output) = 0;
+};
diff --git a/wpilibc/shared/include/PIDSource.h b/wpilibc/shared/include/PIDSource.h
new file mode 100644
index 0000000..8f46ea4
--- /dev/null
+++ b/wpilibc/shared/include/PIDSource.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
+
+enum class PIDSourceType { kDisplacement, kRate };
+
+/**
+ * PIDSource interface is a generic sensor source for the PID class.
+ * All sensors that can be used with the PID class will implement the PIDSource
+ * that
+ * returns a standard value that will be used in the PID code.
+ */
+class PIDSource {
+ public:
+ virtual void SetPIDSourceType(PIDSourceType pidSource);
+ PIDSourceType GetPIDSourceType() const;
+ virtual double PIDGet() = 0;
+
+ protected:
+ PIDSourceType m_pidSource = PIDSourceType::kDisplacement;
+};
diff --git a/wpilibc/shared/include/Resource.h b/wpilibc/shared/include/Resource.h
new file mode 100644
index 0000000..f7061c8
--- /dev/null
+++ b/wpilibc/shared/include/Resource.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 "ErrorBase.h"
+#include <stdint.h>
+#include <memory>
+#include <vector>
+
+#include "HAL/cpp/priority_mutex.h"
+
+/**
+ * The Resource class is a convenient way to track allocated resources.
+ * It tracks them as indicies in the range [0 .. elements - 1].
+ * E.g. the library uses this to track hardware channel allocation.
+ *
+ * The Resource class does not allocate the hardware channels or other
+ * resources; it just tracks which indices were marked in use by
+ * Allocate and not yet freed by Free.
+ */
+class Resource : public ErrorBase {
+ public:
+ virtual ~Resource() = default;
+
+ Resource(const Resource&) = delete;
+ Resource& operator=(const Resource&) = delete;
+
+ static void CreateResourceObject(std::unique_ptr<Resource>& r, uint32_t elements);
+ explicit Resource(uint32_t size);
+ uint32_t Allocate(const std::string &resourceDesc);
+ uint32_t Allocate(uint32_t index, const std::string &resourceDesc);
+ void Free(uint32_t index);
+
+ private:
+ std::vector<bool> m_isAllocated;
+ priority_recursive_mutex m_allocateLock;
+
+ static priority_recursive_mutex m_createLock;
+};
diff --git a/wpilibc/shared/include/RobotState.h b/wpilibc/shared/include/RobotState.h
new file mode 100644
index 0000000..53bf32a
--- /dev/null
+++ b/wpilibc/shared/include/RobotState.h
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <memory>
+
+class RobotStateInterface {
+ public:
+ virtual ~RobotStateInterface() = default;
+ virtual bool IsDisabled() const = 0;
+ virtual bool IsEnabled() const = 0;
+ virtual bool IsOperatorControl() const = 0;
+ virtual bool IsAutonomous() const = 0;
+ virtual bool IsTest() const = 0;
+};
+
+class RobotState {
+ private:
+ static std::shared_ptr<RobotStateInterface> impl;
+
+ public:
+ static void SetImplementation(RobotStateInterface& i);
+ static void SetImplementation(std::shared_ptr<RobotStateInterface> i);
+ static bool IsDisabled();
+ static bool IsEnabled();
+ static bool IsOperatorControl();
+ static bool IsAutonomous();
+ static bool IsTest();
+};
diff --git a/wpilibc/shared/include/SensorBase.h b/wpilibc/shared/include/SensorBase.h
new file mode 100644
index 0000000..8638b5e
--- /dev/null
+++ b/wpilibc/shared/include/SensorBase.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 "ErrorBase.h"
+#include <stdio.h>
+#include "Base.h"
+
+/**
+ * Base class for all sensors.
+ * Stores most recent status information as well as containing utility functions
+ * for checking
+ * channels and error processing.
+ */
+class SensorBase : public ErrorBase {
+ public:
+ SensorBase();
+ virtual ~SensorBase() = default;
+
+ SensorBase(const SensorBase&) = delete;
+ SensorBase& operator=(const SensorBase&) = delete;
+
+ static void DeleteSingletons();
+
+ static uint32_t GetDefaultSolenoidModule() { return 0; }
+
+ static bool CheckSolenoidModule(uint8_t moduleNumber);
+ static bool CheckDigitalChannel(uint32_t channel);
+ static bool CheckRelayChannel(uint32_t channel);
+ static bool CheckPWMChannel(uint32_t channel);
+ static bool CheckAnalogInput(uint32_t channel);
+ static bool CheckAnalogOutput(uint32_t channel);
+ static bool CheckSolenoidChannel(uint32_t channel);
+ static bool CheckPDPChannel(uint32_t channel);
+
+ static const uint32_t kDigitalChannels = 26;
+ static const uint32_t kAnalogInputs = 8;
+ static const uint32_t kAnalogOutputs = 2;
+ static const uint32_t kSolenoidChannels = 8;
+ static const uint32_t kSolenoidModules = 2;
+ static const uint32_t kPwmChannels = 20;
+ static const uint32_t kRelayChannels = 8;
+ static const uint32_t kPDPChannels = 16;
+ static const uint32_t kChassisSlots = 8;
+
+ protected:
+ void AddToSingletonList();
+
+ static void* m_digital_ports[kDigitalChannels];
+ static void* m_relay_ports[kRelayChannels];
+ static void* m_pwm_ports[kPwmChannels];
+
+ private:
+ static SensorBase* m_singletonList;
+ SensorBase* m_nextSingleton = nullptr;
+};
diff --git a/wpilibc/shared/include/SmartDashboard/NamedSendable.h b/wpilibc/shared/include/SmartDashboard/NamedSendable.h
new file mode 100644
index 0000000..0dd1b24
--- /dev/null
+++ b/wpilibc/shared/include/SmartDashboard/NamedSendable.h
@@ -0,0 +1,28 @@
+/*
+ * NamedSendable.h
+ *
+ * Created on: Oct 19, 2012
+ * Author: Mitchell Wills
+ */
+
+#ifndef NAMEDSENDABLE_H_
+#define NAMEDSENDABLE_H_
+
+#include <string>
+#include "SmartDashboard/Sendable.h"
+
+/**
+ * The interface for sendable objects that gives the sendable a default name in
+ * the Smart Dashboard
+ *
+ */
+class NamedSendable : public Sendable {
+ public:
+ /**
+ * @return the name of the subtable of SmartDashboard that the Sendable object
+ * will use
+ */
+ virtual std::string GetName() const = 0;
+};
+
+#endif /* NAMEDSENDABLE_H_ */
diff --git a/wpilibc/shared/include/SmartDashboard/Sendable.h b/wpilibc/shared/include/SmartDashboard/Sendable.h
new file mode 100644
index 0000000..613c7f8
--- /dev/null
+++ b/wpilibc/shared/include/SmartDashboard/Sendable.h
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __SMART_DASHBOARD_DATA__
+#define __SMART_DASHBOARD_DATA__
+
+#include <string>
+#include <memory>
+#include "tables/ITable.h"
+
+class Sendable {
+ public:
+ /**
+ * Initializes a table for this sendable object.
+ * @param subtable The table to put the values in.
+ */
+ virtual void InitTable(std::shared_ptr<ITable> subtable) = 0;
+
+ /**
+ * @return the table that is currently associated with the sendable
+ */
+ virtual std::shared_ptr<ITable> GetTable() const = 0;
+
+ /**
+ * @return the string representation of the named data type that will be used
+ * by the smart dashboard for this sendable
+ */
+ virtual std::string GetSmartDashboardType() const = 0;
+};
+
+#endif
diff --git a/wpilibc/shared/include/SmartDashboard/SendableChooser.h b/wpilibc/shared/include/SmartDashboard/SendableChooser.h
new file mode 100644
index 0000000..bfdf877
--- /dev/null
+++ b/wpilibc/shared/include/SmartDashboard/SendableChooser.h
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __SENDABLE_CHOOSER_H__
+#define __SENDABLE_CHOOSER_H__
+
+#include "SmartDashboard/Sendable.h"
+#include "tables/ITable.h"
+#include <map>
+#include <memory>
+#include <string>
+
+/**
+ * The {@link SendableChooser} class is a useful tool for presenting a selection
+ * of options
+ * to the {@link SmartDashboard}.
+ *
+ * <p>For instance, you may wish to be able to select between multiple
+ * autonomous modes.
+ * You can do this by putting every possible {@link Command} you want to run as
+ * an autonomous into
+ * a {@link SendableChooser} and then put it into the {@link SmartDashboard} to
+ * have a list of options
+ * appear on the laptop. Once autonomous starts, simply ask the {@link
+ * SendableChooser} what the selected
+ * value is.</p>
+ *
+ * @see SmartDashboard
+ */
+class SendableChooser : public Sendable {
+ public:
+ virtual ~SendableChooser() = default;
+
+ void AddObject(const std::string &name, void *object);
+ void AddDefault(const std::string &name, void *object);
+ void *GetSelected();
+
+ virtual void InitTable(std::shared_ptr<ITable> subtable);
+ virtual std::shared_ptr<ITable> GetTable() const;
+ virtual std::string GetSmartDashboardType() const;
+
+ private:
+ std::string m_defaultChoice;
+ std::map<std::string, void *> m_choices;
+ std::shared_ptr<ITable> m_table;
+};
+
+#endif
diff --git a/wpilibc/shared/include/SmartDashboard/SmartDashboard.h b/wpilibc/shared/include/SmartDashboard/SmartDashboard.h
new file mode 100644
index 0000000..a1cfb22
--- /dev/null
+++ b/wpilibc/shared/include/SmartDashboard/SmartDashboard.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 __SMART_DASHBOARD_H__
+#define __SMART_DASHBOARD_H__
+
+#include "SensorBase.h"
+#include <map>
+#include <string>
+#include "SmartDashboard/Sendable.h"
+#include "SmartDashboard/NamedSendable.h"
+#include "tables/ITable.h"
+
+class SmartDashboard : public SensorBase {
+ public:
+ static void init();
+
+ static void PutData(llvm::StringRef key, Sendable *data);
+ static void PutData(NamedSendable *value);
+ static Sendable *GetData(llvm::StringRef keyName);
+
+ static void PutBoolean(llvm::StringRef keyName, bool value);
+ static bool GetBoolean(llvm::StringRef keyName, bool defaultValue);
+
+ static void PutNumber(llvm::StringRef keyName, double value);
+ static double GetNumber(llvm::StringRef keyName, double defaultValue);
+
+ static void PutString(llvm::StringRef keyName, llvm::StringRef value);
+ static std::string GetString(llvm::StringRef keyName,
+ llvm::StringRef defaultValue);
+
+ static void PutValue(llvm::StringRef keyName,
+ std::shared_ptr<nt::Value> value);
+ static std::shared_ptr<nt::Value> GetValue(llvm::StringRef keyName);
+
+ private:
+ virtual ~SmartDashboard() = default;
+
+ /** The {@link NetworkTable} used by {@link SmartDashboard} */
+ static std::shared_ptr<ITable> m_table;
+
+ /**
+ * A map linking tables in the SmartDashboard to the {@link
+ * SmartDashboardData} objects
+ * they came from.
+ */
+ static std::map<std::shared_ptr<ITable> , Sendable *> m_tablesToData;
+};
+
+#endif
diff --git a/wpilibc/shared/include/Task.h b/wpilibc/shared/include/Task.h
new file mode 100644
index 0000000..7752d0d
--- /dev/null
+++ b/wpilibc/shared/include/Task.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 "ErrorBase.h"
+#include "HAL/Task.hpp"
+#include <iostream>
+#include <string>
+#include <thread>
+
+/**
+ * Wrapper class around std::thread that allows changing thread priority
+ */
+class Task : public ErrorBase {
+ public:
+ static const uint32_t kDefaultPriority = 60;
+
+ Task() = default;
+ Task(const Task&) = delete;
+ Task& operator=(const Task&) = delete;
+ Task& operator=(Task&& task);
+
+ template <class Function, class... Args>
+ Task(const std::string& name, Function&& function, Args&&... args);
+
+ virtual ~Task();
+
+ bool joinable() const noexcept;
+ void join();
+ void detach();
+ std::thread::id get_id() const noexcept;
+ std::thread::native_handle_type native_handle();
+
+ bool Verify();
+
+ int32_t GetPriority();
+
+ bool SetPriority(int32_t priority);
+
+ std::string GetName() const;
+
+ private:
+ std::thread m_thread;
+ std::string m_taskName;
+ bool HandleError(STATUS results);
+};
+
+#include "Task.inc"
diff --git a/wpilibc/shared/include/Task.inc b/wpilibc/shared/include/Task.inc
new file mode 100644
index 0000000..3514b92
--- /dev/null
+++ b/wpilibc/shared/include/Task.inc
@@ -0,0 +1,26 @@
+#include "HAL/HAL.hpp"
+#include <atomic>
+
+/**
+ * Create and launch a task.
+ *
+ * @param name The name of the task. "FRC_" will be prepended to the task name.
+ * @param function The address of the function to run as the new task.
+ * @param args A parameter pack of arguments to pass to the function.
+ */
+template <class Function, class... Args>
+Task::Task(const std::string& name, Function&& function, Args&&... args) {
+ m_taskName = "FRC_";
+ m_taskName += name;
+
+ std::cout << "[HAL] Starting task " << m_taskName << "..." << std::endl;
+
+ m_thread = std::thread(std::forward<std::decay_t<Function>>(function),
+ std::forward<Args>(args)...);
+ //TODO: lvuser does not currently have permissions to set the priority.
+ //SetPriority(kDefaultPriority);
+
+ static std::atomic<int32_t> instances{0};
+ instances++;
+ HALReport(HALUsageReporting::kResourceType_Task, instances, 0, m_taskName.c_str());
+}
diff --git a/wpilibc/shared/include/Timer.h b/wpilibc/shared/include/Timer.h
new file mode 100644
index 0000000..60b9604
--- /dev/null
+++ b/wpilibc/shared/include/Timer.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "HAL/cpp/priority_mutex.h"
+
+typedef void (*TimerInterruptHandler)(void *param);
+
+void Wait(double seconds);
+double GetClock();
+double GetTime();
+
+/**
+ * Timer objects measure accumulated time in seconds.
+ * The timer object functions like a stopwatch. It can be started, stopped, and
+ * cleared. When the
+ * timer is running its value counts up in seconds. When stopped, the timer
+ * holds the current
+ * value. The implementation simply records the time when started and subtracts
+ * the current time
+ * whenever the value is requested.
+ */
+class Timer {
+ public:
+ Timer();
+ virtual ~Timer() = default;
+
+ Timer(const Timer&) = delete;
+ Timer& operator=(const Timer&) = delete;
+
+ double Get() const;
+ void Reset();
+ void Start();
+ void Stop();
+ bool HasPeriodPassed(double period);
+
+ static double GetFPGATimestamp();
+ static double GetPPCTimestamp();
+ static double GetMatchTime();
+
+ // The time, in seconds, at which the 32-bit FPGA timestamp rolls over to 0
+ static const double kRolloverTime;
+
+ private:
+ double m_startTime = 0.0;
+ double m_accumulatedTime = 0.0;
+ bool m_running = false;
+ mutable priority_mutex m_mutex;
+};
diff --git a/wpilibc/shared/include/Utility.h b/wpilibc/shared/include/Utility.h
new file mode 100644
index 0000000..69edb63
--- /dev/null
+++ b/wpilibc/shared/include/Utility.h
@@ -0,0 +1,51 @@
+/*---------------------------------------------------------------------------*/
+/* 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
+
+/** @file
+ * Contains global utility functions
+ */
+
+#include <stdint.h>
+#include <string>
+
+#define wpi_assert(condition) \
+ wpi_assert_impl(condition, #condition, "", __FILE__, __LINE__, __FUNCTION__)
+#define wpi_assertWithMessage(condition, message) \
+ wpi_assert_impl(condition, #condition, message, __FILE__, __LINE__, \
+ __FUNCTION__)
+
+#define wpi_assertEqual(a, b) \
+ wpi_assertEqual_impl(a, b, #a, #b, "", __FILE__, __LINE__, __FUNCTION__)
+#define wpi_assertEqualWithMessage(a, b, message) \
+ wpi_assertEqual_impl(a, b, #a, #b, message, __FILE__, __LINE__, __FUNCTION__)
+
+#define wpi_assertNotEqual(a, b) \
+ wpi_assertNotEqual_impl(a, b, #a, #b, "", __FILE__, __LINE__, __FUNCTION__)
+#define wpi_assertNotEqualWithMessage(a, b, message) \
+ wpi_assertNotEqual_impl(a, b, #a, #b, message, __FILE__, __LINE__, \
+ __FUNCTION__)
+
+bool wpi_assert_impl(bool conditionValue, const char *conditionText,
+ const char *message, const char *fileName,
+ uint32_t lineNumber, const char *funcName);
+bool wpi_assertEqual_impl(int valueA, int valueB, const char *valueAString,
+ const char *valueBString, const char *message,
+ const char *fileName, uint32_t lineNumber,
+ const char *funcName);
+bool wpi_assertNotEqual_impl(int valueA, int valueB, const char *valueAString,
+ const char *valueBString, const char *message,
+ const char *fileName, uint32_t lineNumber,
+ const char *funcName);
+
+void wpi_suspendOnAssertEnabled(bool enabled);
+
+uint16_t GetFPGAVersion();
+uint32_t GetFPGARevision();
+uint32_t GetFPGATime();
+bool GetUserButton();
+std::string GetStackTrace(uint32_t offset);
diff --git a/wpilibc/shared/include/WPIErrors.h b/wpilibc/shared/include/WPIErrors.h
new file mode 100644
index 0000000..8346fdf
--- /dev/null
+++ b/wpilibc/shared/include/WPIErrors.h
@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "stdint.h"
+
+#ifdef WPI_ERRORS_DEFINE_STRINGS
+#define S(label, offset, message) \
+ const char * wpi_error_s_##label = message; \
+ const int32_t wpi_error_value_##label = offset
+#else
+#define S(label, offset, message) \
+ extern const char * wpi_error_s_##label; \
+ const int32_t wpi_error_value_##label = offset
+#endif
+
+/*
+ * Fatal errors
+ */
+S(ModuleIndexOutOfRange, -1,
+ "Allocating module that is out of range or not found");
+S(ChannelIndexOutOfRange, -1, "Allocating channel that is out of range");
+S(NotAllocated, -2, "Attempting to free unallocated resource");
+S(ResourceAlreadyAllocated, -3, "Attempted to reuse an allocated resource");
+S(NoAvailableResources, -4, "No available resources to allocate");
+S(NullParameter, -5, "A pointer parameter to a method is nullptr");
+S(Timeout, -6, "A timeout has been exceeded");
+S(CompassManufacturerError, -7, "Compass manufacturer doesn't match HiTechnic");
+S(CompassTypeError, -8,
+ "Compass type doesn't match expected type for HiTechnic compass");
+S(IncompatibleMode, -9, "The object is in an incompatible mode");
+S(AnalogTriggerLimitOrderError, -10,
+ "AnalogTrigger limits error. Lower limit > Upper Limit");
+S(AnalogTriggerPulseOutputError, -11,
+ "Attempted to read AnalogTrigger pulse output.");
+S(TaskError, -12, "Task can't be started");
+S(TaskIDError, -13, "Task error: Invalid ID.");
+S(TaskDeletedError, -14, "Task error: Task already deleted.");
+S(TaskOptionsError, -15, "Task error: Invalid options.");
+S(TaskMemoryError, -16, "Task can't be started due to insufficient memory.");
+S(TaskPriorityError, -17, "Task error: Invalid priority [1-255].");
+S(DriveUninitialized, -18, "RobotDrive not initialized for the C interface");
+S(CompressorNonMatching, -19,
+ "Compressor slot/channel doesn't match previous instance");
+S(CompressorAlreadyDefined, -20, "Creating a second compressor instance");
+S(CompressorUndefined, -21,
+ "Using compressor functions without defining compressor");
+S(InconsistentArrayValueAdded, -22,
+ "When packing data into an array to the dashboard, not all values added were "
+ "of the same type.");
+S(MismatchedComplexTypeClose, -23,
+ "When packing data to the dashboard, a Close for a complex type was called "
+ "without a matching Open.");
+S(DashboardDataOverflow, -24,
+ "When packing data to the dashboard, too much data was packed and the buffer "
+ "overflowed.");
+S(DashboardDataCollision, -25,
+ "The same buffer was used for packing data and for printing.");
+S(EnhancedIOMissing, -26, "IO is not attached or Enhanced IO is not enabled.");
+S(LineNotOutput, -27,
+ "Cannot SetDigitalOutput for a line not configured for output.");
+S(ParameterOutOfRange, -28, "A parameter is out of range.");
+S(SPIClockRateTooLow, -29, "SPI clock rate was below the minimum supported");
+S(JaguarVersionError, -30, "Jaguar firmware version error");
+S(JaguarMessageNotFound, -31, "Jaguar message not found");
+S(NetworkTablesReadError, -40, "Error reading NetworkTables socket");
+S(NetworkTablesBufferFull, -41, "Buffer full writing to NetworkTables socket");
+S(NetworkTablesWrongType, -42,
+ "The wrong type was read from the NetworkTables entry");
+S(NetworkTablesCorrupt, -43, "NetworkTables data stream is corrupt");
+S(SmartDashboardMissingKey, -43, "SmartDashboard data does not exist");
+S(CommandIllegalUse, -50, "Illegal use of Command");
+S(UnsupportedInSimulation, -80, "Unsupported in simulation");
+
+/*
+ * Warnings
+ */
+S(SampleRateTooHigh, 1, "Analog module sample rate is too high");
+S(VoltageOutOfRange, 2,
+ "Voltage to convert to raw value is out of range [-10; 10]");
+S(CompressorTaskError, 3, "Compressor task won't start");
+S(LoopTimingError, 4, "Digital module loop timing is not the expected value");
+S(NonBinaryDigitalValue, 5, "Digital output value is not 0 or 1");
+S(IncorrectBatteryChannel, 6,
+ "Battery measurement channel is not correct value");
+S(BadJoystickIndex, 7, "Joystick index is out of range, should be 0-3");
+S(BadJoystickAxis, 8, "Joystick axis or POV is out of range");
+S(InvalidMotorIndex, 9, "Motor index is out of range, should be 0-3");
+S(DriverStationTaskError, 10, "Driver Station task won't start");
+S(EnhancedIOPWMPeriodOutOfRange, 11,
+ "Driver Station Enhanced IO PWM Output period out of range.");
+S(SPIWriteNoMOSI, 12, "Cannot write to SPI port with no MOSI output");
+S(SPIReadNoMISO, 13, "Cannot read from SPI port with no MISO input");
+S(SPIReadNoData, 14, "No data available to read from SPI");
+S(IncompatibleState, 15,
+ "Incompatible State: The operation cannot be completed");
+
+#undef S
diff --git a/wpilibc/shared/include/interfaces/Accelerometer.h b/wpilibc/shared/include/interfaces/Accelerometer.h
new file mode 100644
index 0000000..528d120
--- /dev/null
+++ b/wpilibc/shared/include/interfaces/Accelerometer.h
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 3-axis accelerometers
+ */
+class Accelerometer {
+ public:
+ virtual ~Accelerometer() = default;
+
+ enum Range { kRange_2G = 0, kRange_4G = 1, kRange_8G = 2, kRange_16G = 3 };
+
+ /**
+ * Common interface for setting the measuring range of an accelerometer.
+ *
+ * @param range The maximum acceleration, positive or negative, that the
+ * accelerometer will measure. Not all accelerometers support all ranges.
+ */
+ virtual void SetRange(Range range) = 0;
+
+ /**
+ * Common interface for getting the x axis acceleration
+ *
+ * @return The acceleration along the x axis in g-forces
+ */
+ virtual double GetX() = 0;
+
+ /**
+ * Common interface for getting the y axis acceleration
+ *
+ * @return The acceleration along the y axis in g-forces
+ */
+ virtual double GetY() = 0;
+
+ /**
+ * Common interface for getting the z axis acceleration
+ *
+ * @return The acceleration along the z axis in g-forces
+ */
+ virtual double GetZ() = 0;
+};
diff --git a/wpilibc/shared/include/interfaces/Gyro.h b/wpilibc/shared/include/interfaces/Gyro.h
new file mode 100644
index 0000000..8317ca5
--- /dev/null
+++ b/wpilibc/shared/include/interfaces/Gyro.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 yaw rate gyros
+ */
+class Gyro {
+ public:
+ virtual ~Gyro() = default;
+
+ /**
+ * Calibrate the gyro by running for a number of samples and computing the
+ * center value. 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.
+ */
+ virtual void Calibrate() = 0;
+
+ /**
+ * 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.
+ */
+ virtual void Reset() = 0;
+
+ /**
+ * 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 it will continue from 360 to 361 degrees. This
+ * allows algorithms that wouldn't want to see a discontinuity in the gyro
+ * output as it sweeps past from 360 to 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.
+ */
+ virtual float GetAngle() const = 0;
+
+ /**
+ * 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
+ */
+ virtual double GetRate() const = 0;
+};
diff --git a/wpilibc/shared/include/interfaces/Potentiometer.h b/wpilibc/shared/include/interfaces/Potentiometer.h
new file mode 100644
index 0000000..d52892d
--- /dev/null
+++ b/wpilibc/shared/include/interfaces/Potentiometer.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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 INTERFACES_POTENTIOMETER_H
+#define INTERFACES_POTENTIOMETER_H
+
+#include "PIDSource.h"
+
+/**
+ * Interface for potentiometers.
+ */
+class Potentiometer : public PIDSource {
+ public:
+ virtual ~Potentiometer() = default;
+
+ /**
+ * Common interface for getting the current value of a potentiometer.
+ *
+ * @return The current set speed. Value is between -1.0 and 1.0.
+ */
+ virtual double Get() const = 0;
+
+ virtual void SetPIDSourceType(PIDSourceType pidSource) override;
+};
+
+#endif
diff --git a/wpilibc/shared/src/Buttons/Button.cpp b/wpilibc/shared/src/Buttons/Button.cpp
new file mode 100644
index 0000000..b57fd89
--- /dev/null
+++ b/wpilibc/shared/src/Buttons/Button.cpp
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Buttons/Button.h"
+
+/**
+ * Specifies the command to run when a button is first pressed
+ * @param command The pointer to the command to run
+ */
+void Button::WhenPressed(Command *command) { WhenActive(command); }
+
+/**
+ * Specifies the command to be scheduled while the button is pressed
+ * The command will be scheduled repeatedly while the button is pressed and will
+ * be canceled when the button is released.
+ * @param command The pointer to the command to run
+ */
+void Button::WhileHeld(Command *command) { WhileActive(command); }
+
+/**
+ * Specifies the command to run when the button is released
+ * The command will be scheduled a single time.
+ * @param The pointer to the command to run
+ */
+void Button::WhenReleased(Command *command) { WhenInactive(command); }
+
+/**
+ * Cancels the specificed command when the button is pressed
+ * @param The command to be canceled
+ */
+void Button::CancelWhenPressed(Command *command) { CancelWhenActive(command); }
+
+/**
+ * Toggle the specified command when the button is pressed
+ * @param The command to be toggled
+ */
+void Button::ToggleWhenPressed(Command *command) { ToggleWhenActive(command); }
diff --git a/wpilibc/shared/src/Buttons/ButtonScheduler.cpp b/wpilibc/shared/src/Buttons/ButtonScheduler.cpp
new file mode 100644
index 0000000..ddc9025
--- /dev/null
+++ b/wpilibc/shared/src/Buttons/ButtonScheduler.cpp
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Buttons/ButtonScheduler.h"
+
+#include "Commands/Scheduler.h"
+
+ButtonScheduler::ButtonScheduler(bool last, Trigger *button, Command *orders)
+ : m_pressedLast(last), m_button(button), m_command(orders) {}
+
+void ButtonScheduler::Start() { Scheduler::GetInstance()->AddButton(this); }
diff --git a/wpilibc/shared/src/Buttons/CancelButtonScheduler.cpp b/wpilibc/shared/src/Buttons/CancelButtonScheduler.cpp
new file mode 100644
index 0000000..1d2b6cc
--- /dev/null
+++ b/wpilibc/shared/src/Buttons/CancelButtonScheduler.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Buttons/CancelButtonScheduler.h"
+
+#include "Buttons/Button.h"
+#include "Commands/Command.h"
+
+CancelButtonScheduler::CancelButtonScheduler(bool last, Trigger *button,
+ Command *orders)
+ : ButtonScheduler(last, button, orders) {
+ pressedLast = m_button->Grab();
+}
+
+void CancelButtonScheduler::Execute() {
+ if (m_button->Grab()) {
+ if (!pressedLast) {
+ pressedLast = true;
+ m_command->Cancel();
+ }
+ } else {
+ pressedLast = false;
+ }
+}
diff --git a/wpilibc/shared/src/Buttons/HeldButtonScheduler.cpp b/wpilibc/shared/src/Buttons/HeldButtonScheduler.cpp
new file mode 100644
index 0000000..04c8e3b
--- /dev/null
+++ b/wpilibc/shared/src/Buttons/HeldButtonScheduler.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Buttons/HeldButtonScheduler.h"
+
+#include "Buttons/Button.h"
+#include "Commands/Command.h"
+
+HeldButtonScheduler::HeldButtonScheduler(bool last, Trigger *button,
+ Command *orders)
+ : ButtonScheduler(last, button, orders) {}
+
+void HeldButtonScheduler::Execute() {
+ if (m_button->Grab()) {
+ m_pressedLast = true;
+ m_command->Start();
+ } else {
+ if (m_pressedLast) {
+ m_pressedLast = false;
+ m_command->Cancel();
+ }
+ }
+}
diff --git a/wpilibc/shared/src/Buttons/InternalButton.cpp b/wpilibc/shared/src/Buttons/InternalButton.cpp
new file mode 100644
index 0000000..8c9b22a
--- /dev/null
+++ b/wpilibc/shared/src/Buttons/InternalButton.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Buttons/InternalButton.h"
+
+InternalButton::InternalButton(bool inverted)
+ : m_pressed(inverted), m_inverted(inverted) {}
+
+void InternalButton::SetInverted(bool inverted) { m_inverted = inverted; }
+
+void InternalButton::SetPressed(bool pressed) { m_pressed = pressed; }
+
+bool InternalButton::Get() { return m_pressed ^ m_inverted; }
diff --git a/wpilibc/shared/src/Buttons/JoystickButton.cpp b/wpilibc/shared/src/Buttons/JoystickButton.cpp
new file mode 100644
index 0000000..c2bdf62
--- /dev/null
+++ b/wpilibc/shared/src/Buttons/JoystickButton.cpp
@@ -0,0 +1,13 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Buttons/JoystickButton.h"
+
+JoystickButton::JoystickButton(GenericHID *joystick, int buttonNumber)
+ : m_joystick(joystick), m_buttonNumber(buttonNumber) {}
+
+bool JoystickButton::Get() { return m_joystick->GetRawButton(m_buttonNumber); }
diff --git a/wpilibc/shared/src/Buttons/NetworkButton.cpp b/wpilibc/shared/src/Buttons/NetworkButton.cpp
new file mode 100644
index 0000000..858703c
--- /dev/null
+++ b/wpilibc/shared/src/Buttons/NetworkButton.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Buttons/NetworkButton.h"
+#include "networktables/NetworkTable.h"
+
+NetworkButton::NetworkButton(const std::string &tableName, const std::string &field)
+ : // TODO how is this supposed to work???
+ m_netTable(NetworkTable::GetTable(tableName)),
+ m_field(field) {}
+
+NetworkButton::NetworkButton(std::shared_ptr<ITable> table, const std::string &field)
+ : m_netTable(table), m_field(field) {}
+
+bool NetworkButton::Get() {
+ /*if (m_netTable->isConnected())
+ return m_netTable->GetBoolean(m_field.c_str());
+ else
+ return false;*/
+ return m_netTable->GetBoolean(m_field, false);
+}
diff --git a/wpilibc/shared/src/Buttons/PressedButtonScheduler.cpp b/wpilibc/shared/src/Buttons/PressedButtonScheduler.cpp
new file mode 100644
index 0000000..5a0c506
--- /dev/null
+++ b/wpilibc/shared/src/Buttons/PressedButtonScheduler.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Buttons/PressedButtonScheduler.h"
+
+#include "Buttons/Button.h"
+#include "Commands/Command.h"
+
+PressedButtonScheduler::PressedButtonScheduler(bool last, Trigger *button,
+ Command *orders)
+ : ButtonScheduler(last, button, orders) {}
+
+void PressedButtonScheduler::Execute() {
+ if (m_button->Grab()) {
+ if (!m_pressedLast) {
+ m_pressedLast = true;
+ m_command->Start();
+ }
+ } else {
+ m_pressedLast = false;
+ }
+}
diff --git a/wpilibc/shared/src/Buttons/ReleasedButtonScheduler.cpp b/wpilibc/shared/src/Buttons/ReleasedButtonScheduler.cpp
new file mode 100644
index 0000000..e7131e6
--- /dev/null
+++ b/wpilibc/shared/src/Buttons/ReleasedButtonScheduler.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Buttons/ReleasedButtonScheduler.h"
+
+#include "Buttons/Button.h"
+#include "Commands/Command.h"
+
+ReleasedButtonScheduler::ReleasedButtonScheduler(bool last, Trigger *button,
+ Command *orders)
+ : ButtonScheduler(last, button, orders) {}
+
+void ReleasedButtonScheduler::Execute() {
+ if (m_button->Grab()) {
+ m_pressedLast = true;
+ } else {
+ if (m_pressedLast) {
+ m_pressedLast = false;
+ m_command->Start();
+ }
+ }
+}
diff --git a/wpilibc/shared/src/Buttons/ToggleButtonScheduler.cpp b/wpilibc/shared/src/Buttons/ToggleButtonScheduler.cpp
new file mode 100644
index 0000000..c4048d2
--- /dev/null
+++ b/wpilibc/shared/src/Buttons/ToggleButtonScheduler.cpp
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Buttons/ToggleButtonScheduler.h"
+
+#include "Buttons/Button.h"
+#include "Commands/Command.h"
+
+ToggleButtonScheduler::ToggleButtonScheduler(bool last, Trigger *button,
+ Command *orders)
+ : ButtonScheduler(last, button, orders) {
+ pressedLast = m_button->Grab();
+}
+
+void ToggleButtonScheduler::Execute() {
+ if (m_button->Grab()) {
+ if (!pressedLast) {
+ pressedLast = true;
+ if (m_command->IsRunning()) {
+ m_command->Cancel();
+ } else {
+ m_command->Start();
+ }
+ }
+ } else {
+ pressedLast = false;
+ }
+}
diff --git a/wpilibc/shared/src/Buttons/Trigger.cpp b/wpilibc/shared/src/Buttons/Trigger.cpp
new file mode 100644
index 0000000..54a1eba
--- /dev/null
+++ b/wpilibc/shared/src/Buttons/Trigger.cpp
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Buttons/Button.h"
+
+#include "Buttons/HeldButtonScheduler.h"
+#include "Buttons/PressedButtonScheduler.h"
+#include "Buttons/ReleasedButtonScheduler.h"
+#include "Buttons/ToggleButtonScheduler.h"
+#include "Buttons/CancelButtonScheduler.h"
+
+bool Trigger::Grab() {
+ if (Get())
+ return true;
+ else if (m_table != nullptr) {
+ // if (m_table->isConnected())//TODO is connected on button?
+ return m_table->GetBoolean("pressed", false);
+ /*else
+ return false;*/
+ } else
+ return false;
+}
+
+void Trigger::WhenActive(Command *command) {
+ auto pbs = new PressedButtonScheduler(Grab(), this, command);
+ pbs->Start();
+}
+
+void Trigger::WhileActive(Command *command) {
+ auto hbs = new HeldButtonScheduler(Grab(), this, command);
+ hbs->Start();
+}
+
+void Trigger::WhenInactive(Command *command) {
+ auto rbs = new ReleasedButtonScheduler(Grab(), this, command);
+ rbs->Start();
+}
+
+void Trigger::CancelWhenActive(Command *command) {
+ auto cbs = new CancelButtonScheduler(Grab(), this, command);
+ cbs->Start();
+}
+
+void Trigger::ToggleWhenActive(Command *command) {
+ auto tbs = new ToggleButtonScheduler(Grab(), this, command);
+ tbs->Start();
+}
+
+std::string Trigger::GetSmartDashboardType() const { return "Button"; }
+
+void Trigger::InitTable(std::shared_ptr<ITable> table) {
+ m_table = table;
+ if (m_table != nullptr) {
+ m_table->PutBoolean("pressed", Get());
+ }
+}
+
+std::shared_ptr<ITable> Trigger::GetTable() const { return m_table; }
diff --git a/wpilibc/shared/src/Commands/Command.cpp b/wpilibc/shared/src/Commands/Command.cpp
new file mode 100644
index 0000000..37effbe
--- /dev/null
+++ b/wpilibc/shared/src/Commands/Command.cpp
@@ -0,0 +1,392 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Commands/Command.h"
+#include "Commands/CommandGroup.h"
+#include "Commands/Scheduler.h"
+#include "RobotState.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+#include <typeinfo>
+
+static const std::string kName = "name";
+static const std::string kRunning = "running";
+static const std::string kIsParented = "isParented";
+
+int Command::m_commandCounter = 0;
+
+/**
+ * Creates a new command.
+ * The name of this command will be default.
+ */
+Command::Command() : Command("", -1.0) {}
+
+/**
+ * Creates a new command with the given name and no timeout.
+ * @param name the name for this command
+ */
+Command::Command(const std::string &name) : Command(name, -1.0) {}
+
+/**
+ * Creates a new command with the given timeout and a default name.
+ * @param timeout the time (in seconds) before this command "times out"
+ * @see Command#isTimedOut() isTimedOut()
+ */
+Command::Command(double timeout) : Command("", timeout) {}
+
+/**
+ * Creates a new command with the given name and timeout.
+ * @param name the name of the command
+ * @param timeout the time (in seconds) before this command "times out"
+ * @see Command#isTimedOut() isTimedOut()
+ */
+Command::Command(const std::string &name, double timeout) {
+ // We use -1.0 to indicate no timeout.
+ if (timeout < 0.0 && timeout != -1.0)
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+
+ m_timeout = timeout;
+
+ // If name contains an empty string
+ if (name.length() == 0) {
+ m_name = std::string("Command_") + std::string(typeid(*this).name());
+ }
+ else {
+ m_name = name;
+ }
+}
+
+Command::~Command() {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Get the ID (sequence number) for this command
+ * The ID is a unique sequence number that is incremented for each command.
+ * @return the ID of this command
+ */
+int Command::GetID() const { return m_commandID; }
+
+/**
+ * Sets the timeout of this command.
+ * @param timeout the timeout (in seconds)
+ * @see Command#isTimedOut() isTimedOut()
+ */
+void Command::SetTimeout(double timeout) {
+ if (timeout < 0.0)
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+ else
+ m_timeout = timeout;
+}
+
+/**
+ * Returns the time since this command was initialized (in seconds).
+ * This function will work even if there is no specified timeout.
+ * @return the time since this command was initialized (in seconds).
+ */
+double Command::TimeSinceInitialized() const {
+ if (m_startTime < 0.0)
+ return 0.0;
+ else
+ return Timer::GetFPGATimestamp() - m_startTime;
+}
+
+/**
+ * This method specifies that the given {@link Subsystem} is used by this
+ * command.
+ * This method is crucial to the functioning of the Command System in general.
+ *
+ * <p>Note that the recommended way to call this method is in the
+ * constructor.</p>
+ *
+ * @param subsystem the {@link Subsystem} required
+ * @see Subsystem
+ */
+void Command::Requires(Subsystem *subsystem) {
+ if (!AssertUnlocked("Can not add new requirement to command")) return;
+
+ if (subsystem != nullptr)
+ m_requirements.insert(subsystem);
+ else
+ wpi_setWPIErrorWithContext(NullParameter, "subsystem");
+}
+
+/**
+ * Called when the command has been removed.
+ * This will call {@link Command#interrupted() interrupted()} or {@link
+ * Command#end() end()}.
+ */
+void Command::Removed() {
+ if (m_initialized) {
+ if (IsCanceled()) {
+ Interrupted();
+ _Interrupted();
+ } else {
+ End();
+ _End();
+ }
+ }
+ m_initialized = false;
+ m_canceled = false;
+ m_running = false;
+ if (m_table != nullptr) m_table->PutBoolean(kRunning, false);
+}
+
+/**
+ * Starts up the command. Gets the command ready to start.
+ * <p>Note that the command will eventually start, however it will not
+ * necessarily
+ * do so immediately, and may in fact be canceled before initialize is even
+ * called.</p>
+ */
+void Command::Start() {
+ LockChanges();
+ if (m_parent != nullptr)
+ wpi_setWPIErrorWithContext(
+ CommandIllegalUse,
+ "Can not start a command that is part of a command group");
+
+ Scheduler::GetInstance()->AddCommand(this);
+}
+
+/**
+ * The run method is used internally to actually run the commands.
+ * @return whether or not the command should stay within the {@link Scheduler}.
+ */
+bool Command::Run() {
+ if (!m_runWhenDisabled && m_parent == nullptr && RobotState::IsDisabled())
+ Cancel();
+
+ if (IsCanceled()) return false;
+
+ if (!m_initialized) {
+ m_initialized = true;
+ StartTiming();
+ _Initialize();
+ Initialize();
+ }
+ _Execute();
+ Execute();
+ return !IsFinished();
+}
+
+void Command::_Initialize() {}
+
+void Command::_Interrupted() {}
+
+void Command::_Execute() {}
+
+void Command::_End() {}
+
+/**
+ * Called to indicate that the timer should start.
+ * This is called right before {@link Command#initialize() initialize()} is,
+ * inside the
+ * {@link Command#run() run()} method.
+ */
+void Command::StartTiming() { m_startTime = Timer::GetFPGATimestamp(); }
+
+/**
+ * Returns whether or not the {@link Command#timeSinceInitialized()
+ * timeSinceInitialized()}
+ * method returns a number which is greater than or equal to the timeout for the
+ * command.
+ * If there is no timeout, this will always return false.
+ * @return whether the time has expired
+ */
+bool Command::IsTimedOut() const {
+ return m_timeout != -1 && TimeSinceInitialized() >= m_timeout;
+}
+
+/**
+ * Returns the requirements (as an std::set of {@link Subsystem Subsystems}
+ * pointers) of this command
+ * @return the requirements (as an std::set of {@link Subsystem Subsystems}
+ * pointers) of this command
+ */
+Command::SubsystemSet Command::GetRequirements() const {
+ return m_requirements;
+}
+
+/**
+ * Prevents further changes from being made
+ */
+void Command::LockChanges() { m_locked = true; }
+
+/**
+ * If changes are locked, then this will generate a CommandIllegalUse error.
+ * @param message the message to report on error (it is appended by a default
+ * message)
+ * @return true if assert passed, false if assert failed
+ */
+bool Command::AssertUnlocked(const std::string &message) {
+ if (m_locked) {
+ std::string buf = message + " after being started or being added to a command group";
+ wpi_setWPIErrorWithContext(CommandIllegalUse, buf);
+ return false;
+ }
+ return true;
+}
+
+/**
+ * Sets the parent of this command. No actual change is made to the group.
+ * @param parent the parent
+ */
+void Command::SetParent(CommandGroup *parent) {
+ if (parent == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "parent");
+ } else if (m_parent != nullptr) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Can not give command to a command group after "
+ "already being put in a command group");
+ } else {
+ LockChanges();
+ m_parent = parent;
+ if (m_table != nullptr) {
+ m_table->PutBoolean(kIsParented, true);
+ }
+ }
+}
+
+/**
+ * This is used internally to mark that the command has been started.
+ * The lifecycle of a command is:
+ *
+ * startRunning() is called.
+ * run() is called (multiple times potentially)
+ * removed() is called
+ *
+ * It is very important that startRunning and removed be called in order or some
+ * assumptions
+ * of the code will be broken.
+ */
+void Command::StartRunning() {
+ m_running = true;
+ m_startTime = -1;
+ if (m_table != nullptr) m_table->PutBoolean(kRunning, true);
+}
+
+/**
+ * Returns whether or not the command is running.
+ * This may return true even if the command has just been canceled, as it may
+ * not have yet called {@link Command#interrupted()}.
+ * @return whether or not the command is running
+ */
+bool Command::IsRunning() const { return m_running; }
+
+/**
+ * This will cancel the current command.
+ * <p>This will cancel the current command eventually. It can be called
+ * multiple times.
+ * And it can be called when the command is not running. If the command is
+ * running though,
+ * then the command will be marked as canceled and eventually removed.</p>
+ * <p>A command can not be canceled
+ * if it is a part of a command group, you must cancel the command group
+ * instead.</p>
+ */
+void Command::Cancel() {
+ if (m_parent != nullptr)
+ wpi_setWPIErrorWithContext(
+ CommandIllegalUse,
+ "Can not cancel a command that is part of a command group");
+
+ _Cancel();
+}
+
+/**
+ * This works like cancel(), except that it doesn't throw an exception if it is
+ * a part
+ * of a command group. Should only be called by the parent command group.
+ */
+void Command::_Cancel() {
+ if (IsRunning()) m_canceled = true;
+}
+
+/**
+ * Returns whether or not this has been canceled.
+ * @return whether or not this has been canceled
+ */
+bool Command::IsCanceled() const { return m_canceled; }
+
+/**
+ * Returns whether or not this command can be interrupted.
+ * @return whether or not this command can be interrupted
+ */
+bool Command::IsInterruptible() const { return m_interruptible; }
+
+/**
+ * Sets whether or not this command can be interrupted.
+ * @param interruptible whether or not this command can be interrupted
+ */
+void Command::SetInterruptible(bool interruptible) {
+ m_interruptible = interruptible;
+}
+
+/**
+ * Checks if the command requires the given {@link Subsystem}.
+ * @param system the system
+ * @return whether or not the subsystem is required (false if given nullptr)
+ */
+bool Command::DoesRequire(Subsystem *system) const {
+ return m_requirements.count(system) > 0;
+}
+
+/**
+ * Returns the {@link CommandGroup} that this command is a part of.
+ * Will return null if this {@link Command} is not in a group.
+ * @return the {@link CommandGroup} that this command is a part of (or null if
+ * not in group)
+ */
+CommandGroup *Command::GetGroup() const { return m_parent; }
+
+/**
+ * Sets whether or not this {@link Command} should run when the robot is
+ * disabled.
+ *
+ * <p>By default a command will not run when the robot is disabled, and will in
+ * fact be canceled.</p>
+ * @param run whether or not this command should run when the robot is disabled
+ */
+void Command::SetRunWhenDisabled(bool run) { m_runWhenDisabled = run; }
+
+/**
+ * Returns whether or not this {@link Command} will run when the robot is
+ * disabled, or if it will cancel itself.
+ * @return whether or not this {@link Command} will run when the robot is
+ * disabled, or if it will cancel itself
+ */
+bool Command::WillRunWhenDisabled() const { return m_runWhenDisabled; }
+
+std::string Command::GetName() const {
+ return m_name;
+}
+
+std::string Command::GetSmartDashboardType() const { return "Command"; }
+
+void Command::InitTable(std::shared_ptr<ITable> table) {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+ m_table = table;
+ if (m_table != nullptr) {
+ m_table->PutString(kName, GetName());
+ m_table->PutBoolean(kRunning, IsRunning());
+ m_table->PutBoolean(kIsParented, m_parent != nullptr);
+ m_table->AddTableListener(kRunning, this, false);
+ }
+}
+
+std::shared_ptr<ITable> Command::GetTable() const { return m_table; }
+
+void Command::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (!value->IsBoolean()) return;
+ if (value->GetBoolean()) {
+ if (!IsRunning()) Start();
+ } else {
+ if (IsRunning()) Cancel();
+ }
+}
diff --git a/wpilibc/shared/src/Commands/CommandGroup.cpp b/wpilibc/shared/src/Commands/CommandGroup.cpp
new file mode 100644
index 0000000..edb5ec9
--- /dev/null
+++ b/wpilibc/shared/src/Commands/CommandGroup.cpp
@@ -0,0 +1,338 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Commands/CommandGroup.h"
+#include "WPIErrors.h"
+
+/**
+ * Creates a new {@link CommandGroup CommandGroup} with the given name.
+ * @param name the name for this command group
+ */
+CommandGroup::CommandGroup(const std::string &name) : Command(name) {}
+
+/**
+ * Adds a new {@link Command Command} to the group. The {@link Command Command}
+ * will be started after
+ * all the previously added {@link Command Commands}.
+ *
+ * <p>Note that any requirements the given {@link Command Command} has will be
+ * added to the
+ * group. For this reason, a {@link Command Command's} requirements can not be
+ * changed after
+ * being added to a group.</p>
+ *
+ * <p>It is recommended that this method be called in the constructor.</p>
+ *
+ * @param command The {@link Command Command} to be added
+ */
+void CommandGroup::AddSequential(Command *command) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+ if (!AssertUnlocked("Cannot add new command to command group")) return;
+
+ command->SetParent(this);
+
+ m_commands.push_back(
+ CommandGroupEntry(command, CommandGroupEntry::kSequence_InSequence));
+ // Iterate through command->GetRequirements() and call Requires() on each
+ // required subsystem
+ Command::SubsystemSet requirements = command->GetRequirements();
+ auto iter = requirements.begin();
+ for (; iter != requirements.end(); iter++) Requires(*iter);
+}
+
+/**
+ * Adds a new {@link Command Command} to the group with a given timeout.
+ * The {@link Command Command} will be started after all the previously added
+ * commands.
+ *
+ * <p>Once the {@link Command Command} is started, it will be run until it
+ * finishes or the time
+ * expires, whichever is sooner. Note that the given {@link Command Command}
+ * will have no
+ * knowledge that it is on a timer.</p>
+ *
+ * <p>Note that any requirements the given {@link Command Command} has will be
+ * added to the
+ * group. For this reason, a {@link Command Command's} requirements can not be
+ * changed after
+ * being added to a group.</p>
+ *
+ * <p>It is recommended that this method be called in the constructor.</p>
+ *
+ * @param command The {@link Command Command} to be added
+ * @param timeout The timeout (in seconds)
+ */
+void CommandGroup::AddSequential(Command *command, double timeout) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+ if (!AssertUnlocked("Cannot add new command to command group")) return;
+ if (timeout < 0.0) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+ return;
+ }
+
+ command->SetParent(this);
+
+ m_commands.push_back(CommandGroupEntry(
+ command, CommandGroupEntry::kSequence_InSequence, timeout));
+ // Iterate through command->GetRequirements() and call Requires() on each
+ // required subsystem
+ Command::SubsystemSet requirements = command->GetRequirements();
+ auto iter = requirements.begin();
+ for (; iter != requirements.end(); iter++) Requires(*iter);
+}
+
+/**
+ * Adds a new child {@link Command} to the group. The {@link Command} will be
+ * started after
+ * all the previously added {@link Command Commands}.
+ *
+ * <p>Instead of waiting for the child to finish, a {@link CommandGroup} will
+ * have it
+ * run at the same time as the subsequent {@link Command Commands}. The child
+ * will run until either
+ * it finishes, a new child with conflicting requirements is started, or
+ * the main sequence runs a {@link Command} with conflicting requirements. In
+ * the latter
+ * two cases, the child will be canceled even if it says it can't be
+ * interrupted.</p>
+ *
+ * <p>Note that any requirements the given {@link Command Command} has will be
+ * added to the
+ * group. For this reason, a {@link Command Command's} requirements can not be
+ * changed after
+ * being added to a group.</p>
+ *
+ * <p>It is recommended that this method be called in the constructor.</p>
+ *
+ * @param command The command to be added
+ */
+void CommandGroup::AddParallel(Command *command) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+ if (!AssertUnlocked("Cannot add new command to command group")) return;
+
+ command->SetParent(this);
+
+ m_commands.push_back(
+ CommandGroupEntry(command, CommandGroupEntry::kSequence_BranchChild));
+ // Iterate through command->GetRequirements() and call Requires() on each
+ // required subsystem
+ Command::SubsystemSet requirements = command->GetRequirements();
+ auto iter = requirements.begin();
+ for (; iter != requirements.end(); iter++) Requires(*iter);
+}
+
+/**
+ * Adds a new child {@link Command} to the group with the given timeout. The
+ * {@link Command} will be started after
+ * all the previously added {@link Command Commands}.
+ *
+ * <p>Once the {@link Command Command} is started, it will run until it
+ * finishes, is interrupted,
+ * or the time expires, whichever is sooner. Note that the given {@link Command
+ * Command} will have no
+ * knowledge that it is on a timer.</p>
+ *
+ * <p>Instead of waiting for the child to finish, a {@link CommandGroup} will
+ * have it
+ * run at the same time as the subsequent {@link Command Commands}. The child
+ * will run until either
+ * it finishes, the timeout expires, a new child with conflicting requirements
+ * is started, or
+ * the main sequence runs a {@link Command} with conflicting requirements. In
+ * the latter
+ * two cases, the child will be canceled even if it says it can't be
+ * interrupted.</p>
+ *
+ * <p>Note that any requirements the given {@link Command Command} has will be
+ * added to the
+ * group. For this reason, a {@link Command Command's} requirements can not be
+ * changed after
+ * being added to a group.</p>
+ *
+ * <p>It is recommended that this method be called in the constructor.</p>
+ *
+ * @param command The command to be added
+ * @param timeout The timeout (in seconds)
+ */
+void CommandGroup::AddParallel(Command *command, double timeout) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+ if (!AssertUnlocked("Cannot add new command to command group")) return;
+ if (timeout < 0.0) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+ return;
+ }
+
+ command->SetParent(this);
+
+ m_commands.push_back(CommandGroupEntry(
+ command, CommandGroupEntry::kSequence_BranchChild, timeout));
+ // Iterate through command->GetRequirements() and call Requires() on each
+ // required subsystem
+ Command::SubsystemSet requirements = command->GetRequirements();
+ auto iter = requirements.begin();
+ for (; iter != requirements.end(); iter++) Requires(*iter);
+}
+
+void CommandGroup::_Initialize() { m_currentCommandIndex = -1; }
+
+void CommandGroup::_Execute() {
+ CommandGroupEntry entry;
+ Command *cmd = nullptr;
+ bool firstRun = false;
+
+ if (m_currentCommandIndex == -1) {
+ firstRun = true;
+ m_currentCommandIndex = 0;
+ }
+
+ while ((unsigned)m_currentCommandIndex < m_commands.size()) {
+ if (cmd != nullptr) {
+ if (entry.IsTimedOut()) cmd->_Cancel();
+
+ if (cmd->Run()) {
+ break;
+ } else {
+ cmd->Removed();
+ m_currentCommandIndex++;
+ firstRun = true;
+ cmd = nullptr;
+ continue;
+ }
+ }
+
+ entry = m_commands[m_currentCommandIndex];
+ cmd = nullptr;
+
+ switch (entry.m_state) {
+ case CommandGroupEntry::kSequence_InSequence:
+ cmd = entry.m_command;
+ if (firstRun) {
+ cmd->StartRunning();
+ CancelConflicts(cmd);
+ firstRun = false;
+ }
+ break;
+
+ case CommandGroupEntry::kSequence_BranchPeer:
+ m_currentCommandIndex++;
+ entry.m_command->Start();
+ break;
+
+ case CommandGroupEntry::kSequence_BranchChild:
+ m_currentCommandIndex++;
+ CancelConflicts(entry.m_command);
+ entry.m_command->StartRunning();
+ m_children.push_back(entry);
+ break;
+ }
+ }
+
+ // Run Children
+ auto iter = m_children.begin();
+ for (; iter != m_children.end();) {
+ entry = *iter;
+ Command *child = entry.m_command;
+ if (entry.IsTimedOut()) child->_Cancel();
+
+ if (!child->Run()) {
+ child->Removed();
+ iter = m_children.erase(iter);
+ } else {
+ iter++;
+ }
+ }
+}
+
+void CommandGroup::_End() {
+ // Theoretically, we don't have to check this, but we do if teams override the
+ // IsFinished method
+ if (m_currentCommandIndex != -1 &&
+ (unsigned)m_currentCommandIndex < m_commands.size()) {
+ Command *cmd = m_commands[m_currentCommandIndex].m_command;
+ cmd->_Cancel();
+ cmd->Removed();
+ }
+
+ auto iter = m_children.begin();
+ for (; iter != m_children.end(); iter++) {
+ Command *cmd = iter->m_command;
+ cmd->_Cancel();
+ cmd->Removed();
+ }
+ m_children.clear();
+}
+
+void CommandGroup::_Interrupted() { _End(); }
+
+// Can be overwritten by teams
+void CommandGroup::Initialize() {}
+
+// Can be overwritten by teams
+void CommandGroup::Execute() {}
+
+// Can be overwritten by teams
+void CommandGroup::End() {}
+
+// Can be overwritten by teams
+void CommandGroup::Interrupted() {}
+
+bool CommandGroup::IsFinished() {
+ return (unsigned)m_currentCommandIndex >= m_commands.size() &&
+ m_children.empty();
+}
+
+bool CommandGroup::IsInterruptible() const {
+ if (!Command::IsInterruptible()) return false;
+
+ if (m_currentCommandIndex != -1 &&
+ (unsigned)m_currentCommandIndex < m_commands.size()) {
+ Command *cmd = m_commands[m_currentCommandIndex].m_command;
+ if (!cmd->IsInterruptible()) return false;
+ }
+
+ auto iter = m_children.cbegin();
+ for (; iter != m_children.cend(); iter++) {
+ if (!iter->m_command->IsInterruptible()) return false;
+ }
+
+ return true;
+}
+
+void CommandGroup::CancelConflicts(Command *command) {
+ auto childIter = m_children.begin();
+ for (; childIter != m_children.end();) {
+ Command *child = childIter->m_command;
+ bool erased = false;
+
+ Command::SubsystemSet requirements = command->GetRequirements();
+ auto requirementIter = requirements.begin();
+ for (; requirementIter != requirements.end(); requirementIter++) {
+ if (child->DoesRequire(*requirementIter)) {
+ child->_Cancel();
+ child->Removed();
+ childIter = m_children.erase(childIter);
+ erased = true;
+ break;
+ }
+ }
+ if (!erased) childIter++;
+ }
+}
+
+int CommandGroup::GetSize() const { return m_children.size(); }
diff --git a/wpilibc/shared/src/Commands/CommandGroupEntry.cpp b/wpilibc/shared/src/Commands/CommandGroupEntry.cpp
new file mode 100644
index 0000000..c9f3fa4
--- /dev/null
+++ b/wpilibc/shared/src/Commands/CommandGroupEntry.cpp
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Commands/CommandGroupEntry.h"
+
+#include "Commands/Command.h"
+
+CommandGroupEntry::CommandGroupEntry(Command *command, Sequence state,
+ double timeout)
+ : m_timeout(timeout), m_command(command), m_state(state) {}
+
+bool CommandGroupEntry::IsTimedOut() const {
+ if (m_timeout < 0.0) return false;
+ double time = m_command->TimeSinceInitialized();
+ if (time == 0.0) return false;
+ return time >= m_timeout;
+}
diff --git a/wpilibc/shared/src/Commands/PIDCommand.cpp b/wpilibc/shared/src/Commands/PIDCommand.cpp
new file mode 100644
index 0000000..c6ac919
--- /dev/null
+++ b/wpilibc/shared/src/Commands/PIDCommand.cpp
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Commands/PIDCommand.h"
+
+#include "float.h"
+
+PIDCommand::PIDCommand(const std::string &name, double p, double i, double d, double f,
+ double period)
+ : Command(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double f, double period) {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+PIDCommand::PIDCommand(const std::string &name, double p, double i, double d)
+ : Command(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(const std::string &name, double p, double i, double d,
+ double period)
+ : Command(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double period) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+void PIDCommand::_Initialize() { m_controller->Enable(); }
+
+void PIDCommand::_End() { m_controller->Disable(); }
+
+void PIDCommand::_Interrupted() { _End(); }
+
+void PIDCommand::SetSetpointRelative(double deltaSetpoint) {
+ SetSetpoint(GetSetpoint() + deltaSetpoint);
+}
+
+void PIDCommand::PIDWrite(float output) { UsePIDOutput(output); }
+
+double PIDCommand::PIDGet() { return ReturnPIDInput(); }
+
+std::shared_ptr<PIDController> PIDCommand::GetPIDController() const {
+ return m_controller;
+}
+
+void PIDCommand::SetSetpoint(double setpoint) {
+ m_controller->SetSetpoint(setpoint);
+}
+
+double PIDCommand::GetSetpoint() const { return m_controller->GetSetpoint(); }
+
+double PIDCommand::GetPosition() { return ReturnPIDInput(); }
+
+std::string PIDCommand::GetSmartDashboardType() const { return "PIDCommand"; }
+void PIDCommand::InitTable(std::shared_ptr<ITable> table) {
+ m_controller->InitTable(table);
+ Command::InitTable(table);
+}
diff --git a/wpilibc/shared/src/Commands/PIDSubsystem.cpp b/wpilibc/shared/src/Commands/PIDSubsystem.cpp
new file mode 100644
index 0000000..b3d327f
--- /dev/null
+++ b/wpilibc/shared/src/Commands/PIDSubsystem.cpp
@@ -0,0 +1,225 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Commands/PIDSubsystem.h"
+#include "PIDController.h"
+#include "float.h"
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ */
+PIDSubsystem::PIDSubsystem(const std::string &name, double p, double i, double d)
+ : Subsystem(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedforward value
+ */
+PIDSubsystem::PIDSubsystem(const std::string &name, double p, double i, double d,
+ double f)
+ : Subsystem(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values. It will also space the time
+ * between PID loop calculations to be equal to the given period.
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedfoward value
+ * @param period the time (in seconds) between calculations
+ */
+PIDSubsystem::PIDSubsystem(const std::string &name, double p, double i, double d,
+ double f, double period)
+ : Subsystem(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ * It will use the class name as its name.
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ */
+PIDSubsystem::PIDSubsystem(double p, double i, double d)
+ : Subsystem("PIDSubsystem") {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ * It will use the class name as its name.
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedforward value
+ */
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f)
+ : Subsystem("PIDSubsystem") {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ * It will use the class name as its name.
+ * It will also space the time
+ * between PID loop calculations to be equal to the given period.
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedforward value
+ * @param period the time (in seconds) between calculations
+ */
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f,
+ double period)
+ : Subsystem("PIDSubsystem") {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+/**
+ * Enables the internal {@link PIDController}
+ */
+void PIDSubsystem::Enable() { m_controller->Enable(); }
+
+/**
+ * Disables the internal {@link PIDController}
+ */
+void PIDSubsystem::Disable() { m_controller->Disable(); }
+
+/**
+ * Returns the {@link PIDController} used by this {@link PIDSubsystem}.
+ * Use this if you would like to fine tune the pid loop.
+ *
+ * @return the {@link PIDController} used by this {@link PIDSubsystem}
+ */
+std::shared_ptr<PIDController> PIDSubsystem::GetPIDController() {
+ return m_controller;
+}
+
+/**
+ * Sets the setpoint to the given value. If {@link PIDCommand#SetRange(double,
+ * double) SetRange(...)}
+ * was called,
+ * then the given setpoint
+ * will be trimmed to fit within the range.
+ * @param setpoint the new setpoint
+ */
+void PIDSubsystem::SetSetpoint(double setpoint) {
+ m_controller->SetSetpoint(setpoint);
+}
+
+/**
+ * Adds the given value to the setpoint.
+ * If {@link PIDCommand#SetRange(double, double) SetRange(...)} was used,
+ * then the bounds will still be honored by this method.
+ * @param deltaSetpoint the change in the setpoint
+ */
+void PIDSubsystem::SetSetpointRelative(double deltaSetpoint) {
+ SetSetpoint(GetSetpoint() + deltaSetpoint);
+}
+
+/**
+ * Return the current setpoint
+ * @return The current setpoint
+ */
+double PIDSubsystem::GetSetpoint() { return m_controller->GetSetpoint(); }
+
+/**
+ * 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 PIDSubsystem::SetInputRange(float minimumInput, float maximumInput) {
+ m_controller->SetInputRange(minimumInput, maximumInput);
+}
+
+/**
+ * Sets the maximum and minimum values to write.
+ *
+ * @param minimumOutput the minimum value to write to the output
+ * @param maximumOutput the maximum value to write to the output
+ */
+void PIDSubsystem::SetOutputRange(float minimumOutput, float maximumOutput) {
+ m_controller->SetOutputRange(minimumOutput, maximumOutput);
+}
+
+/*
+ * Set the absolute error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDSubsystem::SetAbsoluteTolerance(float absValue) {
+ m_controller->SetAbsoluteTolerance(absValue);
+}
+
+/*
+ * Set the percentage error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDSubsystem::SetPercentTolerance(float percent) {
+ m_controller->SetPercentTolerance(percent);
+}
+
+/*
+ * 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. Use OnTarget() in the IsFinished() method of
+ * commands
+ * that use this subsystem.
+ *
+ * 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.
+ *
+ * @return true if the error is within the percentage tolerance of the input
+ * range
+ */
+bool PIDSubsystem::OnTarget() const { return m_controller->OnTarget(); }
+
+/**
+ * Returns the current position
+ * @return the current position
+ */
+double PIDSubsystem::GetPosition() { return ReturnPIDInput(); }
+
+/**
+ * Returns the current rate
+ * @return the current rate
+ */
+double PIDSubsystem::GetRate() { return ReturnPIDInput(); }
+
+void PIDSubsystem::PIDWrite(float output) { UsePIDOutput(output); }
+
+double PIDSubsystem::PIDGet() { return ReturnPIDInput(); }
+
+std::string PIDSubsystem::GetSmartDashboardType() const { return "PIDCommand"; }
+void PIDSubsystem::InitTable(std::shared_ptr<ITable> table) {
+ m_controller->InitTable(table);
+ Subsystem::InitTable(table);
+}
diff --git a/wpilibc/shared/src/Commands/PrintCommand.cpp b/wpilibc/shared/src/Commands/PrintCommand.cpp
new file mode 100644
index 0000000..b5e8dc7
--- /dev/null
+++ b/wpilibc/shared/src/Commands/PrintCommand.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Commands/PrintCommand.h"
+#include "stdio.h"
+#include <sstream>
+
+PrintCommand::PrintCommand(const std::string &message)
+ : Command(((std::stringstream &)(std::stringstream("Print \"") << message
+ << "\""))
+ .str()
+ .c_str()) {
+ m_message = message;
+}
+
+void PrintCommand::Initialize() { printf("%s", m_message.c_str()); }
+
+void PrintCommand::Execute() {}
+
+bool PrintCommand::IsFinished() { return true; }
+
+void PrintCommand::End() {}
+
+void PrintCommand::Interrupted() {}
diff --git a/wpilibc/shared/src/Commands/Scheduler.cpp b/wpilibc/shared/src/Commands/Scheduler.cpp
new file mode 100644
index 0000000..0b796e3
--- /dev/null
+++ b/wpilibc/shared/src/Commands/Scheduler.cpp
@@ -0,0 +1,273 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Commands/Scheduler.h"
+
+#include "Buttons/ButtonScheduler.h"
+#include "Commands/Subsystem.h"
+#include "HLUsageReporting.h"
+#include "WPIErrors.h"
+#include <iostream>
+#include <set>
+#include <algorithm>
+
+Scheduler::Scheduler() {
+ HLUsageReporting::ReportScheduler();
+}
+
+/**
+ * Returns the {@link Scheduler}, creating it if one does not exist.
+ * @return the {@link Scheduler}
+ */
+Scheduler *Scheduler::GetInstance() {
+ static Scheduler instance;
+ return &instance;
+}
+
+void Scheduler::SetEnabled(bool enabled) { m_enabled = enabled; }
+
+/**
+ * Add a command to be scheduled later.
+ * In any pass through the scheduler, all commands are added to the additions
+ * list, then
+ * at the end of the pass, they are all scheduled.
+ * @param command The command to be scheduled
+ */
+void Scheduler::AddCommand(Command *command) {
+ std::lock_guard<priority_mutex> sync(m_additionsLock);
+ if (std::find(m_additions.begin(), m_additions.end(), command) !=
+ m_additions.end())
+ return;
+ m_additions.push_back(command);
+}
+
+void Scheduler::AddButton(ButtonScheduler *button) {
+ std::lock_guard<priority_mutex> sync(m_buttonsLock);
+ m_buttons.push_back(button);
+}
+
+void Scheduler::ProcessCommandAddition(Command *command) {
+ if (command == nullptr) return;
+
+ // Check to make sure no adding during adding
+ if (m_adding) {
+ wpi_setWPIErrorWithContext(IncompatibleState,
+ "Can not start command from cancel method");
+ return;
+ }
+
+ // Only add if not already in
+ auto found = m_commands.find(command);
+ if (found == m_commands.end()) {
+ // Check that the requirements can be had
+ Command::SubsystemSet requirements = command->GetRequirements();
+ Command::SubsystemSet::iterator iter;
+ for (iter = requirements.begin(); iter != requirements.end(); iter++) {
+ Subsystem *lock = *iter;
+ if (lock->GetCurrentCommand() != nullptr &&
+ !lock->GetCurrentCommand()->IsInterruptible())
+ return;
+ }
+
+ // Give it the requirements
+ m_adding = true;
+ for (iter = requirements.begin(); iter != requirements.end(); iter++) {
+ Subsystem *lock = *iter;
+ if (lock->GetCurrentCommand() != nullptr) {
+ lock->GetCurrentCommand()->Cancel();
+ Remove(lock->GetCurrentCommand());
+ }
+ lock->SetCurrentCommand(command);
+ }
+ m_adding = false;
+
+ m_commands.insert(command);
+
+ command->StartRunning();
+ m_runningCommandsChanged = true;
+ }
+}
+
+/**
+ * Runs a single iteration of the loop. This method should be called often in
+ * order to have a functioning
+ * {@link Command} system. The loop has five stages:
+ *
+ * <ol>
+ * <li> Poll the Buttons </li>
+ * <li> Execute/Remove the Commands </li>
+ * <li> Send values to SmartDashboard </li>
+ * <li> Add Commands </li>
+ * <li> Add Defaults </li>
+ * </ol>
+ */
+void Scheduler::Run() {
+ // Get button input (going backwards preserves button priority)
+ {
+ if (!m_enabled) return;
+
+ std::lock_guard<priority_mutex> sync(m_buttonsLock);
+ auto rButtonIter = m_buttons.rbegin();
+ for (; rButtonIter != m_buttons.rend(); rButtonIter++) {
+ (*rButtonIter)->Execute();
+ }
+ }
+
+ m_runningCommandsChanged = false;
+
+ // Loop through the commands
+ auto commandIter = m_commands.begin();
+ for (; commandIter != m_commands.end();) {
+ Command *command = *commandIter;
+ // Increment before potentially removing to keep the iterator valid
+ commandIter++;
+ if (!command->Run()) {
+ Remove(command);
+ m_runningCommandsChanged = true;
+ }
+ }
+
+ // Add the new things
+ {
+ std::lock_guard<priority_mutex> sync(m_additionsLock);
+ auto additionsIter = m_additions.begin();
+ for (; additionsIter != m_additions.end(); additionsIter++) {
+ ProcessCommandAddition(*additionsIter);
+ }
+ m_additions.clear();
+ }
+
+ // Add in the defaults
+ auto subsystemIter = m_subsystems.begin();
+ for (; subsystemIter != m_subsystems.end(); subsystemIter++) {
+ Subsystem *lock = *subsystemIter;
+ if (lock->GetCurrentCommand() == nullptr) {
+ ProcessCommandAddition(lock->GetDefaultCommand());
+ }
+ lock->ConfirmCommand();
+ }
+
+ UpdateTable();
+}
+
+/**
+ * Registers a {@link Subsystem} to this {@link Scheduler}, so that the {@link
+ * Scheduler} might know
+ * if a default {@link Command} needs to be run. All {@link Subsystem
+ * Subsystems} should call this.
+ * @param system the system
+ */
+void Scheduler::RegisterSubsystem(Subsystem *subsystem) {
+ if (subsystem == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "subsystem");
+ return;
+ }
+ m_subsystems.insert(subsystem);
+}
+
+/**
+ * Removes the {@link Command} from the {@link Scheduler}.
+ * @param command the command to remove
+ */
+void Scheduler::Remove(Command *command) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+
+ if (!m_commands.erase(command)) return;
+
+ Command::SubsystemSet requirements = command->GetRequirements();
+ auto iter = requirements.begin();
+ for (; iter != requirements.end(); iter++) {
+ Subsystem *lock = *iter;
+ lock->SetCurrentCommand(nullptr);
+ }
+
+ command->Removed();
+}
+
+void Scheduler::RemoveAll() {
+ while (m_commands.size() > 0) {
+ Remove(*m_commands.begin());
+ }
+}
+
+/**
+ * Completely resets the scheduler. Undefined behavior if running.
+ */
+void Scheduler::ResetAll() {
+ RemoveAll();
+ m_subsystems.clear();
+ m_buttons.clear();
+ m_additions.clear();
+ m_commands.clear();
+ m_table = nullptr;
+}
+
+/**
+ * Update the network tables associated with the Scheduler object on the
+ * SmartDashboard
+ */
+void Scheduler::UpdateTable() {
+ CommandSet::iterator commandIter;
+ if (m_table != nullptr) {
+ // Get the list of possible commands to cancel
+ auto new_toCancel = m_table->GetValue("Cancel");
+ if (new_toCancel)
+ toCancel = new_toCancel->GetDoubleArray();
+ else
+ toCancel.resize(0);
+ // m_table->RetrieveValue("Ids", *ids);
+
+ // cancel commands that have had the cancel buttons pressed
+ // on the SmartDashboad
+ if (!toCancel.empty()) {
+ for (commandIter = m_commands.begin(); commandIter != m_commands.end();
+ ++commandIter) {
+ for (unsigned i = 0; i < toCancel.size(); i++) {
+ Command *c = *commandIter;
+ if (c->GetID() == toCancel[i]) {
+ c->Cancel();
+ }
+ }
+ }
+ toCancel.resize(0);
+ m_table->PutValue("Cancel", nt::Value::MakeDoubleArray(toCancel));
+ }
+
+ // Set the running commands
+ if (m_runningCommandsChanged) {
+ commands.resize(0);
+ ids.resize(0);
+ for (commandIter = m_commands.begin(); commandIter != m_commands.end();
+ ++commandIter) {
+ Command *c = *commandIter;
+ commands.push_back(c->GetName());
+ ids.push_back(c->GetID());
+ }
+ m_table->PutValue("Names", nt::Value::MakeStringArray(commands));
+ m_table->PutValue("Ids", nt::Value::MakeDoubleArray(ids));
+ }
+ }
+}
+
+std::string Scheduler::GetName() const { return "Scheduler"; }
+
+std::string Scheduler::GetType() const { return "Scheduler"; }
+
+std::string Scheduler::GetSmartDashboardType() const { return "Scheduler"; }
+
+void Scheduler::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+
+ m_table->PutValue("Names", nt::Value::MakeStringArray(commands));
+ m_table->PutValue("Ids", nt::Value::MakeDoubleArray(ids));
+ m_table->PutValue("Cancel", nt::Value::MakeDoubleArray(toCancel));
+}
+
+std::shared_ptr<ITable> Scheduler::GetTable() const { return m_table; }
diff --git a/wpilibc/shared/src/Commands/StartCommand.cpp b/wpilibc/shared/src/Commands/StartCommand.cpp
new file mode 100644
index 0000000..60db054
--- /dev/null
+++ b/wpilibc/shared/src/Commands/StartCommand.cpp
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Commands/StartCommand.h"
+
+StartCommand::StartCommand(Command *commandToStart) : Command("StartCommand") {
+ m_commandToFork = commandToStart;
+}
+
+void StartCommand::Initialize() { m_commandToFork->Start(); }
+
+void StartCommand::Execute() {}
+
+void StartCommand::End() {}
+
+void StartCommand::Interrupted() {}
+
+bool StartCommand::IsFinished() { return true; }
diff --git a/wpilibc/shared/src/Commands/Subsystem.cpp b/wpilibc/shared/src/Commands/Subsystem.cpp
new file mode 100644
index 0000000..0b4b92f
--- /dev/null
+++ b/wpilibc/shared/src/Commands/Subsystem.cpp
@@ -0,0 +1,147 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Commands/Subsystem.h"
+
+#include "Commands/Command.h"
+#include "Commands/Scheduler.h"
+#include "WPIErrors.h"
+
+/**
+ * Creates a subsystem with the given name
+ * @param name the name of the subsystem
+ */
+Subsystem::Subsystem(const std::string &name) {
+ m_name = name;
+ Scheduler::GetInstance()->RegisterSubsystem(this);
+}
+/**
+ * Initialize the default command for this subsystem
+ * This is meant to be the place to call SetDefaultCommand in a subsystem and
+ * will be called
+ * on all the subsystems by the CommandBase method before the program starts
+ * running by using
+ * the list of all registered Subsystems inside the Scheduler.
+ *
+ * This should be overridden by a Subsystem that has a default Command
+ */
+void Subsystem::InitDefaultCommand() {}
+
+/**
+ * Sets the default command. If this is not called or is called with null,
+ * then there will be no default command for the subsystem.
+ *
+ * <p><b>WARNING:</b> This should <b>NOT</b> be called in a constructor if the
+ * subsystem is a
+ * singleton.</p>
+ *
+ * @param command the default command (or null if there should be none)
+ */
+void Subsystem::SetDefaultCommand(Command *command) {
+ if (command == nullptr) {
+ m_defaultCommand = nullptr;
+ } else {
+ bool found = false;
+ Command::SubsystemSet requirements = command->GetRequirements();
+ auto iter = requirements.begin();
+ for (; iter != requirements.end(); iter++) {
+ if (*iter == this) {
+ found = true;
+ break;
+ }
+ }
+
+ if (!found) {
+ wpi_setWPIErrorWithContext(
+ CommandIllegalUse, "A default command must require the subsystem");
+ return;
+ }
+
+ m_defaultCommand = command;
+ }
+ if (m_table != nullptr) {
+ if (m_defaultCommand != nullptr) {
+ m_table->PutBoolean("hasDefault", true);
+ m_table->PutString("default", m_defaultCommand->GetName());
+ } else {
+ m_table->PutBoolean("hasDefault", false);
+ }
+ }
+}
+
+/**
+ * Returns the default command (or null if there is none).
+ * @return the default command
+ */
+Command *Subsystem::GetDefaultCommand() {
+ if (!m_initializedDefaultCommand) {
+ m_initializedDefaultCommand = true;
+ InitDefaultCommand();
+ }
+ return m_defaultCommand;
+}
+
+/**
+ * Sets the current command
+ * @param command the new current command
+ */
+void Subsystem::SetCurrentCommand(Command *command) {
+ m_currentCommand = command;
+ m_currentCommandChanged = true;
+}
+
+/**
+ * Returns the command which currently claims this subsystem.
+ * @return the command which currently claims this subsystem
+ */
+Command *Subsystem::GetCurrentCommand() const { return m_currentCommand; }
+
+/**
+ * Call this to alert Subsystem that the current command is actually the
+ * command.
+ * Sometimes, the {@link Subsystem} is told that it has no command while the
+ * {@link Scheduler}
+ * is going through the loop, only to be soon after given a new one. This will
+ * avoid that situation.
+ */
+void Subsystem::ConfirmCommand() {
+ if (m_currentCommandChanged) {
+ if (m_table != nullptr) {
+ if (m_currentCommand != nullptr) {
+ m_table->PutBoolean("hasCommand", true);
+ m_table->PutString("command", m_currentCommand->GetName());
+ } else {
+ m_table->PutBoolean("hasCommand", false);
+ }
+ }
+ m_currentCommandChanged = false;
+ }
+}
+
+std::string Subsystem::GetName() const { return m_name; }
+
+std::string Subsystem::GetSmartDashboardType() const { return "Subsystem"; }
+
+void Subsystem::InitTable(std::shared_ptr<ITable> table) {
+ m_table = table;
+ if (m_table != nullptr) {
+ if (m_defaultCommand != nullptr) {
+ m_table->PutBoolean("hasDefault", true);
+ m_table->PutString("default", m_defaultCommand->GetName());
+ } else {
+ m_table->PutBoolean("hasDefault", false);
+ }
+ if (m_currentCommand != nullptr) {
+ m_table->PutBoolean("hasCommand", true);
+ m_table->PutString("command", m_currentCommand->GetName());
+ } else {
+ m_table->PutBoolean("hasCommand", false);
+ }
+ }
+}
+
+std::shared_ptr<ITable> Subsystem::GetTable() const { return m_table; }
diff --git a/wpilibc/shared/src/Commands/WaitCommand.cpp b/wpilibc/shared/src/Commands/WaitCommand.cpp
new file mode 100644
index 0000000..f4e4c00
--- /dev/null
+++ b/wpilibc/shared/src/Commands/WaitCommand.cpp
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Commands/WaitCommand.h"
+#include <sstream>
+
+WaitCommand::WaitCommand(double timeout)
+ : Command(
+ ((std::stringstream &)(std::stringstream("Wait(") << timeout << ")"))
+ .str()
+ .c_str(),
+ timeout) {}
+
+WaitCommand::WaitCommand(const std::string &name, double timeout)
+ : Command(name, timeout) {}
+
+void WaitCommand::Initialize() {}
+
+void WaitCommand::Execute() {}
+
+bool WaitCommand::IsFinished() { return IsTimedOut(); }
+
+void WaitCommand::End() {}
+
+void WaitCommand::Interrupted() {}
diff --git a/wpilibc/shared/src/Commands/WaitForChildren.cpp b/wpilibc/shared/src/Commands/WaitForChildren.cpp
new file mode 100644
index 0000000..a3a129d
--- /dev/null
+++ b/wpilibc/shared/src/Commands/WaitForChildren.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Commands/WaitForChildren.h"
+#include "Commands/CommandGroup.h"
+
+WaitForChildren::WaitForChildren(double timeout)
+ : Command("WaitForChildren", timeout) {}
+
+WaitForChildren::WaitForChildren(const std::string &name, double timeout)
+ : Command(name, timeout) {}
+
+void WaitForChildren::Initialize() {}
+
+void WaitForChildren::Execute() {}
+
+void WaitForChildren::End() {}
+
+void WaitForChildren::Interrupted() {}
+
+bool WaitForChildren::IsFinished() {
+ return GetGroup() == nullptr || GetGroup()->GetSize() == 0;
+}
diff --git a/wpilibc/shared/src/Commands/WaitUntilCommand.cpp b/wpilibc/shared/src/Commands/WaitUntilCommand.cpp
new file mode 100644
index 0000000..ea0e664
--- /dev/null
+++ b/wpilibc/shared/src/Commands/WaitUntilCommand.cpp
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "Commands/WaitUntilCommand.h"
+#include "Timer.h"
+
+/**
+ * A {@link WaitCommand} will wait until a certain match time before finishing.
+ * This will wait until the game clock reaches some value, then continue to the
+ * next command.
+ * @see CommandGroup
+ */
+WaitUntilCommand::WaitUntilCommand(double time)
+ : Command("WaitUntilCommand", time) {
+ m_time = time;
+}
+
+WaitUntilCommand::WaitUntilCommand(const std::string &name, double time)
+ : Command(name, time) {
+ m_time = time;
+}
+
+void WaitUntilCommand::Initialize() {}
+
+void WaitUntilCommand::Execute() {}
+
+/**
+ * Check if we've reached the actual finish time.
+ */
+bool WaitUntilCommand::IsFinished() { return Timer::GetMatchTime() >= m_time; }
+
+void WaitUntilCommand::End() {}
+
+void WaitUntilCommand::Interrupted() {}
diff --git a/wpilibc/shared/src/Error.cpp b/wpilibc/shared/src/Error.cpp
new file mode 100644
index 0000000..1206361
--- /dev/null
+++ b/wpilibc/shared/src/Error.cpp
@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "Error.h"
+
+#include <iostream>
+#include <sstream>
+#include <cstring>
+#include <cstdlib>
+#include <stdint.h>
+
+#include "DriverStation.h"
+#include "Timer.h"
+#include "Utility.h"
+
+void Error::Clone(const Error& error) {
+ m_code = error.m_code;
+ m_message = error.m_message;
+ m_filename = error.m_filename;
+ m_function = error.m_function;
+ m_lineNumber = error.m_lineNumber;
+ m_originatingObject = error.m_originatingObject;
+ m_timestamp = error.m_timestamp;
+}
+
+Error::Code Error::GetCode() const { return m_code; }
+
+std::string Error::GetMessage() const { return m_message; }
+
+std::string Error::GetFilename() const { return m_filename; }
+
+std::string Error::GetFunction() const { return m_function; }
+
+uint32_t Error::GetLineNumber() const { return m_lineNumber; }
+
+const ErrorBase* Error::GetOriginatingObject() const {
+ return m_originatingObject;
+}
+
+double Error::GetTimestamp() const { return m_timestamp; }
+
+void Error::Set(Code code, llvm::StringRef contextMessage,
+ llvm::StringRef filename, llvm::StringRef function,
+ uint32_t lineNumber, const ErrorBase* originatingObject) {
+ bool report = true;
+
+ if (code == m_code && GetTime() - m_timestamp < 1) {
+ report = false;
+ }
+
+ m_code = code;
+ m_message = contextMessage;
+ m_filename = filename;
+ m_function = function;
+ m_lineNumber = lineNumber;
+ m_originatingObject = originatingObject;
+
+ if (report) {
+ m_timestamp = GetTime();
+ Report();
+ }
+}
+
+void Error::Report() {
+ std::stringstream errorStream;
+
+ errorStream << "Error on line " << m_lineNumber << " ";
+#if defined(_UNIX)
+ errorStream << "of " << basename(m_filename.c_str()) << ": ";
+#elif defined(_WIN32)
+ const int MAX_DIR = 100;
+ char basename[MAX_DIR];
+ _splitpath_s(m_filename.c_str(), NULL, 0, basename, MAX_DIR, NULL, 0, NULL, 0);
+ errorStream << "of " << basename << ": ";
+#endif
+
+ errorStream << m_message << std::endl;
+ errorStream << GetStackTrace(4);
+
+ std::string error = errorStream.str();
+
+ DriverStation::ReportError(error);
+}
+
+void Error::Clear() {
+ m_code = 0;
+ m_message = "";
+ m_filename = "";
+ m_function = "";
+ m_lineNumber = 0;
+ m_originatingObject = nullptr;
+ m_timestamp = 0.0;
+}
diff --git a/wpilibc/shared/src/ErrorBase.cpp b/wpilibc/shared/src/ErrorBase.cpp
new file mode 100644
index 0000000..329c983
--- /dev/null
+++ b/wpilibc/shared/src/ErrorBase.cpp
@@ -0,0 +1,191 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "ErrorBase.h"
+#define WPI_ERRORS_DEFINE_STRINGS
+#include "WPIErrors.h"
+
+#include <errno.h>
+#include <cstring>
+#include <cstdio>
+#include <sstream>
+
+priority_mutex ErrorBase::_globalErrorMutex;
+Error ErrorBase::_globalError;
+
+/**
+ * @brief Retrieve the current error.
+ * Get the current error information associated with this sensor.
+ */
+Error& ErrorBase::GetError() { return m_error; }
+
+const Error& ErrorBase::GetError() const { return m_error; }
+
+/**
+ * @brief Clear the current error information associated with this sensor.
+ */
+void ErrorBase::ClearError() const { m_error.Clear(); }
+
+/**
+ * @brief Set error information associated with a C library call that set an
+ * error to the "errno" global variable.
+ *
+ * @param contextMessage A custom message from the code that set the error.
+ * @param filename Filename of the error source
+ * @param function Function of the error source
+ * @param lineNumber Line number of the error source
+ */
+void ErrorBase::SetErrnoError(llvm::StringRef contextMessage,
+ llvm::StringRef filename,
+ llvm::StringRef function,
+ uint32_t lineNumber) const {
+ std::string err;
+ int errNo = errno;
+ if (errNo == 0) {
+ err = "OK: ";
+ err += contextMessage;
+ } else {
+ char buf[256];
+ snprintf(buf, 256, "%s (0x%08X): %.*s", strerror(errNo), errNo,
+ contextMessage.size(), contextMessage.data());
+ err = buf;
+ }
+
+ // Set the current error information for this object.
+ m_error.Set(-1, err, filename, function, lineNumber, this);
+
+ // Update the global error if there is not one already set.
+ std::lock_guard<priority_mutex> mutex(_globalErrorMutex);
+ if (_globalError.GetCode() == 0) {
+ _globalError.Clone(m_error);
+ }
+}
+
+/**
+ * @brief Set the current error information associated from the nivision Imaq
+ * API.
+ *
+ * @param success The return from the function
+ * @param contextMessage A custom message from the code that set the error.
+ * @param filename Filename of the error source
+ * @param function Function of the error source
+ * @param lineNumber Line number of the error source
+ */
+void ErrorBase::SetImaqError(int success, llvm::StringRef contextMessage,
+ llvm::StringRef filename, llvm::StringRef function,
+ uint32_t lineNumber) const {
+ // If there was an error
+ if (success <= 0) {
+ std::stringstream err;
+ err << success << ": " << contextMessage;
+
+ // Set the current error information for this object.
+ m_error.Set(success, err.str(), filename, function, lineNumber, this);
+
+ // Update the global error if there is not one already set.
+ std::lock_guard<priority_mutex> mutex(_globalErrorMutex);
+ if (_globalError.GetCode() == 0) {
+ _globalError.Clone(m_error);
+ }
+ }
+}
+
+/**
+ * @brief Set the current error information associated with this sensor.
+ *
+ * @param code The error code
+ * @param contextMessage A custom message from the code that set the error.
+ * @param filename Filename of the error source
+ * @param function Function of the error source
+ * @param lineNumber Line number of the error source
+ */
+void ErrorBase::SetError(Error::Code code, llvm::StringRef contextMessage,
+ llvm::StringRef filename, llvm::StringRef function,
+ uint32_t lineNumber) const {
+ // If there was an error
+ if (code != 0) {
+ // Set the current error information for this object.
+ m_error.Set(code, contextMessage, filename, function, lineNumber, this);
+
+ // Update the global error if there is not one already set.
+ std::lock_guard<priority_mutex> mutex(_globalErrorMutex);
+ if (_globalError.GetCode() == 0) {
+ _globalError.Clone(m_error);
+ }
+ }
+}
+
+/**
+ * @brief Set the current error information associated with this sensor.
+ *
+ * @param errorMessage The error message from WPIErrors.h
+ * @param contextMessage A custom message from the code that set the error.
+ * @param filename Filename of the error source
+ * @param function Function of the error source
+ * @param lineNumber Line number of the error source
+ */
+void ErrorBase::SetWPIError(llvm::StringRef errorMessage, Error::Code code,
+ llvm::StringRef contextMessage,
+ llvm::StringRef filename, llvm::StringRef function,
+ uint32_t lineNumber) const {
+ std::string err = errorMessage.str() + ": " + contextMessage.str();
+
+ // Set the current error information for this object.
+ m_error.Set(code, err, filename, function, lineNumber, this);
+
+ // Update the global error if there is not one already set.
+ std::lock_guard<priority_mutex> mutex(_globalErrorMutex);
+ if (_globalError.GetCode() == 0) {
+ _globalError.Clone(m_error);
+ }
+}
+
+void ErrorBase::CloneError(const ErrorBase& rhs) const {
+ m_error.Clone(rhs.GetError());
+}
+
+/**
+@brief Check if the current error code represents a fatal error.
+
+@return true if the current error is fatal.
+*/
+bool ErrorBase::StatusIsFatal() const { return m_error.GetCode() < 0; }
+
+void ErrorBase::SetGlobalError(Error::Code code, llvm::StringRef contextMessage,
+ llvm::StringRef filename,
+ llvm::StringRef function, uint32_t lineNumber) {
+ // If there was an error
+ if (code != 0) {
+ std::lock_guard<priority_mutex> mutex(_globalErrorMutex);
+
+ // Set the current error information for this object.
+ _globalError.Set(code, contextMessage, filename, function, lineNumber,
+ nullptr);
+ }
+}
+
+void ErrorBase::SetGlobalWPIError(llvm::StringRef errorMessage,
+ llvm::StringRef contextMessage,
+ llvm::StringRef filename,
+ llvm::StringRef function,
+ uint32_t lineNumber) {
+ std::string err = errorMessage.str() + ": " + contextMessage.str();
+
+ std::lock_guard<priority_mutex> mutex(_globalErrorMutex);
+ if (_globalError.GetCode() != 0) {
+ _globalError.Clear();
+ }
+ _globalError.Set(-1, err, filename, function, lineNumber, nullptr);
+}
+
+/**
+ * Retrieve the current global error.
+*/
+Error& ErrorBase::GetGlobalError() {
+ std::lock_guard<priority_mutex> mutex(_globalErrorMutex);
+ return _globalError;
+}
diff --git a/wpilibc/shared/src/HLUsageReporting.cpp b/wpilibc/shared/src/HLUsageReporting.cpp
new file mode 100644
index 0000000..bcbc6b6
--- /dev/null
+++ b/wpilibc/shared/src/HLUsageReporting.cpp
@@ -0,0 +1,20 @@
+
+#include "HLUsageReporting.h"
+
+HLUsageReportingInterface* HLUsageReporting::impl = nullptr;
+
+void HLUsageReporting::SetImplementation(HLUsageReportingInterface* i) {
+ impl = i;
+}
+
+void HLUsageReporting::ReportScheduler() {
+ if (impl != nullptr) {
+ impl->ReportScheduler();
+ }
+}
+
+void HLUsageReporting::ReportSmartDashboard() {
+ if (impl != nullptr) {
+ impl->ReportSmartDashboard();
+ }
+}
diff --git a/wpilibc/shared/src/LiveWindow/LiveWindow.cpp b/wpilibc/shared/src/LiveWindow/LiveWindow.cpp
new file mode 100644
index 0000000..793ddcc
--- /dev/null
+++ b/wpilibc/shared/src/LiveWindow/LiveWindow.cpp
@@ -0,0 +1,215 @@
+#include "LiveWindow/LiveWindow.h"
+#include "networktables/NetworkTable.h"
+#include <algorithm>
+#include <sstream>
+
+/**
+ * Get an instance of the LiveWindow main class
+ * This is a singleton to guarantee that there is only a single instance
+ * regardless of
+ * how many times GetInstance is called.
+ */
+LiveWindow *LiveWindow::GetInstance() {
+ static LiveWindow instance;
+ return &instance;
+}
+
+/**
+ * LiveWindow constructor.
+ * Allocate the necessary tables.
+ */
+LiveWindow::LiveWindow() : m_scheduler(Scheduler::GetInstance()) {
+ m_liveWindowTable = NetworkTable::GetTable("LiveWindow");
+ m_statusTable = m_liveWindowTable->GetSubTable("~STATUS~");
+}
+
+/**
+ * Change the enabled status of LiveWindow
+ * If it changes to enabled, start livewindow running otherwise stop it
+ */
+void LiveWindow::SetEnabled(bool enabled) {
+ if (m_enabled == enabled) return;
+ if (enabled) {
+ if (m_firstTime) {
+ InitializeLiveWindowComponents();
+ m_firstTime = false;
+ }
+ m_scheduler->SetEnabled(false);
+ m_scheduler->RemoveAll();
+ for (auto& elem : m_components) {
+ elem.first->StartLiveWindowMode();
+ }
+ } else {
+ for (auto& elem : m_components) {
+ elem.first->StopLiveWindowMode();
+ }
+ m_scheduler->SetEnabled(true);
+ }
+ m_enabled = enabled;
+ m_statusTable->PutBoolean("LW Enabled", m_enabled);
+}
+
+/**
+ * @name AddSensor(subsystem, name, component)
+ * Add a Sensor associated with the subsystem and call it by the given name.
+ * @param subsystem The subsystem this component is part of.
+ * @param name The name of this component.
+ * @param component A LiveWindowSendable component that represents a sensor.
+ */
+//@{
+/**
+ * @brief Use a STL smart pointer to share ownership of component.
+ */
+void LiveWindow::AddSensor(const std::string &subsystem, const std::string &name,
+ std::shared_ptr<LiveWindowSendable> component) {
+ m_components[component].subsystem = subsystem;
+ m_components[component].name = name;
+ m_components[component].isSensor = true;
+}
+
+/**
+ * @brief Pass a reference to LiveWindow and retain ownership of the component.
+ */
+void LiveWindow::AddSensor(const std::string &subsystem,
+ const std::string &name,
+ LiveWindowSendable &component) {
+ AddSensor(subsystem, name, std::shared_ptr<LiveWindowSendable>(
+ &component, [](LiveWindowSendable*){}));
+}
+
+/**
+ * @brief Use a raw pointer to the LiveWindow.
+ * @deprecated Prefer smart pointers or references.
+ */
+void LiveWindow::AddSensor(const std::string &subsystem,
+ const std::string &name,
+ LiveWindowSendable *component) {
+ AddSensor(subsystem, name, std::shared_ptr<LiveWindowSendable>(
+ component, NullDeleter<LiveWindowSendable>()));
+}
+//@}
+
+/**
+ * @name AddActuator(subsystem, name, component)
+ * Add an Actuator associated with the subsystem and call it by the given name.
+ * @param subsystem The subsystem this component is part of.
+ * @param name The name of this component.
+ * @param component A LiveWindowSendable component that represents a actuator.
+ */
+//@{
+/**
+ * @brief Use a STL smart pointer to share ownership of component.
+ */
+void LiveWindow::AddActuator(const std::string &subsystem, const std::string &name,
+ std::shared_ptr<LiveWindowSendable> component) {
+ m_components[component].subsystem = subsystem;
+ m_components[component].name = name;
+ m_components[component].isSensor = false;
+}
+
+/**
+ * @brief Pass a reference to LiveWindow and retain ownership of the component.
+ */
+void LiveWindow::AddActuator(const std::string &subsystem,
+ const std::string &name,
+ LiveWindowSendable &component) {
+ AddActuator(subsystem, name, std::shared_ptr<LiveWindowSendable>(
+ &component, [](LiveWindowSendable*){}));
+}
+
+/**
+ * @brief Use a raw pointer to the LiveWindow.
+ * @deprecated Prefer smart pointers or references.
+ */
+void LiveWindow::AddActuator(const std::string &subsystem, const std::string &name,
+ LiveWindowSendable *component) {
+ AddActuator(subsystem, name,
+ std::shared_ptr<LiveWindowSendable>(
+ component, NullDeleter<LiveWindowSendable>()));
+}
+//@}
+
+/**
+ * Meant for internal use in other WPILib classes.
+ */
+void LiveWindow::AddSensor(std::string type, int channel,
+ LiveWindowSendable *component) {
+ std::ostringstream oss;
+ oss << type << "[" << channel << "]";
+ AddSensor("Ungrouped", oss.str(), component);
+ std::shared_ptr<LiveWindowSendable> component_stl(
+ component, NullDeleter<LiveWindowSendable>());
+ if (std::find(m_sensors.begin(), m_sensors.end(), component_stl) ==
+ m_sensors.end())
+ m_sensors.push_back(component_stl);
+}
+
+/**
+ * Meant for internal use in other WPILib classes.
+ */
+void LiveWindow::AddActuator(std::string type, int channel,
+ LiveWindowSendable *component) {
+ std::ostringstream oss;
+ oss << type << "[" << channel << "]";
+ AddActuator("Ungrouped", oss.str(), std::shared_ptr<LiveWindowSendable>(
+ component, [](LiveWindowSendable *) {}));
+}
+
+/**
+ * Meant for internal use in other WPILib classes.
+ */
+void LiveWindow::AddActuator(std::string type, int module, int channel,
+ LiveWindowSendable *component) {
+ std::ostringstream oss;
+ oss << type << "[" << module << "," << channel << "]";
+ AddActuator("Ungrouped", oss.str(), std::shared_ptr<LiveWindowSendable>(
+ component, [](LiveWindowSendable *) {}));
+}
+
+/**
+ * Tell all the sensors to update (send) their values
+ * Actuators are handled through callbacks on their value changing from the
+ * SmartDashboard widgets.
+ */
+void LiveWindow::UpdateValues() {
+ for (auto& elem : m_sensors) {
+ elem->UpdateTable();
+ }
+}
+
+/**
+ * This method is called periodically to cause the sensors to send new values
+ * to the SmartDashboard.
+ */
+void LiveWindow::Run() {
+ if (m_enabled) {
+ UpdateValues();
+ }
+}
+
+/**
+ * Initialize all the LiveWindow elements the first time we enter LiveWindow
+ * mode. By holding off creating the NetworkTable entries, it allows them to be
+ * redefined before the first time in LiveWindow mode. This allows default
+ * sensor and actuator values to be created that are replaced with the custom
+ * names from users calling addActuator and addSensor.
+ */
+void LiveWindow::InitializeLiveWindowComponents() {
+ for (auto& elem : m_components) {
+ std::shared_ptr<LiveWindowSendable> component = elem.first;
+ LiveWindowComponent c = elem.second;
+ std::string subsystem = c.subsystem;
+ std::string name = c.name;
+ m_liveWindowTable->GetSubTable(subsystem)
+ ->PutString("~TYPE~", "LW Subsystem");
+ std::shared_ptr<ITable> table(
+ m_liveWindowTable->GetSubTable(subsystem)->GetSubTable(name));
+ table->PutString("~TYPE~", component->GetSmartDashboardType());
+ table->PutString("Name", name);
+ table->PutString("Subsystem", subsystem);
+ component->InitTable(table);
+ if (c.isSensor) {
+ m_sensors.push_back(component);
+ }
+ }
+}
diff --git a/wpilibc/shared/src/LiveWindow/LiveWindowStatusListener.cpp b/wpilibc/shared/src/LiveWindow/LiveWindowStatusListener.cpp
new file mode 100644
index 0000000..021798d
--- /dev/null
+++ b/wpilibc/shared/src/LiveWindow/LiveWindowStatusListener.cpp
@@ -0,0 +1,6 @@
+#include "LiveWindow/LiveWindowStatusListener.h"
+#include "Commands/Scheduler.h"
+
+void LiveWindowStatusListener::ValueChanged(ITable *source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value,
+ bool isNew) {}
diff --git a/wpilibc/shared/src/PIDSource.cpp b/wpilibc/shared/src/PIDSource.cpp
new file mode 100644
index 0000000..b6b5200
--- /dev/null
+++ b/wpilibc/shared/src/PIDSource.cpp
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "PIDSource.h"
+
+/**
+ * Set which parameter you are using as a process control variable.
+ *
+ * @param pidSource An enum to select the parameter.
+ */
+void PIDSource::SetPIDSourceType(PIDSourceType pidSource) {
+ m_pidSource = pidSource;
+}
+
+PIDSourceType PIDSource::GetPIDSourceType() const {
+ return m_pidSource;
+}
diff --git a/wpilibc/shared/src/Resource.cpp b/wpilibc/shared/src/Resource.cpp
new file mode 100644
index 0000000..7f9e476
--- /dev/null
+++ b/wpilibc/shared/src/Resource.cpp
@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "Resource.h"
+#include "WPIErrors.h"
+#include "ErrorBase.h"
+
+priority_recursive_mutex Resource::m_createLock;
+
+/**
+ * Allocate storage for a new instance of Resource.
+ * Allocate a bool array of values that will get initialized to indicate that no
+ * resources
+ * have been allocated yet. The indicies of the resources are [0 .. elements -
+ * 1].
+ */
+Resource::Resource(uint32_t elements) {
+ std::lock_guard<priority_recursive_mutex> sync(m_createLock);
+ m_isAllocated = std::vector<bool>(elements, false);
+}
+
+/**
+ * Factory method to create a Resource allocation-tracker *if* needed.
+ *
+ * @param r -- address of the caller's Resource pointer. If *r == nullptr, this
+ * will construct a Resource and make *r point to it. If *r != nullptr, i.e.
+ * the caller already has a Resource instance, this won't do anything.
+ * @param elements -- the number of elements for this Resource allocator to
+ * track, that is, it will allocate resource numbers in the range
+ * [0 .. elements - 1].
+ */
+/*static*/ void Resource::CreateResourceObject(std::unique_ptr<Resource>& r,
+ uint32_t elements) {
+ std::lock_guard<priority_recursive_mutex> sync(m_createLock);
+ if (!r) {
+ r = std::make_unique<Resource>(elements);
+ }
+}
+
+/**
+ * Allocate a resource.
+ * When a resource is requested, mark it allocated. In this case, a free
+ * resource value
+ * within the range is located and returned after it is marked allocated.
+ */
+uint32_t Resource::Allocate(const std::string &resourceDesc) {
+ std::lock_guard<priority_recursive_mutex> sync(m_allocateLock);
+ for (uint32_t i = 0; i < m_isAllocated.size(); i++) {
+ if (!m_isAllocated[i]) {
+ m_isAllocated[i] = true;
+ return i;
+ }
+ }
+ wpi_setWPIErrorWithContext(NoAvailableResources, resourceDesc);
+ return std::numeric_limits<uint32_t>::max();
+}
+
+/**
+ * Allocate a specific resource value.
+ * The user requests a specific resource value, i.e. channel number and it is
+ * verified
+ * unallocated, then returned.
+ */
+uint32_t Resource::Allocate(uint32_t index, const std::string &resourceDesc) {
+ std::lock_guard<priority_recursive_mutex> sync(m_allocateLock);
+ if (index >= m_isAllocated.size()) {
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, resourceDesc);
+ return std::numeric_limits<uint32_t>::max();
+ }
+ if (m_isAllocated[index]) {
+ wpi_setWPIErrorWithContext(ResourceAlreadyAllocated, resourceDesc);
+ return std::numeric_limits<uint32_t>::max();
+ }
+ m_isAllocated[index] = true;
+ return index;
+}
+
+/**
+ * Free an allocated resource.
+ * After a resource is no longer needed, for example a destructor is called for
+ * a channel assignment
+ * class, Free will release the resource value so it can be reused somewhere
+ * else in the program.
+ */
+void Resource::Free(uint32_t index) {
+ std::unique_lock<priority_recursive_mutex> sync(m_allocateLock);
+ if (index == std::numeric_limits<uint32_t>::max()) return;
+ if (index >= m_isAllocated.size()) {
+ wpi_setWPIError(NotAllocated);
+ return;
+ }
+ if (!m_isAllocated[index]) {
+ wpi_setWPIError(NotAllocated);
+ return;
+ }
+ m_isAllocated[index] = false;
+}
diff --git a/wpilibc/shared/src/RobotState.cpp b/wpilibc/shared/src/RobotState.cpp
new file mode 100644
index 0000000..cb28208
--- /dev/null
+++ b/wpilibc/shared/src/RobotState.cpp
@@ -0,0 +1,50 @@
+#include "RobotState.h"
+
+#include "Base.h"
+
+std::shared_ptr<RobotStateInterface> RobotState::impl;
+
+void RobotState::SetImplementation(RobotStateInterface& i) {
+ impl = std::shared_ptr<RobotStateInterface>(
+ &i, NullDeleter<RobotStateInterface>());
+}
+
+void RobotState::SetImplementation(
+ std::shared_ptr<RobotStateInterface> i) {
+ impl = i;
+}
+
+bool RobotState::IsDisabled() {
+ if (impl != nullptr) {
+ return impl->IsDisabled();
+ }
+ return true;
+}
+
+bool RobotState::IsEnabled() {
+ if (impl != nullptr) {
+ return impl->IsEnabled();
+ }
+ return false;
+}
+
+bool RobotState::IsOperatorControl() {
+ if (impl != nullptr) {
+ return impl->IsOperatorControl();
+ }
+ return true;
+}
+
+bool RobotState::IsAutonomous() {
+ if (impl != nullptr) {
+ return impl->IsAutonomous();
+ }
+ return false;
+}
+
+bool RobotState::IsTest() {
+ if (impl != nullptr) {
+ return impl->IsTest();
+ }
+ return false;
+}
diff --git a/wpilibc/shared/src/SmartDashboard/SendableChooser.cpp b/wpilibc/shared/src/SmartDashboard/SendableChooser.cpp
new file mode 100644
index 0000000..255cc3a
--- /dev/null
+++ b/wpilibc/shared/src/SmartDashboard/SendableChooser.cpp
@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "SmartDashboard/SendableChooser.h"
+
+#include <stdio.h>
+
+static const std::string kDefault = "default";
+static const std::string kOptions = "options";
+static const std::string kSelected = "selected";
+
+/**
+ * Adds the given object to the list of options. On the {@link SmartDashboard}
+ * on the desktop,
+ * the object will appear as the given name.
+ * @param name the name of the option
+ * @param object the option
+ */
+void SendableChooser::AddObject(const std::string &name, void *object) {
+ m_choices[name] = object;
+}
+
+/**
+ * Add the given object to the list of options and marks it as the default.
+ * Functionally, this is very close to {@link SendableChooser#AddObject(const
+ * char *name, void *object) AddObject(...)}
+ * except that it will use this as the default option if none other is
+ * explicitly selected.
+ * @param name the name of the option
+ * @param object the option
+ */
+void SendableChooser::AddDefault(const std::string &name, void *object) {
+ m_defaultChoice = name;
+ AddObject(name, object);
+}
+
+/**
+ * Returns the selected option. If there is none selected, it will return the
+ * default. If there is none selected
+ * and no default, then it will return {@code nullptr}.
+ * @return the option selected
+ */
+void *SendableChooser::GetSelected() {
+ std::string selected = m_table->GetString(kSelected, m_defaultChoice);
+ if (selected == "")
+ return nullptr;
+ else
+ return m_choices[selected];
+}
+
+void SendableChooser::InitTable(std::shared_ptr<ITable> subtable) {
+ std::vector<std::string> keys;
+ m_table = subtable;
+ if (m_table != nullptr) {
+ std::map<std::string, void *>::iterator iter;
+ for (iter = m_choices.begin(); iter != m_choices.end(); iter++) {
+ keys.push_back(iter->first);
+ }
+ m_table->PutValue(kOptions, nt::Value::MakeStringArray(std::move(keys)));
+ m_table->PutString(kDefault, m_defaultChoice);
+ }
+}
+
+std::shared_ptr<ITable> SendableChooser::GetTable() const { return m_table; }
+
+std::string SendableChooser::GetSmartDashboardType() const {
+ return "String Chooser";
+}
diff --git a/wpilibc/shared/src/SmartDashboard/SmartDashboard.cpp b/wpilibc/shared/src/SmartDashboard/SmartDashboard.cpp
new file mode 100644
index 0000000..bee0c03
--- /dev/null
+++ b/wpilibc/shared/src/SmartDashboard/SmartDashboard.cpp
@@ -0,0 +1,165 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 "SmartDashboard/SmartDashboard.h"
+
+//#include "NetworkCommunication/UsageReporting.h"
+#include "SmartDashboard/NamedSendable.h"
+#include "WPIErrors.h"
+#include "networktables/NetworkTable.h"
+#include "HLUsageReporting.h"
+
+std::shared_ptr<ITable> SmartDashboard::m_table;
+std::map<std::shared_ptr<ITable> , Sendable *> SmartDashboard::m_tablesToData;
+
+void SmartDashboard::init() {
+ m_table = NetworkTable::GetTable("SmartDashboard");
+
+ HLUsageReporting::ReportSmartDashboard();
+}
+
+/**
+ * Maps the specified key to the specified value in this table.
+ * The key can not be nullptr.
+ * The value can be retrieved by calling the get method with a key that is equal
+ * to the original key.
+ * @param keyName the key
+ * @param value the value
+ */
+void SmartDashboard::PutData(llvm::StringRef key, Sendable *data) {
+ if (data == nullptr) {
+ wpi_setGlobalWPIErrorWithContext(NullParameter, "value");
+ return;
+ }
+ std::shared_ptr<ITable> dataTable(m_table->GetSubTable(key));
+ dataTable->PutString("~TYPE~", data->GetSmartDashboardType());
+ data->InitTable(dataTable);
+ m_tablesToData[dataTable] = data;
+}
+
+/**
+ * Maps the specified key (where the key is the name of the {@link
+ * SmartDashboardNamedData}
+ * to the specified value in this table.
+ * The value can be retrieved by calling the get method with a key that is equal
+ * to the original key.
+ * @param value the value
+ */
+void SmartDashboard::PutData(NamedSendable *value) {
+ if (value == nullptr) {
+ wpi_setGlobalWPIErrorWithContext(NullParameter, "value");
+ return;
+ }
+ PutData(value->GetName(), value);
+}
+
+/**
+ * Returns the value at the specified key.
+ * @param keyName the key
+ * @return the value
+ */
+Sendable *SmartDashboard::GetData(llvm::StringRef key) {
+ std::shared_ptr<ITable> subtable(m_table->GetSubTable(key));
+ Sendable *data = m_tablesToData[subtable];
+ if (data == nullptr) {
+ wpi_setGlobalWPIErrorWithContext(SmartDashboardMissingKey, key);
+ return nullptr;
+ }
+ return data;
+}
+
+/**
+ * Maps the specified key to the specified complex value (such as an array) in
+ * this table.
+ * The key can not be nullptr.
+ * The value can be retrieved by calling the RetrieveValue method with a key
+ * that is equal to the original key.
+ * @param keyName the key
+ * @param value the value
+ */
+void SmartDashboard::PutValue(llvm::StringRef keyName,
+ std::shared_ptr<nt::Value> value) {
+ m_table->PutValue(keyName, value);
+}
+
+/**
+ * Retrieves the complex value (such as an array) in this table into the complex
+ * data object
+ * The key can not be nullptr.
+ * @param keyName the key
+ * @param value the object to retrieve the value into
+ */
+std::shared_ptr<nt::Value> SmartDashboard::GetValue(llvm::StringRef keyName) {
+ return m_table->GetValue(keyName);
+}
+
+/**
+ * Maps the specified key to the specified value in this table.
+ * The key can not be nullptr.
+ * The value can be retrieved by calling the get method with a key that is equal
+ * to the original key.
+ * @param keyName the key
+ * @param value the value
+ */
+void SmartDashboard::PutBoolean(llvm::StringRef keyName, bool value) {
+ m_table->PutBoolean(keyName, value);
+}
+
+/**
+ * Returns the value at the specified key. If the key is not found, returns the
+ * default value.
+ * @param keyName the key
+ * @return the value
+ */
+bool SmartDashboard::GetBoolean(llvm::StringRef keyName, bool defaultValue) {
+ return m_table->GetBoolean(keyName, defaultValue);
+}
+
+/**
+ * Maps the specified key to the specified value in this table.
+ * The key can not be nullptr.
+ * The value can be retrieved by calling the get method with a key that is equal
+ * to the original key.
+ * @param keyName the key
+ * @param value the value
+ */
+void SmartDashboard::PutNumber(llvm::StringRef keyName, double value) {
+ m_table->PutNumber(keyName, value);
+}
+
+/**
+ * Returns the value at the specified key. If the key is not found, returns the
+ * default value.
+ * @param keyName the key
+ * @return the value
+ */
+double SmartDashboard::GetNumber(llvm::StringRef keyName, double defaultValue) {
+ return m_table->GetNumber(keyName, defaultValue);
+}
+
+/**
+ * Maps the specified key to the specified value in this table.
+ * Neither the key nor the value can be nullptr.
+ * The value can be retrieved by calling the get method with a key that is equal
+ * to the original key.
+ * @param keyName the key
+ * @param value the value
+ */
+void SmartDashboard::PutString(llvm::StringRef keyName, llvm::StringRef value) {
+ m_table->PutString(keyName, value);
+}
+
+/**
+ * Returns the value at the specified key. If the key is not found, returns the
+ * default value.
+ * @param keyName the key
+ * @return the value
+ */
+std::string SmartDashboard::GetString(llvm::StringRef keyName,
+ llvm::StringRef defaultValue) {
+ return m_table->GetString(keyName, defaultValue);
+}
diff --git a/wpilibc/shared/src/interfaces/Potentiometer.cpp b/wpilibc/shared/src/interfaces/Potentiometer.cpp
new file mode 100644
index 0000000..3856090
--- /dev/null
+++ b/wpilibc/shared/src/interfaces/Potentiometer.cpp
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015. 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 <interfaces/Potentiometer.h>
+#include <Utility.h>
+
+void Potentiometer::SetPIDSourceType(PIDSourceType pidSource) {
+ if (wpi_assert(pidSource == PIDSourceType::kDisplacement)) {
+ m_pidSource = pidSource;
+ }
+}