Added initial indexer loop. It pulls in discs correctly, but doesn't do anything else.
diff --git a/aos/common/time.cc b/aos/common/time.cc
index 00c936d..995011a 100644
--- a/aos/common/time.cc
+++ b/aos/common/time.cc
@@ -8,21 +8,66 @@
#include "aos/common/logging/logging.h"
#include "aos/common/inttypes.h"
+#include "aos/common/mutex.h"
namespace aos {
namespace time {
-Time Time::Now(clockid_t clock) {
- timespec temp;
- if (clock_gettime(clock, &temp) != 0) {
- // TODO(aschuh): There needs to be a pluggable low level logging interface
- // so we can break this dependency loop. This also would help during
- // startup.
- LOG(FATAL, "clock_gettime(%jd, %p) failed with %d: %s\n",
- static_cast<uintmax_t>(clock), &temp, errno, strerror(errno));
- }
- return Time(temp);
+// State required to enable and use mock time.
+namespace {
+// True if mock time is enabled.
+// This does not need to be checked with the mutex held because setting time to
+// be enabled or disabled is atomic, and all future operations are atomic
+// anyways. If there is a race condition setting or clearing whether time is
+// enabled or not, it will still be a race condition if current_mock_time is
+// also set atomically with enabled.
+bool mock_time_enabled = false;
+// Mutex to make time reads and writes thread safe.
+Mutex time_mutex;
+// Current time when time is mocked.
+Time current_mock_time(0, 0);
+
+// TODO(aschuh): This doesn't include SleepFor and SleepUntil.
+// TODO(aschuh): Create a clock source object and change the default?
+// That would let me create a MockTime clock source.
}
+
+void Time::EnableMockTime(const Time now) {
+ mock_time_enabled = true;
+ MutexLocker time_mutex_locker(&time_mutex);
+ current_mock_time = now;
+}
+
+void Time::DisableMockTime() {
+ MutexLocker time_mutex_locker(&time_mutex);
+ mock_time_enabled = false;
+}
+
+void Time::SetMockTime(const Time now) {
+ MutexLocker time_mutex_locker(&time_mutex);
+ if (!mock_time_enabled) {
+ LOG(FATAL, "Tried to set mock time and mock time is not enabled\n");
+ }
+ current_mock_time = now;
+}
+
+Time Time::Now(clockid_t clock) {
+ if (mock_time_enabled) {
+ MutexLocker time_mutex_locker(&time_mutex);
+ return current_mock_time;
+ } else {
+ timespec temp;
+ if (clock_gettime(clock, &temp) != 0) {
+ // TODO(aschuh): There needs to be a pluggable low level logging interface
+ // so we can break this dependency loop. This also would help during
+ // startup.
+ LOG(FATAL, "clock_gettime(%jd, %p) failed with %d: %s\n",
+ static_cast<uintmax_t>(clock), &temp, errno, strerror(errno));
+ }
+ return Time(temp);
+ }
+}
+
void Time::Check() {
if (nsec_ >= kNSecInSec || nsec_ < 0) {
LOG(FATAL, "0 <= nsec_(%"PRId32") < %"PRId32" isn't true.\n",
diff --git a/aos/common/time.h b/aos/common/time.h
index 1d7ccae..38238f2 100644
--- a/aos/common/time.h
+++ b/aos/common/time.h
@@ -60,6 +60,7 @@
return ans;
}
#endif // SWIG
+
// CLOCK_MONOTONIC on the fitpc and CLOCK_REALTIME on the cRIO because the
// cRIO doesn't have any others.
// CLOCK_REALTIME is the default realtime clock and CLOCK_MONOTONIC doesn't
@@ -159,6 +160,15 @@
Check();
}
+ // Enables returning the mock time value for Now instead of checking the
+ // system clock. This should only be used when testing things depending on
+ // time, or many things may/will break.
+ static void EnableMockTime(const Time now);
+ // Sets now when time is being mocked.
+ static void SetMockTime(const Time now);
+ // Disables mocking time.
+ static void DisableMockTime();
+
private:
int32_t sec_, nsec_;
// LOG(FATAL)s if nsec_ is >= kNSecInSec.
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index 18df792..7a8c8f1 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -2,6 +2,7 @@
'variables': {
'loop_files': [
'DriveTrain.q',
+ 'index_motor.q',
]
},
'targets': [
@@ -37,6 +38,50 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
+ {
+ 'target_name': 'index_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'index.cc',
+ 'index_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'control_loops',
+ '<(AOS)/common/common.gyp:controls',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(EXTERNALS):eigen',
+ ],
+ },
+ {
+ 'target_name': 'index_lib_test',
+ 'type': 'executable',
+ 'sources': [
+ 'index_lib_test.cc',
+ 'transfer_motor_plant.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ 'control_loops',
+ 'index_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(EXTERNALS):eigen',
+ ],
+ },
+ {
+ 'target_name': 'index',
+ 'type': 'executable',
+ 'sources': [
+ 'index_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ 'index_lib',
+ 'control_loops',
+ ],
+ },
+ {
'target_name': 'DriveTrain',
'type': 'executable',
'sources': [
diff --git a/frc971/control_loops/index.cc b/frc971/control_loops/index.cc
new file mode 100644
index 0000000..845f1c6
--- /dev/null
+++ b/frc971/control_loops/index.cc
@@ -0,0 +1,237 @@
+#include "frc971/control_loops/index.h"
+
+#include <stdio.h>
+
+#include <algorithm>
+
+#include "aos/aos_core.h"
+
+#include "aos/common/messages/RobotState.q.h"
+#include "aos/common/control_loop/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/index_motor_plant.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+
+IndexMotor::IndexMotor(control_loops::IndexLoop *my_index)
+ : aos::control_loops::ControlLoop<control_loops::IndexLoop>(my_index),
+ wrist_loop_(new StateFeedbackLoop<2, 1, 1>(MakeIndexLoop())),
+ hopper_disc_count_(0),
+ total_disc_count_(0),
+ loader_up_(false),
+ disc_clamped_(false),
+ disc_ejected_(false),
+ last_bottom_disc_detect_(false) {
+}
+
+const /*static*/ double IndexMotor::kDiscRadius = 10.875 * 0.0254 / 2;
+const /*static*/ double IndexMotor::kRollerRadius = 2.0 * 0.0254 / 2;
+
+bool IndexMotor::FetchConstants() {
+ if (!constants::horizontal_lower_limit(&horizontal_lower_limit_)) {
+ LOG(ERROR, "Failed to fetch the horizontal lower limit constant.\n");
+ return false;
+ }
+ if (!constants::horizontal_upper_limit(&horizontal_upper_limit_)) {
+ LOG(ERROR, "Failed to fetch the horizontal upper limit constant.\n");
+ return false;
+ }
+ if (!constants::horizontal_hall_effect_start_angle(
+ &horizontal_hall_effect_start_angle_)) {
+ LOG(ERROR, "Failed to fetch the horizontal start angle constant.\n");
+ return false;
+ }
+ if (!constants::horizontal_zeroing_speed(
+ &horizontal_zeroing_speed_)) {
+ LOG(ERROR, "Failed to fetch the horizontal zeroing speed constant.\n");
+ return false;
+ }
+
+ return true;
+}
+
+// Distance to move the indexer when grabbing a disc.
+const double kNextPosition = 10.0;
+
+/*static*/ double IndexMotor::ConvertDiscAngleToIndex(const double angle) {
+ return (angle * (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
+}
+
+/*static*/ double IndexMotor::ConvertDiscAngleToDiscPosition(const double angle) {
+ return angle * (kDiscRadius + kRollerRadius);
+}
+
+/*static*/ double IndexMotor::ConvertIndexToDiscAngle(const double angle) {
+ return (angle / (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
+}
+
+/*static*/ double IndexMotor::ConvertIndexToDiscPosition(const double angle) {
+ return IndexMotor::ConvertDiscAngleToDiscPosition(
+ ConvertIndexToDiscAngle(angle));
+}
+
+// Positive angle is towards the shooter, and positive power is towards the
+// shooter.
+void IndexMotor::RunIteration(
+ const control_loops::IndexLoop::Goal *goal,
+ const control_loops::IndexLoop::Position *position,
+ control_loops::IndexLoop::Output *output,
+ control_loops::IndexLoop::Status *status) {
+ // Make goal easy to work with.
+ Goal goal_enum = static_cast<Goal>(goal->goal_state);
+
+ // Disable the motors now so that all early returns will return with the
+ // motors disabled.
+ if (output) {
+ output->transfer_voltage = 0.0;
+ output->index_voltage = 0.0;
+ }
+
+ status->ready_to_intake = false;
+
+ // Cache the constants to avoid error handling down below.
+ if (!FetchConstants()) {
+ return;
+ }
+
+ if (position) {
+ wrist_loop_->Y << position->index_position;
+ }
+ const double index_position = wrist_loop_->X_hat(0, 0);
+
+ bool safe_to_change_state_ = true;
+ switch (safe_goal_) {
+ case HOLD:
+ // The goal should already be good, so sit tight with everything the same
+ // as it was.
+ printf("HOLD Not implemented\n");
+ break;
+ case READY_LOWER:
+ printf("READY_LOWER Not implemented\n");
+ break;
+ case INTAKE:
+ {
+ Time now = Time::Now();
+ if (hopper_disc_count_ < 4) {
+ output->transfer_voltage = 12.0;
+ }
+ // Posedge of the disc entering the beam break.
+ if (position) {
+ if (position->bottom_disc_detect && !last_bottom_disc_detect_) {
+ transfer_frisbee_.Reset();
+ transfer_frisbee_.bottom_posedge_time_ = now;
+ printf("Posedge of bottom disc %f\n",
+ transfer_frisbee_.bottom_posedge_time_.ToSeconds());
+ ++hopper_disc_count_;
+ }
+
+ // Disc exited the beam break now.
+ if (!position->bottom_disc_detect && last_bottom_disc_detect_) {
+ transfer_frisbee_.bottom_negedge_time_ = now;
+ printf("Negedge of bottom disc %f\n",
+ transfer_frisbee_.bottom_negedge_time_.ToSeconds());
+ frisbees_.push_front(transfer_frisbee_);
+ }
+
+ if (position->bottom_disc_detect) {
+ output->transfer_voltage = 12.0;
+ // Must wait until the disc gets out before we can change state.
+ safe_to_change_state_ = false;
+
+ // TODO(aschuh): A disc on the way through needs to start moving the
+ // indexer if it isn't already moving. Maybe?
+
+ Time elapsed_posedge_time = now -
+ transfer_frisbee_.bottom_posedge_time_;
+ if (elapsed_posedge_time >= Time::InSeconds(0.3)) {
+ // It has been too long. The disc must be jammed.
+ LOG(ERROR, "Been way too long. Jammed disc?\n");
+ printf("Been way too long. Jammed disc?\n");
+ }
+ }
+
+ for (Frisbee &frisbee : frisbees_) {
+ if (!frisbee.has_been_indexed_) {
+ output->transfer_voltage = 12.0;
+ Time elapsed_posedge_time = now -
+ frisbee.bottom_posedge_time_;
+ if (elapsed_posedge_time >= Time::InSeconds(0.07)) {
+ // Should have just engaged.
+ // Save the indexer position, and the time.
+
+ // It has been long enough since the disc entered the indexer.
+ // Treat now as the time at which it contacted the indexer.
+ LOG(INFO, "Grabbed on the index now at %f\n", index_position);
+ printf("Grabbed on the index now at %f\n", index_position);
+ frisbee.has_been_indexed_ = true;
+ frisbee.index_start_position_ = index_position;
+ frisbee.index_start_time_ = now;
+ }
+ }
+ if (!frisbee.has_been_indexed_) {
+ // Discs must all be indexed before it is safe to stop indexing.
+ safe_to_change_state_ = false;
+ }
+ }
+
+ double new_index_position = wrist_loop_->R(0, 0);
+
+ // TODO(aschuh): As we loop through, assess the state of the indexer
+ // and figure if the bottom disc is in a place such that we can
+ // intake without filling the hopper early.
+ // status->ready_to_intake = false;
+
+ for (Frisbee &frisbee : frisbees_) {
+ if (frisbee.has_been_indexed_) {
+ // We want to store it pi from where the disc was grabbed
+ // (for now).
+ new_index_position = ::std::max(
+ new_index_position,
+ (frisbee.index_start_position_ +
+ ConvertDiscAngleToIndex(M_PI)));
+ // TODO(aschuh): We should be able to pick the M_PI knowing if
+ // the next disc is coming in hot or not.
+ }
+ }
+ wrist_loop_->R << new_index_position, 0.0;
+ }
+ printf("INTAKE Not implemented\n");
+ }
+ break;
+ case READY_SHOOTER:
+ printf("READY_SHOOTER Not implemented\n");
+ break;
+ case SHOOT:
+ printf("SHOOT Not implemented\n");
+ break;
+ }
+
+ // Update the observer.
+ wrist_loop_->Update(position != NULL, output == NULL);
+
+ if (position) {
+ LOG(DEBUG, "pos=%f currently %f\n",
+ position->index_position, index_position);
+ last_bottom_disc_detect_ = position->bottom_disc_detect;
+ }
+
+ status->hopper_disc_count = hopper_disc_count_;
+ status->total_disc_count = total_disc_count_;
+
+
+ if (output) {
+ output->index_voltage = wrist_loop_->U(0, 0);
+ }
+
+ if (safe_to_change_state_) {
+ safe_goal_ = goal_enum;
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/index.h b/frc971/control_loops/index.h
new file mode 100644
index 0000000..c62c639
--- /dev/null
+++ b/frc971/control_loops/index.h
@@ -0,0 +1,125 @@
+#ifndef FRC971_CONTROL_LOOPS_WRIST_H_
+#define FRC971_CONTROL_LOOPS_WRIST_H_
+
+#include <memory>
+#include <deque>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "aos/common/time.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/index_motor.q.h"
+#include "frc971/control_loops/index_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+
+class IndexMotor
+ : public aos::control_loops::ControlLoop<control_loops::IndexLoop> {
+ public:
+ explicit IndexMotor(
+ control_loops::IndexLoop *my_index = &control_loops::index);
+
+ // Converts the angle of the indexer to the angle of the disc.
+ static double ConvertIndexToDiscAngle(const double angle);
+ // Converts the angle of the indexer to the position that the center of the
+ // disc has traveled.
+ static double ConvertIndexToDiscPosition(const double angle);
+
+ // Converts the angle around the indexer to the position of the index roller.
+ static double ConvertDiscAngleToIndex(const double angle);
+ // Converts the angle around the indexer to the position of the disc in the
+ // indexer.
+ static double ConvertDiscAngleToDiscPosition(const double angle);
+
+ // Disc radius in meters.
+ const static double kDiscRadius;
+ // Roller radius in meters.
+ const static double kRollerRadius;
+
+ class Frisbee {
+ public:
+ Frisbee()
+ : bottom_posedge_time_(0, 0),
+ bottom_negedge_time_(0, 0),
+ index_start_time_(0, 0) {
+ Reset();
+ }
+
+ void Reset() {
+ bottom_posedge_time_ = ::aos::time::Time(0, 0);
+ bottom_negedge_time_ = ::aos::time::Time(0, 0);
+ index_start_time_ = ::aos::time::Time(0, 0);
+ has_been_indexed_ = false;
+ index_start_position_ = 0.0;
+ }
+
+ ::aos::time::Time bottom_posedge_time_;
+ ::aos::time::Time bottom_negedge_time_;
+ ::aos::time::Time index_start_time_;
+ bool has_been_indexed_;
+ double index_start_position_;
+ };
+
+ protected:
+ virtual void RunIteration(
+ const control_loops::IndexLoop::Goal *goal,
+ const control_loops::IndexLoop::Position *position,
+ control_loops::IndexLoop::Output *output,
+ control_loops::IndexLoop::Status *status);
+
+ private:
+ // Fetches and locally caches the latest set of constants.
+ bool FetchConstants();
+
+ // The state feedback control loop to talk to for the index.
+ ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> wrist_loop_;
+
+ // Local cache of the index geometry constants.
+ double horizontal_lower_limit_;
+ double horizontal_upper_limit_;
+ double horizontal_hall_effect_start_angle_;
+ double horizontal_zeroing_speed_;
+
+ // Count of the number of discs that we have collected.
+ uint32_t hopper_disc_count_;
+ uint32_t total_disc_count_;
+
+ enum Goal {
+ // Hold position, in a low power state.
+ HOLD = 0,
+ // Get ready to load discs by shifting the discs down.
+ READY_LOWER = 1,
+ // Ready the discs, spin up the transfer roller, and accept discs.
+ INTAKE = 2,
+ // Get ready to shoot, and place a disc in the loader.
+ READY_SHOOTER = 3,
+ // Shoot at will.
+ SHOOT = 4
+ };
+
+ // The current goal
+ Goal safe_goal_;
+
+ // Current state of the pistons.
+ bool loader_up_;
+ bool disc_clamped_;
+ bool disc_ejected_;
+
+ //::aos::time::Time disc_bottom_posedge_time_;
+ //::aos::time::Time disc_bottom_negedge_time_;
+ // The frisbee that is flying through the transfer rollers.
+ Frisbee transfer_frisbee_;
+
+ bool last_bottom_disc_detect_;
+
+ // Frisbees are in order such that the newest frisbee is on the front.
+ ::std::deque<Frisbee> frisbees_;
+ // std::array ?
+
+ DISALLOW_COPY_AND_ASSIGN(IndexMotor);
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_WRIST_H_
diff --git a/frc971/control_loops/index_lib_test.cc b/frc971/control_loops/index_lib_test.cc
new file mode 100644
index 0000000..418a9b6
--- /dev/null
+++ b/frc971/control_loops/index_lib_test.cc
@@ -0,0 +1,315 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/index_motor.q.h"
+#include "frc971/control_loops/index.h"
+#include "frc971/control_loops/index_motor_plant.h"
+#include "frc971/control_loops/transfer_motor_plant.h"
+#include "frc971/constants.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// TODO(aschuh): Figure out these constants.
+const double kTransferStartPosition = 0.0;
+const double kIndexStartPosition = 0.5;
+const double kIndexStopPosition = 2.5;
+const double kGrabberStopPosition = 2.625;
+const double kGrabberMovementVelocity = 0.4;
+
+// Start and stop position of the bottom disc detect sensor in meters.
+const double kBottomDiscDetectStart = -0.08;
+const double kBottomDiscDetectStop = 0.200025;
+
+const double kTopDiscDetectStart = 18.0;
+const double kTopDiscDetectStop = 19.0;
+
+// Disc radius in meters.
+const double kDiscRadius = 11.875 * 0.0254 / 2;
+// Roller radius in meters.
+const double kRollerRadius = 2.0 * 0.0254 / 2;
+
+class Frisbee {
+ public:
+ // Creates a frisbee starting at the specified position in the frisbee path,
+ // and with the transfer and index rollers at the specified positions.
+ Frisbee(double transfer_roller_position,
+ double index_roller_position,
+ double position = 0.0)
+ : transfer_roller_position_(transfer_roller_position),
+ index_roller_position_(index_roller_position),
+ clamped_(false),
+ position_(position) {
+ }
+
+ // Returns true if the frisbee is controlled by the transfer roller.
+ bool IsTouchingTransfer() const {
+ return (position_ >= kTransferStartPosition &&
+ position_ <= kIndexStartPosition);
+ }
+
+ // Returns true if the frisbee is controlled by the indexing roller.
+ bool IsTouchingIndex() const {
+ return (position_ >= kIndexStartPosition &&
+ position_ <= kIndexStopPosition);
+ }
+
+ // Returns true if the frisbee is in a position such that the grabber will
+ // pull it into the loader.
+ bool IsTouchingGrabber() const {
+ return (position_ >= kIndexStopPosition &&
+ position_ <= kGrabberStopPosition);
+ }
+
+ // Returns true if the disc is triggering the bottom disc detect sensor.
+ bool bottom_disc_detect() const {
+ return (position_ >= kBottomDiscDetectStart &&
+ position_ <= kBottomDiscDetectStop);
+ }
+
+ // Returns true if the disc is triggering the top disc detect sensor.
+ bool top_disc_detect() const {
+ return (position_ >= kTopDiscDetectStart &&
+ position_ <= kTopDiscDetectStop);
+ }
+
+ // Converts the angle of the indexer to the distance traveled by the center of
+ // the disc.
+ double ConvertIndexToDiscPosition(const double angle) const {
+ return (angle * (kDiscRadius + kRollerRadius) /
+ (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
+ }
+
+ // Converts the angle of the transfer to the distance traveled by the center
+ // of the disc.
+ double ConvertTransferToDiscPosition(const double angle) const {
+ return ConvertIndexToDiscPosition(angle);
+ }
+
+ // Updates the position of the frisbee in the frisbee path.
+ void UpdatePosition(double transfer_roller_position,
+ double index_roller_position,
+ bool clamped) {
+ // TODO(aschuh): Assert that you can't slide the frisbee through the
+ // clamp.
+ if (IsTouchingTransfer()) {
+ position_ += ConvertTransferToDiscPosition(transfer_roller_position -
+ transfer_roller_position_);
+ } else if (IsTouchingIndex()) {
+ position_ += ConvertIndexToDiscPosition(index_roller_position -
+ index_roller_position_);
+ } else if (IsTouchingGrabber()) {
+ if (clamped) {
+ position_ = ::std::min(position_ + kGrabberMovementVelocity / 100.0,
+ kGrabberStopPosition);
+ }
+ } else {
+ // TODO(aschuh): Deal with lifting.
+ // TODO(aschuh): Deal with shooting.
+ // We must wait long enough for the disc to leave the loader before
+ // lowering.
+ }
+ transfer_roller_position_ = transfer_roller_position;
+ index_roller_position_ = index_roller_position;
+ clamped_ = clamped;
+ printf("Disc is at %f\n", position_);
+ }
+
+ double position() const {
+ return position_;
+ }
+
+ private:
+ double transfer_roller_position_;
+ double index_roller_position_;
+ bool clamped_;
+ double position_;
+};
+
+
+// Class which simulates the index and sends out queue messages containing the
+// position.
+class IndexMotorSimulation {
+ public:
+ // Constructs a motor simulation. initial_position is the inital angle of the
+ // index, which will be treated as 0 by the encoder.
+ IndexMotorSimulation()
+ : index_plant_(new StateFeedbackPlant<2, 1, 1>(MakeIndexPlant())),
+ transfer_plant_(new StateFeedbackPlant<2, 1, 1>(MakeTransferPlant())),
+ my_index_loop_(".frc971.control_loops.index",
+ 0x1a7b7094, ".frc971.control_loops.index.goal",
+ ".frc971.control_loops.index.position",
+ ".frc971.control_loops.index.output",
+ ".frc971.control_loops.index.status") {
+ }
+
+ // Starts a disc at the start of the index.
+ void InsertDisc() {
+ frisbees.push_back(Frisbee(transfer_roller_position(),
+ index_roller_position()));
+ }
+
+ // Returns true if the bottom disc sensor is triggered.
+ bool BottomDiscDetect() const {
+ bool bottom_disc_detect = false;
+ for (const Frisbee &frisbee : frisbees) {
+ bottom_disc_detect |= frisbee.bottom_disc_detect();
+ }
+ return bottom_disc_detect;
+ }
+
+ // Returns true if the top disc sensor is triggered.
+ bool TopDiscDetect() const {
+ bool top_disc_detect = false;
+ for (const Frisbee &frisbee : frisbees) {
+ top_disc_detect |= frisbee.top_disc_detect();
+ }
+ return top_disc_detect;
+ }
+
+ void UpdateDiscs(bool clamped) {
+ for (Frisbee &frisbee : frisbees) {
+ // TODO(aschuh): Simulate clamping
+ frisbee.UpdatePosition(transfer_roller_position(),
+ index_roller_position(),
+ clamped);
+ }
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::IndexLoop::Position> position =
+ my_index_loop_.position.MakeMessage();
+ position->index_position = index_roller_position();
+ position->bottom_disc_detect = BottomDiscDetect();
+ position->top_disc_detect = TopDiscDetect();
+ printf("bdd: %x tdd: %x\n", position->bottom_disc_detect,
+ position->top_disc_detect);
+ position.Send();
+ }
+
+ // Simulates the index moving for one timestep.
+ void Simulate() {
+ EXPECT_TRUE(my_index_loop_.output.FetchLatest());
+
+ index_plant_->U << my_index_loop_.output->index_voltage;
+ index_plant_->Update();
+
+ transfer_plant_->U << my_index_loop_.output->transfer_voltage;
+ transfer_plant_->Update();
+ printf("tv: %f iv: %f tp : %f ip: %f\n",
+ my_index_loop_.output->transfer_voltage,
+ my_index_loop_.output->index_voltage,
+ transfer_roller_position(), index_roller_position());
+
+ UpdateDiscs(my_index_loop_.output->disc_clamped);
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> index_plant_;
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> transfer_plant_;
+
+ // Returns the absolute angle of the index.
+ double index_roller_position() const {
+ return index_plant_->Y(0, 0);
+ }
+
+ // Returns the absolute angle of the index.
+ double transfer_roller_position() const {
+ return transfer_plant_->Y(0, 0);
+ }
+
+ ::std::vector<Frisbee> frisbees;
+
+ private:
+ IndexLoop my_index_loop_;
+};
+
+
+class IndexTest : public ::testing::Test {
+ protected:
+ IndexTest() : my_index_loop_(".frc971.control_loops.index",
+ 0x1a7b7094, ".frc971.control_loops.index.goal",
+ ".frc971.control_loops.index.position",
+ ".frc971.control_loops.index.output",
+ ".frc971.control_loops.index.status"),
+ index_motor_(&my_index_loop_),
+ index_motor_plant_(),
+ loop_count_(0) {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ Time::EnableMockTime(Time(0, 0));
+ }
+
+ virtual ~IndexTest() {
+ ::aos::robot_state.Clear();
+ Time::DisableMockTime();
+ }
+
+ // Sends a DS packet with the enable bit set to enabled.
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ // Updates the current mock time.
+ void UpdateTime() {
+ loop_count_ += 1;
+ Time::SetMockTime(Time::InMS(10 * loop_count_));
+ }
+
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ IndexLoop my_index_loop_;
+
+ // Create a loop and simulation plant.
+ IndexMotor index_motor_;
+ IndexMotorSimulation index_motor_plant_;
+
+ int loop_count_;
+};
+
+// Tests that the index grabs 1 disc and places it at the correct position.
+TEST_F(IndexTest, GrabSingleDisc) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 250; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (i == 100) {
+ EXPECT_EQ(0, index_motor_plant_.index_roller_position());
+ index_motor_plant_.InsertDisc();
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ kIndexStartPosition + IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ index_motor_plant_.frisbees[0].position(), 0.01);
+}
+
+// Test that pulling in a second disc works correctly.
+// Test that HOLD still finishes the disc correctly.
+// Test that pulling a disc down works correctly and ready_to_intake waits.
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/index_main.cc b/frc971/control_loops/index_main.cc
new file mode 100644
index 0000000..cf18412
--- /dev/null
+++ b/frc971/control_loops/index_main.cc
@@ -0,0 +1,11 @@
+#include "frc971/control_loops/wrist.h"
+
+#include "aos/aos_core.h"
+
+int main() {
+ ::aos::Init();
+ frc971::control_loops::WristMotor wrist;
+ looper.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/index_motor.q b/frc971/control_loops/index_motor.q
new file mode 100644
index 0000000..bd1564b
--- /dev/null
+++ b/frc971/control_loops/index_motor.q
@@ -0,0 +1,53 @@
+package frc971.control_loops;
+
+import "aos/common/control_loop/control_loops.q";
+
+queue_group IndexLoop {
+ implements aos.control_loops.ControlLoop;
+
+ message Goal {
+ // The state for the indexer to be in.
+ // 0 means hold position, in a low power state.
+ // 1 means get ready to load discs by shifting the discs down.
+ // 2 means ready the discs, spin up the transfer roller, and accept discs.
+ // 3 means get ready to shoot, and place a disc grabbed in the loader.
+ // 4 means shoot at will.
+ int32_t goal_state;
+ };
+
+ message Position {
+ // Index position
+ double index_position;
+ bool bottom_disc_detect;
+ bool top_disc_detect;
+ };
+
+ message Output {
+ // Transfer roller output voltage.
+ // Positive means into the robot.
+ double transfer_voltage;
+ // Index roller. Positive means up towards the shooter.
+ double index_voltage;
+ // Loader pistons.
+ bool loader_up;
+ bool disc_clamped;
+ bool disc_ejected;
+ };
+
+ message Status {
+ // Number of discs in the hopper
+ int32_t hopper_disc_count;
+ // Number of shot since reboot.
+ int32_t total_disc_count;
+ // Ready to load.
+ bool preloaded;
+ bool ready_to_intake;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group IndexLoop index;
diff --git a/frc971/control_loops/index_motor_plant.cc b/frc971/control_loops/index_motor_plant.cc
new file mode 100644
index 0000000..5b77deb
--- /dev/null
+++ b/frc971/control_loops/index_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/index_motor_plant.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+
+StateFeedbackPlant<2, 1, 1> MakeIndexPlant() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0490373507155, 9.35402266105;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeIndexLoop() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.64731520998, 56.0569452572;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1.06905877421, 0.0368709177253;
+ return StateFeedbackLoop<2, 1, 1>(L, K, MakeIndexPlant());
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/index_motor_plant.h b/frc971/control_loops/index_motor_plant.h
new file mode 100644
index 0000000..4d4f3b1
--- /dev/null
+++ b/frc971/control_loops/index_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_INDEX_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_INDEX_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<2, 1, 1> MakeIndexPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeIndexLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#endif // FRC971_CONTROL_LOOPS_INDEX_MOTOR_PLANT_H_
diff --git a/frc971/control_loops/python/index.py b/frc971/control_loops/python/index.py
new file mode 100755
index 0000000..5ce7613
--- /dev/null
+++ b/frc971/control_loops/python/index.py
@@ -0,0 +1,91 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Index(control_loop.ControlLoop):
+ def __init__(self):
+ super(Index, self).__init__("Index")
+ # Stall Torque in N m
+ self.stall_torque = 0.4862
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0
+ # Free Current in Amps
+ self.free_current = 1.5
+ # Moment of inertia of the index in kg m^2
+ self.J = 0.00013
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current + 0.024 + .003
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (13.5 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 1.0 / ((40.0 / 11.0) * (34.0 / 30.0))
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.75, .6])
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ index = Index()
+ simulated_x = []
+ simulated_v = []
+ for _ in xrange(100):
+ index.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(index.X[0, 0])
+ simulated_v.append(index.X[1, 0])
+
+ pylab.plot(range(100), simulated_v)
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ index = Index()
+ close_loop_x = []
+ R = numpy.matrix([[1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(index.K * (R - index.X_hat), index.U_min, index.U_max)
+ index.UpdateObserver(U)
+ index.Update(U)
+ close_loop_x.append(index.X[0, 0])
+
+ pylab.plot(range(100), close_loop_x)
+ pylab.show()
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .cc file name and .h file name"
+ else:
+ index.DumpHeaderFile(argv[1])
+ index.DumpCppFile(argv[2], argv[1])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/transfer.py b/frc971/control_loops/python/transfer.py
new file mode 100755
index 0000000..2ec7926
--- /dev/null
+++ b/frc971/control_loops/python/transfer.py
@@ -0,0 +1,91 @@
+#!/usr/bin/python
+
+import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Transfer(control_loop.ControlLoop):
+ def __init__(self):
+ super(Transfer, self).__init__("Transfer")
+ # Stall Torque in N m
+ self.stall_torque = 0.4862
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0
+ # Free Current in Amps
+ self.free_current = 1.5
+ # Moment of inertia of the transfer in kg m^2
+ self.J = 0.00013
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current + 0.024 + .003
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (13.5 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 1.0 / ((40.0 / 11.0) * (34.0 / 30.0))
+ # Control loop time step
+ self.dt = 0.01
+
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.dt, self.C)
+
+ self.PlaceControllerPoles([.75, .6])
+
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+ # Simulate the response of the system to a step input.
+ transfer = Transfer()
+ simulated_x = []
+ simulated_v = []
+ for _ in xrange(100):
+ transfer.Update(numpy.matrix([[12.0]]))
+ simulated_x.append(transfer.X[0, 0])
+ simulated_v.append(transfer.X[1, 0])
+
+ pylab.plot(range(100), simulated_v)
+ pylab.show()
+
+ # Simulate the closed loop response of the system to a step input.
+ transfer = Transfer()
+ close_loop_x = []
+ R = numpy.matrix([[1.0], [0.0]])
+ for _ in xrange(100):
+ U = numpy.clip(transfer.K * (R - transfer.X_hat), transfer.U_min, transfer.U_max)
+ transfer.UpdateObserver(U)
+ transfer.Update(U)
+ close_loop_x.append(transfer.X[0, 0])
+
+ #pylab.plot(range(100), close_loop_x)
+ #pylab.show()
+
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ print "Expected .cc file name and .h file name"
+ else:
+ transfer.DumpHeaderFile(argv[1])
+ transfer.DumpCppFile(argv[2], argv[1])
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/transfer_motor_plant.cc b/frc971/control_loops/transfer_motor_plant.cc
new file mode 100644
index 0000000..ac3357a
--- /dev/null
+++ b/frc971/control_loops/transfer_motor_plant.cc
@@ -0,0 +1,34 @@
+#include "frc971/control_loops/transfer_motor_plant.h"
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+
+StateFeedbackPlant<2, 1, 1> MakeTransferPlant() {
+ Eigen::Matrix<double, 2, 2> A;
+ A << 1.0, 0.00867533005665, 0.0, 0.747315209983;
+ Eigen::Matrix<double, 2, 1> B;
+ B << 0.0490373507155, 9.35402266105;
+ Eigen::Matrix<double, 1, 2> C;
+ C << 1, 0;
+ Eigen::Matrix<double, 1, 1> D;
+ D << 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max << 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min << -12.0;
+ return StateFeedbackPlant<2, 1, 1>(A, B, C, D, U_max, U_min);
+}
+
+StateFeedbackLoop<2, 1, 1> MakeTransferLoop() {
+ Eigen::Matrix<double, 2, 1> L;
+ L << 1.64731520998, 56.0569452572;
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1.06905877421, 0.0368709177253;
+ return StateFeedbackLoop<2, 1, 1>(L, K, MakeTransferPlant());
+}
+
+} // namespace frc971
+} // namespace control_loops
diff --git a/frc971/control_loops/transfer_motor_plant.h b/frc971/control_loops/transfer_motor_plant.h
new file mode 100644
index 0000000..41690c1
--- /dev/null
+++ b/frc971/control_loops/transfer_motor_plant.h
@@ -0,0 +1,16 @@
+#ifndef FRC971_CONTROL_LOOPS_TRANSFER_MOTOR_PLANT_H_
+#define FRC971_CONTROL_LOOPS_TRANSFER_MOTOR_PLANT_H_
+
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+
+StateFeedbackPlant<2, 1, 1> MakeTransferPlant();
+
+StateFeedbackLoop<2, 1, 1> MakeTransferLoop();
+
+} // namespace frc971
+} // namespace control_loops
+
+#endif // FRC971_CONTROL_LOOPS_TRANSFER_MOTOR_PLANT_H_