No more range based loops. A very sad day.
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index fa1940e..98d51b8 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -151,8 +151,10 @@
// Disable the motors now so that all early returns will return with the
// motors disabled.
+ double intake_voltage = 0.0;
double transfer_voltage = 0.0;
if (output) {
+ output->intake_voltage = 0.0;
output->transfer_voltage = 0.0;
output->index_voltage = 0.0;
}
@@ -204,7 +206,7 @@
}
if (position->bottom_disc_detect) {
- transfer_voltage = 12.0;
+ intake_voltage = transfer_voltage = 12.0;
// Must wait until the disc gets out before we can change state.
safe_to_change_state_ = false;
@@ -221,11 +223,12 @@
}
// Check all non-indexed discs and see if they should be indexed.
- for (Frisbee &frisbee : frisbees_) {
- if (!frisbee.has_been_indexed_) {
- transfer_voltage = 12.0;
+ for (auto frisbee = frisbees_.begin();
+ frisbee != frisbees_.end(); ++frisbee) {
+ if (!frisbee->has_been_indexed_) {
+ intake_voltage = transfer_voltage = 12.0;
Time elapsed_negedge_time = now -
- frisbee.bottom_negedge_time_;
+ frisbee->bottom_negedge_time_;
if (elapsed_negedge_time >= Time::InSeconds(0.005)) {
// Should have just engaged.
// Save the indexer position, and the time.
@@ -234,11 +237,11 @@
// Treat now as the time at which it contacted the indexer.
LOG(INFO, "Grabbed on the index now at %f\n", index_position);
printf("Grabbed on the index now at %f\n", index_position);
- frisbee.has_been_indexed_ = true;
- frisbee.index_start_position_ = index_position;
+ frisbee->has_been_indexed_ = true;
+ frisbee->index_start_position_ = index_position;
}
}
- if (!frisbee.has_been_indexed_) {
+ if (!frisbee->has_been_indexed_) {
// All discs must be indexed before it is safe to stop indexing.
safe_to_change_state_ = false;
}
@@ -285,7 +288,7 @@
// Turn on the transfer roller if we are ready.
if (status->ready_to_intake && hopper_disc_count_ < 4 &&
safe_goal_ == Goal::INTAKE) {
- transfer_voltage = 12.0;
+ intake_voltage = transfer_voltage = 12.0;
}
}
printf("INTAKE\n");
@@ -482,6 +485,7 @@
status->preloaded = (loader_state_ != LoaderState::READY);
if (output) {
+ output->intake_voltage = intake_voltage;
output->transfer_voltage = transfer_voltage;
output->index_voltage = wrist_loop_->U(0, 0);
output->loader_up = loader_up_;
diff --git a/frc971/control_loops/index/index_lib_test.cc b/frc971/control_loops/index/index_lib_test.cc
index 01805c9..cb834ec 100644
--- a/frc971/control_loops/index/index_lib_test.cc
+++ b/frc971/control_loops/index/index_lib_test.cc
@@ -190,8 +190,9 @@
// Returns true if the bottom disc sensor is triggered.
bool BottomDiscDetect() const {
bool bottom_disc_detect = false;
- for (const Frisbee &frisbee : frisbees) {
- bottom_disc_detect |= frisbee.bottom_disc_detect();
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ bottom_disc_detect |= frisbee->bottom_disc_detect();
}
return bottom_disc_detect;
}
@@ -199,29 +200,32 @@
// Returns true if the top disc sensor is triggered.
bool TopDiscDetect() const {
bool top_disc_detect = false;
- for (const Frisbee &frisbee : frisbees) {
- top_disc_detect |= frisbee.top_disc_detect();
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ top_disc_detect |= frisbee->top_disc_detect();
}
return top_disc_detect;
}
// Updates all discs, and verifies that the state of the system is sane.
void UpdateDiscs(bool clamped, bool lifted, bool ejected) {
- for (Frisbee &frisbee : frisbees) {
- frisbee.UpdatePosition(transfer_roller_position(),
- index_roller_position(),
- clamped,
- lifted,
- ejected);
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ frisbee->UpdatePosition(transfer_roller_position(),
+ index_roller_position(),
+ clamped,
+ lifted,
+ ejected);
}
// Make sure nobody is too close to anybody else.
Frisbee *last_frisbee = NULL;
- for (Frisbee &frisbee : frisbees) {
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
if (last_frisbee) {
- const double distance = frisbee.position() - last_frisbee->position();
+ const double distance = frisbee->position() - last_frisbee->position();
double min_distance;
- if (frisbee.IsTouchingTransfer() ||
+ if (frisbee->IsTouchingTransfer() ||
last_frisbee->IsTouchingTransfer()) {
min_distance = 0.3;
} else {
@@ -231,7 +235,7 @@
EXPECT_LT(min_distance, ::std::abs(distance)) << "Discs too close";
}
- last_frisbee = &frisbee;
+ last_frisbee = &*frisbee;
}
// Remove any shot frisbees.
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
index 35a2e2a..1ff9ab4 100644
--- a/frc971/control_loops/index/index_motor.q
+++ b/frc971/control_loops/index/index_motor.q
@@ -23,6 +23,9 @@
};
message Output {
+ // Intake roller(s) output voltage.
+ // Positive means into the robot.
+ double intake_voltage;
// Transfer roller output voltage.
// Positive means into the robot.
double transfer_voltage;