added the bottom sensor too
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 3932ab5..4ec77ba 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -109,7 +109,8 @@
/*static*/ const int IndexMotor::kLiftingDelay = 2;
/*static*/ const int IndexMotor::kLiftingTimeout = 65;
/*static*/ const int IndexMotor::kShootingDelay = 10;
-/*static*/ const int IndexMotor::kLoweringDelay = 20;
+/*static*/ const int IndexMotor::kLoweringDelay = 4;
+/*static*/ const int IndexMotor::kLoweringTimeout = 120;
// TODO(aschuh): Tune these.
/*static*/ const double
@@ -900,6 +901,8 @@
if (position->loader_top) {
if (loader_countdown_ > 0) {
--loader_countdown_;
+ loader_timeout_ = 0;
+ break;
} else {
loader_state_ = LoaderState::LIFTED;
}
@@ -910,9 +913,10 @@
if (loader_timeout_ > kLiftingTimeout) {
LOG(ERROR, "Loader timeout while LIFTING %d\n", loader_timeout_);
loader_state_ = LoaderState::LIFTED;
+ } else {
+ break;
}
}
- break;
case LoaderState::LIFTED:
LOG(DEBUG, "Loader LIFTED\n");
// Disc lifted. Time to eject it out.
@@ -941,6 +945,7 @@
disc_ejected_ = true;
loader_state_ = LoaderState::LOWERING;
loader_countdown_ = kLoweringDelay;
+ loader_timeout_ = 0;
--hopper_disc_count_;
++shot_disc_count_;
case LoaderState::LOWERING:
@@ -949,11 +954,24 @@
loader_up_ = false;
disc_clamped_ = false;
disc_ejected_ = true;
- if (loader_countdown_ > 0) {
- --loader_countdown_;
- break;
+ if (position->loader_bottom) {
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ loader_timeout_ = 0;
+ break;
+ } else {
+ loader_state_ = LoaderState::LOWERED;
+ }
} else {
- loader_state_ = LoaderState::LOWERED;
+ // Restart the countdown if it bounces back up or something.
+ loader_countdown_ = kLoweringDelay;
+ ++loader_timeout_;
+ if (loader_timeout_ > kLoweringTimeout) {
+ LOG(ERROR, "Loader timeout while LOWERING %d\n", loader_timeout_);
+ loader_state_ = LoaderState::LOWERED;
+ } else {
+ break;
+ }
}
case LoaderState::LOWERED:
LOG(DEBUG, "Loader LOWERED\n");
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index 8abb119..7c68a7f 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -143,8 +143,12 @@
static const int kLiftingTimeout;
// Time that it takes to shoot the disc in cycles.
static const int kShootingDelay;
- // Time that it takes to lower the loader in cycles.
+ // Time that it takes to finish lowering the loader after the sensor is
+ // triggered in cycles.
static const int kLoweringDelay;
+ // Time until we give up lowering and move on in cycles.
+ // It's a long time because we really don't want to ever hit this.
+ static const int kLoweringTimeout;
// Object representing a Frisbee tracked by the indexer.
class Frisbee {
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
index 3956b18..f8fe449 100644
--- a/frc971/control_loops/index/index_motor.q
+++ b/frc971/control_loops/index/index_motor.q
@@ -45,8 +45,10 @@
int32_t top_disc_negedge_count;
double top_disc_negedge_position;
- // Whether the hall effect for the loader being at the top is triggered.
+ // Whether the hall effects for the loader are triggered (have a magnet in
+ // front of them).
bool loader_top;
+ bool loader_bottom;
};
message Output {
diff --git a/frc971/input/gyro_board_data.h b/frc971/input/gyro_board_data.h
index 3650e96..cc0a4db 100644
--- a/frc971/input/gyro_board_data.h
+++ b/frc971/input/gyro_board_data.h
@@ -42,6 +42,7 @@
uint8_t top_disc : 1;
uint8_t bottom_disc : 1;
uint8_t loader_top : 1;
+ uint8_t loader_bottom : 1;
};
uint32_t digitals;
};
diff --git a/frc971/input/gyro_board_reader.cc b/frc971/input/gyro_board_reader.cc
index de60cb1..4f1f958 100644
--- a/frc971/input/gyro_board_reader.cc
+++ b/frc971/input/gyro_board_reader.cc
@@ -225,6 +225,7 @@
data->capture_bottom_fall_delay))
.bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
.loader_top(data->loader_top)
+ .loader_bottom(data->loader_bottom)
.Send();
}
diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
index 4dec23e..574f5f5 100644
--- a/frc971/input/gyro_sensor_receiver.cc
+++ b/frc971/input/gyro_sensor_receiver.cc
@@ -148,6 +148,8 @@
.bottom_disc_negedge_wait_position(index_translate(
data->capture_bottom_fall_delay))
.bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
+ .loader_top(data->loader_top)
+ .loader_bottom(data->loader_bottom)
.Send();
}
diff --git a/gyro_board/src/usb/analog.c b/gyro_board/src/usb/analog.c
index c9d6752..ccd8854 100644
--- a/gyro_board/src/usb/analog.c
+++ b/gyro_board/src/usb/analog.c
@@ -522,6 +522,7 @@
packet->bottom_disc = !digital(1);
packet->loader_top = !digital(5);
+ packet->loader_bottom = !digital(6);
packet->capture_shooter_angle_rise = capture_shooter_angle_rise;
packet->shooter_angle_rise_count = shooter_angle_rise_count;
diff --git a/gyro_board/src/usb/analog.h b/gyro_board/src/usb/analog.h
index 8dc0963..8763181 100644
--- a/gyro_board/src/usb/analog.h
+++ b/gyro_board/src/usb/analog.h
@@ -39,6 +39,7 @@
uint8_t top_disc : 1;
uint8_t bottom_disc : 1;
uint8_t loader_top : 1;
+ uint8_t loader_bottom : 1;
};
uint32_t digitals;
};