Indexer now has simpler logic for pushing the disc off the sensor which actually works now.
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 8ba9c33..cb41973 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -20,8 +20,7 @@
namespace frc971 {
namespace control_loops {
-double IndexMotor::Frisbee::ObserveNoTopDiscSensor(
- double index_position, double index_velocity) {
+double IndexMotor::Frisbee::ObserveNoTopDiscSensor(double index_position) {
// The absolute disc position in meters.
double disc_position = absolute_position(index_position);
if (IndexMotor::kTopDiscDetectStart <= disc_position &&
@@ -32,36 +31,15 @@
::std::abs(disc_position - IndexMotor::kTopDiscDetectStop));
double distance_to_below = IndexMotor::ConvertDiscPositionToIndex(
::std::abs(disc_position - IndexMotor::kTopDiscDetectStart));
- if (::std::abs(index_velocity) < 100000) {
- if (distance_to_above < distance_to_below) {
- LOG(INFO, "Moving disc to top slow.\n");
- // Move it up.
- index_start_position_ -= distance_to_above;
- return -distance_to_above;
- } else {
- LOG(INFO, "Moving disc to bottom slow.\n");
- index_start_position_ += distance_to_below;
- return distance_to_below;
- }
+ if (distance_to_above < distance_to_below) {
+ LOG(INFO, "Moving disc to top slow.\n");
+ // Move it up.
+ index_start_position_ -= distance_to_above;
+ return -distance_to_above;
} else {
- if (index_velocity > 0) {
- // Now going up. If we didn't see it before, and we don't see it
- // now but it should be in view, it must still be below. If it were
- // above, it would be going further away from us.
- LOG(INFO, "Moving fast up, shifting disc down. Disc was at %f\n",
- absolute_position(index_position));
- index_start_position_ += distance_to_below;
- LOG(INFO, "Moving fast up, shifting disc down. Disc now at %f\n",
- absolute_position(index_position));
- return distance_to_below;
- } else {
- LOG(INFO, "Moving fast down, shifting disc up. Disc was at %f\n",
- absolute_position(index_position));
- index_start_position_ -= distance_to_above;
- LOG(INFO, "Moving fast down, shifting disc up. Disc now at %f\n",
- absolute_position(index_position));
- return -distance_to_above;
- }
+ LOG(INFO, "Moving disc to bottom slow.\n");
+ index_start_position_ += distance_to_below;
+ return distance_to_below;
}
}
return 0.0;
@@ -139,9 +117,6 @@
/*static*/ const double
IndexMotor::IndexStateFeedbackLoop::kNoMotionCuttoffCount = 20;
-// 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));
}
@@ -358,7 +333,7 @@
frisbee != rend; ++frisbee) {
frisbee->OffsetDisc(cumulative_offset);
double amount_moved = frisbee->ObserveNoTopDiscSensor(
- wrist_loop_->X_hat(0, 0), wrist_loop_->X_hat(1, 0));
+ wrist_loop_->X_hat(0, 0));
cumulative_offset += amount_moved;
}
}
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index c9dc86f..f76ea97 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -185,7 +185,7 @@
// currently blocking the top sensor. This knowledge can be used to move
// this disc if it is believed to be blocking the top sensor.
// Returns the amount that the disc moved due to this observation.
- double ObserveNoTopDiscSensor(double index_position, double index_velocity);
+ double ObserveNoTopDiscSensor(double index_position);
// Posedge and negedge disc times.
::aos::time::Time bottom_posedge_time_;