made it move the wrist once the hopper is clear
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index b5c3a52..1261115 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -57,6 +57,7 @@
is_shooting_(false),
last_bottom_disc_detect_(false),
last_top_disc_detect_(false),
+ hopper_clear_(true),
no_prior_position_(true),
missing_position_count_(0) {
}
@@ -535,6 +536,7 @@
break;
case Goal::READY_LOWER:
case Goal::INTAKE:
+ hopper_clear_ = false;
{
if (position) {
// Posedge of the disc entering the beam break.
@@ -760,6 +762,7 @@
"Emptied the hopper out but there are still discs there\n");
hopper_disc_count_ = 0;
}
+ hopper_clear_ = true;
}
}
}
@@ -792,6 +795,7 @@
hopper_disc_count_);
hopper_disc_count_ = 0;
}
+ hopper_clear_ = true;
}
}
@@ -1036,6 +1040,7 @@
status->shot_disc_count = shot_disc_count_;
status->preloaded = (loader_state_ != LoaderState::READY);
status->is_shooting = is_shooting_;
+ status->hopper_clear = hopper_clear_;
if (output) {
output->intake_voltage = intake_voltage;
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index 667aa79..0be4801 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -334,6 +334,7 @@
// Bottom disc detect from the last valid packet for detecting edges.
bool last_bottom_disc_detect_;
bool last_top_disc_detect_;
+ bool hopper_clear_;
int32_t last_bottom_disc_posedge_count_;
int32_t last_bottom_disc_negedge_count_;
int32_t last_bottom_disc_negedge_wait_count_;
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
index b12e31c..b299b42 100644
--- a/frc971/control_loops/index/index_motor.q
+++ b/frc971/control_loops/index/index_motor.q
@@ -80,6 +80,9 @@
// True from when we're committed to shooting util after the disk is clear
// of the robot.
bool is_shooting;
+ // Goes false when we first get a disk and back true after we finish
+ // clearing.
+ bool hopper_clear;
};
queue Goal goal;