copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
new file mode 100644
index 0000000..80d592a
--- /dev/null
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -0,0 +1,122 @@
+#include <stddef.h>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/messages/RobotState.q.h"
+
+namespace aos {
+namespace control_loops {
+
+// TODO(aschuh): Tests.
+
+template <class T, bool has_position>
+void ControlLoop<T, has_position>::ZeroOutputs() {
+ aos::ScopedMessagePtr<OutputType> output =
+ control_loop_->output.MakeMessage();
+ Zero(output.get());
+ output.Send();
+}
+
+template <class T, bool has_position>
+void ControlLoop<T, has_position>::Iterate() {
+ // Temporary storage for printing out inputs and outputs.
+ char state[1024];
+
+ // Fetch the latest control loop goal and position. If there is no new
+ // goal, we will just reuse the old one.
+ // If there is no goal, we haven't started up fully. It isn't worth
+ // the added complexity for each loop implementation to handle that case.
+ control_loop_->goal.FetchLatest();
+ // TODO(aschuh): Check the age here if we want the loop to stop on old
+ // goals.
+ const GoalType *goal = control_loop_->goal.get();
+ if (goal == NULL) {
+ LOG(ERROR, "No prior control loop goal.\n");
+ ZeroOutputs();
+ return;
+ }
+ goal->Print(state, sizeof(state));
+ LOG(DEBUG, "goal={%s}\n", state);
+
+ // Only pass in a position if we got one this cycle.
+ const PositionType *position = NULL;
+
+ // Only fetch the latest position if we have one.
+ if (has_position) {
+ // If the position is stale, this is really bad. Try fetching a position
+ // and check how fresh it is, and then take the appropriate action.
+ if (control_loop_->position.FetchLatest()) {
+ position = control_loop_->position.get();
+ } else {
+ if (control_loop_->position.get()) {
+ int msec_age = control_loop_->position.Age().ToMSec();
+ if (!control_loop_->position.IsNewerThanMS(kPositionTimeoutMs)) {
+ LOG(ERROR, "Stale position. %d ms > %d ms. Outputs disabled.\n",
+ msec_age, kPositionTimeoutMs);
+ ZeroOutputs();
+ return;
+ } else {
+ LOG(ERROR, "Stale position. %d ms\n", msec_age);
+ }
+ } else {
+ LOG(ERROR, "Never had a position.\n");
+ ZeroOutputs();
+ return;
+ }
+ }
+ position->Print(state, sizeof(state));
+ LOG(DEBUG, "position={%s}\n", state);
+ }
+
+ bool outputs_enabled = false;
+
+ // Check to see if we got a driver station packet recently.
+ if (aos::robot_state.FetchLatest()) {
+ outputs_enabled = true;
+ } else if (aos::robot_state.IsNewerThanMS(kDSPacketTimeoutMs)) {
+ outputs_enabled = true;
+ } else {
+ if (aos::robot_state.get()) {
+ int msec_age = aos::robot_state.Age().ToMSec();
+ LOG(ERROR, "Driver Station packet is too old (%d ms).\n", msec_age);
+ } else {
+ LOG(ERROR, "No Driver Station packet.\n");
+ }
+ }
+
+ // Run the iteration.
+ aos::ScopedMessagePtr<StatusType> status =
+ control_loop_->status.MakeMessage();
+ if (status.get() == NULL) {
+ return;
+ }
+
+ if (outputs_enabled) {
+ aos::ScopedMessagePtr<OutputType> output =
+ control_loop_->output.MakeMessage();
+ RunIteration(goal, position, output.get(), status.get());
+
+ output->Print(state, sizeof(state));
+ LOG(DEBUG, "output={%s}\n", state);
+ output.Send();
+ } else {
+ // The outputs are disabled, so pass NULL in for the output.
+ RunIteration(goal, position, NULL, status.get());
+ ZeroOutputs();
+ }
+
+ status->Print(state, sizeof(state));
+ LOG(DEBUG, "status={%s}\n", state);
+ status.Send();
+}
+
+template <class T, bool has_position>
+void ControlLoop<T, has_position>::Run() {
+ while (true) {
+ ::aos::time::PhasedLoop10MS(0);
+ Iterate();
+ }
+}
+
+} // namespace control_loops
+} // namespace aos
diff --git a/aos/common/control_loop/ControlLoop.h b/aos/common/control_loop/ControlLoop.h
new file mode 100644
index 0000000..b69cf01
--- /dev/null
+++ b/aos/common/control_loop/ControlLoop.h
@@ -0,0 +1,133 @@
+#ifndef AOS_CONTROL_LOOP_CONTROL_LOOP_H_
+#define AOS_CONTROL_LOOP_CONTROL_LOOP_H_
+
+#include <cstring>
+
+#include "aos/aos_core.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/type_traits.h"
+
+namespace aos {
+namespace control_loops {
+
+// Interface to describe runnable jobs.
+class Runnable {
+ public:
+ virtual ~Runnable() {}
+ // Runs forever.
+ virtual void Run() = 0;
+ // Does one quick piece of work and return. Does _not_ block.
+ virtual void Iterate() = 0;
+};
+
+class SerializableControlLoop : public Runnable {
+ public:
+ // Returns the size of all the data to be sent when serialized.
+ virtual size_t SeralizedSize() = 0;
+ // Serialize the current data.
+ virtual void Serialize(char *buffer) const = 0;
+ // Serialize zeroed data in case the data is out of date.
+ virtual void SerializeZeroMessage(char *buffer) const = 0;
+ // Deserialize data into the control loop.
+ virtual void Deserialize(const char *buffer) = 0;
+ // Unique identifier for the control loop.
+ // Most likely the hash of the queue group.
+ virtual uint32_t UniqueID() = 0;
+};
+
+// Provides helper methods to assist in writing control loops.
+// This template expects to be constructed with a queue group as an argument
+// that has a goal, position, status, and output queue.
+// It will then call the RunIteration method every cycle that it has enough
+// valid data for the control loop to run.
+// If has_position is false, the control loop will always use NULL as the
+// position and not check the queue. This is used for "loops" that control
+// motors open loop.
+template <class T, bool has_position = true>
+class ControlLoop : public SerializableControlLoop {
+ public:
+ // Maximum age of position packets before the loop will be disabled due to
+ // invalid position data.
+ static const int kPositionTimeoutMs = 100;
+ // Maximum age of driver station packets before the loop will be disabled.
+ static const int kDSPacketTimeoutMs = 100;
+
+ ControlLoop(T *control_loop) : control_loop_(control_loop) {}
+
+ // Create some convenient typedefs to reference the Goal, Position, Status,
+ // and Output structures.
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->goal.MakeMessage().get()))>::type
+ GoalType;
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->position.MakeMessage().get()))>::type
+ PositionType;
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->status.MakeMessage().get()))>::type
+ StatusType;
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->output.MakeMessage().get()))>::type
+ OutputType;
+
+ // Constructs and sends a message on the output queue which will stop all the
+ // motors. Calls Zero to clear all the state.
+ void ZeroOutputs();
+
+ // Sets the output to zero.
+ // Over-ride if a value of zero is not "off" for this subsystem.
+ virtual void Zero(OutputType *output) { output->Zero(); }
+
+ // Runs the loop forever.
+ virtual void Run();
+
+ // Runs one cycle of the loop.
+ virtual void Iterate();
+
+ // Returns the name of the queue group.
+ const char *name() { return control_loop_->name(); }
+
+ // Methods to serialize all the data that should be sent over the network.
+ virtual size_t SeralizedSize() { return control_loop_->goal->Size(); }
+ virtual void Serialize(char *buffer) const {
+ control_loop_->goal->Serialize(buffer);
+ }
+ virtual void SerializeZeroMessage(char *buffer) const {
+ GoalType zero_goal;
+ zero_goal.Zero();
+ zero_goal.Serialize(buffer);
+ }
+
+ virtual void Deserialize(const char *buffer) {
+ ScopedMessagePtr<GoalType> new_msg = control_loop_->goal.MakeMessage();
+ new_msg->Deserialize(buffer);
+ new_msg.Send();
+ }
+
+ virtual uint32_t UniqueID() { return control_loop_->hash(); }
+
+ protected:
+ // Runs an iteration of the control loop.
+ // goal is the last goal that was sent. It might be any number of cycles old.
+ // position is the current position, or NULL if we didn't get a position this
+ // cycle.
+ // output is the values to be sent to the motors. This is NULL if the output
+ // is going to be ignored and set to 0.
+ // status is the status of the control loop.
+ // Both output and status should be filled in by the implementation.
+ virtual void RunIteration(const GoalType *goal,
+ const PositionType *position,
+ OutputType *output,
+ StatusType *status) = 0;
+
+ private:
+ // Pointer to the queue group
+ T *control_loop_;
+};
+
+} // namespace control_loops
+} // namespace aos
+
+#include "aos/common/control_loop/ControlLoop-tmpl.h" // IWYU pragma: export
+
+#endif
diff --git a/aos/common/control_loop/Timing.cpp b/aos/common/control_loop/Timing.cpp
new file mode 100644
index 0000000..63fda44
--- /dev/null
+++ b/aos/common/control_loop/Timing.cpp
@@ -0,0 +1,19 @@
+#include "aos/common/logging/logging.h"
+#include "aos/common/control_loop/Timing.h"
+
+#include "aos/common/time.h"
+
+namespace aos {
+namespace time {
+
+void PhasedLoopXMS(int ms, int offset) {
+ // TODO(brians): Rewrite this cleaner.
+ // TODO(brians): Tests!
+ int64_t period_nsec = Time::InMS(ms).nsec();
+ SleepUntil(Time::InNS((Time::Now().ToNSec() / period_nsec +
+ static_cast<int64_t>(1)) * period_nsec +
+ Time::InUS(offset).ToNSec()));
+}
+
+} // namespace timing
+} // namespace aos
diff --git a/aos/common/control_loop/Timing.h b/aos/common/control_loop/Timing.h
new file mode 100644
index 0000000..139376d
--- /dev/null
+++ b/aos/common/control_loop/Timing.h
@@ -0,0 +1,19 @@
+#ifndef __AOS_TIMEOUT_H_
+#define __AOS_TIMEOUT_H_
+
+#include <time.h>
+#include <string>
+
+namespace aos {
+namespace time {
+
+// Will not be accurate if ms isn't a factor of 1000.
+// offset is in us.
+void PhasedLoopXMS(int ms, int offset);
+// offset is in us.
+inline void PhasedLoop10MS(int offset) { PhasedLoopXMS(10, offset); }
+
+} // namespace time
+} // namespace aos
+
+#endif
diff --git a/aos/common/control_loop/control_loops.q b/aos/common/control_loop/control_loops.q
new file mode 100644
index 0000000..5ba30ec
--- /dev/null
+++ b/aos/common/control_loop/control_loops.q
@@ -0,0 +1,38 @@
+package aos.control_loops;
+
+interface IsDone {
+ bool done;
+};
+
+interface ControlLoop {
+ queue goal;
+ queue position;
+ queue output;
+ queue IsDone status;
+};
+
+message Goal {
+ double goal;
+};
+
+message Position {
+ double position;
+};
+
+message Output {
+ double pwm;
+};
+
+message Status {
+ bool done;
+};
+
+// Single Input Single Output control loop.
+queue_group SISO {
+ implements ControlLoop;
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};