Require the robot to see the platform before retracting the stilts
If you hit the level 2 button, it will force retract them.
Change-Id: I8d16cd3c06769043c7a5fd50de50aa1d6ac2e3d8
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index d5c8a73..f78bd5a 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -72,6 +72,12 @@
// Position of the stilts, 0 when retracted (defualt), positive lifts robot.
.frc971.PotAndAbsolutePosition stilts;
+
+ // True if the platform detection sensors detect the platform directly
+ // below the robot right behind the left and right wheels. Useful for
+ // determining when the robot is all the way on the platform.
+ bool platform_left_detect;
+ bool platform_right_detect;
};
message Output {
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index a82079b..d024d3f 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -158,7 +158,7 @@
superstructure_queue.status.FetchLatest();
if (!superstructure_queue.status.get() ||
!superstructure_queue.position.get()) {
- LOG(ERROR, "Got no superstructure status packet.\n");
+ LOG(ERROR, "Got no superstructure status or position packet.\n");
return;
}
@@ -342,18 +342,18 @@
constexpr double kDeployStiltPosition = 0.5;
- // TODO(sabina): max height please?
if (data.IsPressed(kFallOver)) {
new_superstructure_goal->stilts.unsafe_goal = 0.71;
new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
new_superstructure_goal->stilts.profile_params.max_acceleration = 1.25;
- } else if (data.IsPressed(kDeployStilt)) {
+ } else if (data.IsPressed(kHalfStilt)) {
+ was_above_ = false;
+ new_superstructure_goal->stilts.unsafe_goal = 0.345;
+ new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
+ } else if (data.IsPressed(kDeployStilt) || was_above_) {
new_superstructure_goal->stilts.unsafe_goal = kDeployStiltPosition;
new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
- } else if (data.IsPressed(kHalfStilt)) {
- new_superstructure_goal->stilts.unsafe_goal = 0.345;
- new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
} else {
new_superstructure_goal->stilts.unsafe_goal = 0.005;
new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
@@ -366,6 +366,18 @@
new_superstructure_goal->elevator.profile_params.max_acceleration = 6;
}
+ // If we've been asked to go above deploy and made it up that high, latch
+ // was_above.
+ if (new_superstructure_goal->stilts.unsafe_goal > kDeployStiltPosition &&
+ superstructure_queue.status->stilts.position >= kDeployStiltPosition) {
+ was_above_ = true;
+ } else if ((superstructure_queue.position->platform_left_detect ||
+ superstructure_queue.position->platform_right_detect) &&
+ !data.IsPressed(kDeployStilt) && !data.IsPressed(kFallOver)) {
+ // TODO(austin): Should make this && rather than ||
+ was_above_ = false;
+ }
+
if (superstructure_queue.status->stilts.position > kDeployStiltPosition &&
new_superstructure_goal->stilts.unsafe_goal == kDeployStiltPosition) {
new_superstructure_goal->stilts.profile_params.max_velocity = 0.30;
@@ -419,6 +431,10 @@
return ::frc971::autonomous::auto_mode->mode;
}
+ // Bool to track if we've been above the deploy position. Once this bool is
+ // set, don't let the stilts retract until we see the platform.
+ bool was_above_ = false;
+
// Current goals here.
ElevatorWristPosition elevator_wrist_pos_ = kStowPos;
bool grab_piece_ = false;
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 95ed28e..17dabde 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -198,6 +198,16 @@
stilts_encoder_.set_potentiometer(::std::move(potentiometer));
}
+ void set_platform_left_detect(
+ ::std::unique_ptr<frc::DigitalInput> platform_left_detect) {
+ platform_left_detect_ = ::std::move(platform_left_detect);
+ }
+
+ void set_platform_right_detect(
+ ::std::unique_ptr<frc::DigitalInput> platform_right_detect) {
+ platform_right_detect_ = ::std::move(platform_right_detect);
+ }
+
// Vacuum pressure sensor
void set_vacuum_sensor(int port) {
vacuum_sensor_ = make_unique<frc::AnalogInput>(port);
@@ -257,6 +267,11 @@
(vacuum_sensor_->GetVoltage() - kMinVoltage) /
(kMaxVoltage - kMinVoltage);
+ superstructure_message->platform_left_detect =
+ !platform_left_detect_->Get();
+ superstructure_message->platform_right_detect =
+ !platform_right_detect_->Get();
+
superstructure_message.Send();
}
@@ -277,6 +292,9 @@
::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
wrist_encoder_, stilts_encoder_;
+ ::std::unique_ptr<frc::DigitalInput> platform_left_detect_;
+ ::std::unique_ptr<frc::DigitalInput> platform_right_detect_;
+
::std::unique_ptr<frc::AnalogInput> vacuum_sensor_;
::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
@@ -668,6 +686,9 @@
reader.set_pwm_trigger(true);
reader.set_vacuum_sensor(7);
+ reader.set_platform_right_detect(make_unique<frc::DigitalInput>(6));
+ reader.set_platform_left_detect(make_unique<frc::DigitalInput>(7));
+
reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(22));
reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(23));