Austin's indexer changes (reset, I think?)
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index c7400d8..82e8595 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -63,6 +63,25 @@
.goal_state(4).Send();
}
+void ResetIndex() {
+ LOG(INFO, "Resetting index\n");
+ control_loops::index_loop.goal.MakeWithBuilder()
+ .goal_state(5).Send();
+}
+
+void WaitForIndexReset() {
+ LOG(INFO, "Waiting for the indexer to reset\n");
+ control_loops::index_loop.status.FetchLatest();
+
+ // Fetch a couple index status packets to make sure that the indexer has run.
+ for (int i = 0; i < 5; ++i) {
+ LOG(DEBUG, "Fetching another index status packet\n");
+ control_loops::index_loop.status.FetchNextBlocking();
+ if (ShouldExitAuto()) return;
+ }
+ LOG(INFO, "Indexer is now reset.\n");
+}
+
void WaitForWrist() {
LOG(INFO, "Waiting for the wrist\n");
control_loops::wrist.status.FetchLatest();
@@ -222,10 +241,13 @@
const double ANGLE_TWO = 0.685;
StopDrivetrain();
+ ResetIndex();
SetWristGoal(WRIST_UP); // wrist must calibrate itself on power-up
SetAngle_AdjustGoal(ANGLE_ONE);
SetShooterVelocity(405.0);
+ WaitForIndexReset();
+
PreloadIndex(); // spin to top and put 1 disc into loader
if (ShouldExitAuto()) return;
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index d3ef652..bdcb8a3 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -237,7 +237,7 @@
Time now = Time::Now();
// Make goal easy to work with and sanity check it.
Goal goal_enum = static_cast<Goal>(goal->goal_state);
- if (goal->goal_state < 0 || goal->goal_state > 4) {
+ if (goal->goal_state < 0 || goal->goal_state > 5) {
LOG(ERROR, "Goal state is %"PRId32" which is out of range. Going to HOLD.\n",
goal->goal_state);
goal_enum = Goal::HOLD;
@@ -638,6 +638,9 @@
}
LOG(DEBUG, "INTAKE\n");
break;
+ case Goal::REINITIALIZE:
+ LOG(WARNING, "Reinitializing the indexer\n");
+ break;
case Goal::READY_SHOOTER:
case Goal::SHOOT:
// Don't let us leave the shoot or preload state if there are 4 discs in
@@ -799,6 +802,7 @@
break;
case Goal::READY_SHOOTER:
case Goal::SHOOT:
+ case Goal::REINITIALIZE:
break;
}
}
@@ -949,6 +953,26 @@
last_top_disc_negedge_count_ = position->top_disc_negedge_count;
}
+ // Clear everything if we are supposed to re-initialize.
+ if (goal_enum == Goal::REINITIALIZE) {
+ safe_goal_ = Goal::REINITIALIZE;
+ no_prior_position_ = true;
+ hopper_disc_count_ = 0;
+ total_disc_count_ = 0;
+ shot_disc_count_ = 0;
+ loader_state_ = LoaderState::READY;
+ loader_goal_ = LoaderGoal::READY;
+ loader_countdown_ = 0;
+ loader_up_ = false;
+ disc_clamped_ = false;
+ disc_ejected_ = false;
+
+ intake_voltage = 0.0;
+ transfer_voltage = 0.0;
+ wrist_loop_->U(0, 0) = 0.0;
+ frisbees_.clear();
+ }
+
status->hopper_disc_count = hopper_disc_count_;
status->total_disc_count = total_disc_count_;
status->shot_disc_count = shot_disc_count_;
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index 62a582c..a18d4c9 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -262,7 +262,9 @@
// Get ready to shoot, and place a disc in the loader.
READY_SHOOTER = 3,
// Shoot at will.
- SHOOT = 4
+ SHOOT = 4,
+ // Reinitialize.
+ REINITIALIZE = 5
};
// These two enums command and track the loader loading discs into the
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
index 61e7a61..b125e38 100644
--- a/frc971/control_loops/index/index_motor.q
+++ b/frc971/control_loops/index/index_motor.q
@@ -12,6 +12,7 @@
// 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.
+ // 5 means re-initialize
int32_t goal_state;
};