Turret safe position is now sideways.
Change-Id: Id98f41c141920d3d6e73b726c1390e04981f9e78
diff --git a/y2017/control_loops/superstructure/column/column.h b/y2017/control_loops/superstructure/column/column.h
index 6bfe1c9..73da9b9 100644
--- a/y2017/control_loops/superstructure/column/column.h
+++ b/y2017/control_loops/superstructure/column/column.h
@@ -178,7 +178,8 @@
static constexpr double kIntakeZeroingMinDistance = 0.08;
static constexpr double kIntakeTolerance = 0.005;
static constexpr double kStuckZeroingTrackingError = 0.02;
- static constexpr double kTurretNearZero = M_PI / 2.0;
+ static constexpr double kTurretMin = -0.1;
+ static constexpr double kTurretMax = M_PI / 2.0 + 0.1;
void Iterate(const control_loops::IndexerGoal *unsafe_indexer_goal,
const control_loops::TurretGoal *unsafe_turret_goal,
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index ee7b80b..344c97b 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -18,6 +18,7 @@
// The maximum voltage the intake roller will be allowed to use.
constexpr double kMaxIntakeRollerVoltage = 12.0;
constexpr double kMaxIndexerRollerVoltage = 12.0;
+constexpr double kTurretTuckAngle = M_PI / 2.0;
} // namespace
typedef ::y2017::constants::Values::ShotParams ShotParams;
@@ -130,10 +131,14 @@
if (!ignore_collisions_) {
// The turret is in a position (or wants to be in a position) where we
// need the intake out. Push it out.
- if (::std::abs(unsafe_goal->turret.angle) >
- column::Column::kTurretNearZero ||
- ::std::abs(column_.turret_position()) >
- column::Column::kTurretNearZero) {
+ const bool column_goal_not_safe =
+ unsafe_goal->turret.angle > column::Column::kTurretMax ||
+ unsafe_goal->turret.angle < column::Column::kTurretMin;
+ const bool column_position_not_safe =
+ column_.turret_position() > column::Column::kTurretMax ||
+ column_.turret_position() < column::Column::kTurretMin;
+
+ if (column_goal_not_safe || column_position_not_safe) {
intake_.set_min_position(column::Column::kIntakeZeroingMinDistance);
} else {
intake_.clear_min_position();
@@ -141,8 +146,7 @@
// The intake is in a position where it could hit. Don't move the turret.
if (intake_.position() < column::Column::kIntakeZeroingMinDistance -
column::Column::kIntakeTolerance &&
- ::std::abs(column_.turret_position()) >
- column::Column::kTurretNearZero) {
+ column_position_not_safe) {
column_.set_freeze(true);
} else {
column_.set_freeze(false);