Hooked vision up to the turret.
Change-Id: I87762fdedf8572f7bdc17711c96b4e2f5a7251ba
diff --git a/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index bac8655..797791f 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -79,6 +79,7 @@
':superstructure_queue',
'//aos/common:ring_buffer',
'//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//y2017/control_loops/drivetrain:polydrivetrain_plants',
'//y2017/vision:vision_queue',
],
)
diff --git a/y2017/control_loops/superstructure/column/BUILD b/y2017/control_loops/superstructure/column/BUILD
index 8704cf5..149ef32 100644
--- a/y2017/control_loops/superstructure/column/BUILD
+++ b/y2017/control_loops/superstructure/column/BUILD
@@ -49,6 +49,7 @@
'//frc971/control_loops:profiled_subsystem',
'//y2017/control_loops/superstructure/intake:intake',
'//y2017/control_loops/superstructure:superstructure_queue',
+ '//y2017/control_loops/superstructure:vision_time_adjuster',
'//y2017:constants',
],
)
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
index 5e9a21c..a6b80c8 100644
--- a/y2017/control_loops/superstructure/column/column.cc
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -382,6 +382,9 @@
bool disable = turret_output == nullptr || indexer_output == nullptr;
profiled_subsystem_.Correct(*position);
+ vision_time_adjuster_.Tick(::aos::monotonic_clock::now(),
+ profiled_subsystem_.X_hat(2, 0));
+
switch (state_) {
case State::UNINITIALIZED:
// Wait in the uninitialized state until the turret is initialized.
@@ -511,6 +514,14 @@
profiled_subsystem_.set_unprofiled_goal(
unsafe_indexer_goal->angular_velocity, unsafe_turret_goal->angle);
+ if (unsafe_turret_goal->track) {
+ if (vision_time_adjuster_.valid()) {
+ LOG(INFO, "Vision aligning to %f\n", vision_time_adjuster_.goal());
+ profiled_subsystem_.set_turret_unprofiled_goal(
+ vision_time_adjuster_.goal());
+ }
+ }
+
if (freeze_) {
profiled_subsystem_.ForceGoal(unsafe_indexer_goal->angular_velocity,
starting_goal_angle);
@@ -599,6 +610,15 @@
profiled_subsystem_.PopulateTurretStatus(turret_status);
turret_status->estopped = (state_ == State::ESTOP);
turret_status->state = static_cast<int32_t>(state_);
+ if (vision_time_adjuster_.valid()) {
+ turret_status->vision_angle = vision_time_adjuster_.goal();
+ turret_status->raw_vision_angle =
+ vision_time_adjuster_.most_recent_vision_reading();
+ turret_status->vision_tracking = true;
+ } else {
+ turret_status->vision_angle = ::std::numeric_limits<double>::quiet_NaN();
+ turret_status->vision_tracking = false;
+ }
profiled_subsystem_.PopulateIndexerStatus(indexer_status);
indexer_status->state = static_cast<int32_t>(indexer_state_);
diff --git a/y2017/control_loops/superstructure/column/column.h b/y2017/control_loops/superstructure/column/column.h
index 6bd7aa5..b952a78 100644
--- a/y2017/control_loops/superstructure/column/column.h
+++ b/y2017/control_loops/superstructure/column/column.h
@@ -15,6 +15,7 @@
#include "y2017/control_loops/superstructure/column/column_zeroing.h"
#include "y2017/control_loops/superstructure/intake/intake.h"
#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/vision_time_adjuster.h"
namespace y2017 {
namespace control_loops {
@@ -176,7 +177,7 @@
static constexpr double kIntakeZeroingMinDistance = 0.08;
static constexpr double kIntakeTolerance = 0.005;
static constexpr double kStuckZeroingTrackingError = 0.02;
- static constexpr double kTurretNearZero = 0.1;
+ static constexpr double kTurretNearZero = M_PI / 2.0;
void Iterate(const control_loops::IndexerGoal *unsafe_indexer_goal,
const control_loops::TurretGoal *unsafe_turret_goal,
@@ -213,6 +214,8 @@
bool freeze_ = false;
+ VisionTimeAdjuster vision_time_adjuster_;
+
// The last time that we transitioned from RUNNING to REVERSING or the
// reverse. Used to implement the timeouts.
::aos::monotonic_clock::time_point last_transition_time_ =
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index 3c44ed3..70508a9 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -39,6 +39,10 @@
// These are from a top view above the robot.
double angle;
+ // If true, ignore the angle and track using vision. If we don't see
+ // anything, we'll use the turret goal above.
+ bool track;
+
// Caps on velocity/acceleration for profiling. 0 for the default.
.frc971.ProfileParameters profile_params;
};
@@ -163,6 +167,10 @@
// State of the estimator.
ColumnEstimatorState estimator_state;
+
+ double raw_vision_angle;
+ double vision_angle;
+ bool vision_tracking;
};
queue_group SuperstructureQueue {