Make climber buttons work
Change-Id: Ia6d83928c0dbaa557fc02050bcdaf6b3b2d2d764
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 6ee38ad..100d90c 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -73,7 +73,6 @@
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
- ":climber",
":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
@@ -135,24 +134,6 @@
],
)
-cc_library(
- name = "climber",
- srcs = [
- "climber.cc",
- ],
- hdrs = [
- "climber.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":superstructure_goal_fbs",
- ":superstructure_output_fbs",
- "//frc971/control_loops:control_loop",
- "//frc971/control_loops:control_loops_fbs",
- "//frc971/control_loops:profiled_subsystem_fbs",
- ],
-)
-
ts_library(
name = "turret_plotter",
srcs = ["turret_plotter.ts"],
diff --git a/y2020/control_loops/superstructure/climber.cc b/y2020/control_loops/superstructure/climber.cc
deleted file mode 100644
index bb449da..0000000
--- a/y2020/control_loops/superstructure/climber.cc
+++ /dev/null
@@ -1,23 +0,0 @@
-#include "y2020/control_loops/superstructure/climber.h"
-
-#include <algorithm>
-
-#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
-
-namespace y2020 {
-namespace control_loops {
-namespace superstructure {
-
-void Climber::Iterate(const Goal *unsafe_goal, OutputT *output) {
- if (unsafe_goal && output) {
- // Pass through the voltage request from the user. Backwards isn't
- // supported, so prevent that.
- output->climber_voltage =
- std::clamp(unsafe_goal->climber_voltage(), 0.0f, 12.0f);
- }
-}
-
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2020
diff --git a/y2020/control_loops/superstructure/climber.h b/y2020/control_loops/superstructure/climber.h
deleted file mode 100644
index 29b7e51..0000000
--- a/y2020/control_loops/superstructure/climber.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef Y2020_CONTROL_LOOPS_SUPERSTRUCTURE_CLIMBER_H_
-#define Y2020_CONTROL_LOOPS_SUPERSTRUCTURE_CLIMBER_H_
-
-#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
-
-namespace y2020 {
-namespace control_loops {
-namespace superstructure {
-
-// Class to encapsulate the climber logic.
-class Climber {
- public:
- void Iterate(const Goal *unsafe_goal, OutputT *output);
-};
-
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2020
-
-#endif // Y2020_CONTROL_LOOPS_SUPERSTRUCTURE_CLIMBER_H_
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 1a61a29..86dcbb1 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -158,8 +158,6 @@
position->shooter(), status->fbb(),
output != nullptr ? &(output_struct) : nullptr, position_timestamp);
- climber_.Iterate(unsafe_goal, output != nullptr ? &(output_struct) : nullptr);
-
const AbsoluteAndAbsoluteEncoderProfiledJointStatus *const hood_status =
GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
@@ -237,6 +235,9 @@
output_struct.feeder_voltage = 0.0;
output_struct.intake_roller_voltage = 0.0;
if (unsafe_goal) {
+ output_struct.climber_voltage =
+ std::clamp(unsafe_goal->climber_voltage(), -12.0f, 12.0f);
+
if (unsafe_goal->shooting() || unsafe_goal->intake_preloading()) {
preloading_timeout_ = position_timestamp + kPreloadingTimeout;
}
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index b0bc9f5..8f3eb0a 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -6,7 +6,6 @@
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/input/joystick_state_generated.h"
#include "y2020/constants.h"
-#include "y2020/control_loops/superstructure/climber.h"
#include "y2020/control_loops/superstructure/shooter/shooter.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2020/control_loops/superstructure/superstructure_output_generated.h"
@@ -70,8 +69,6 @@
drivetrain_status_fetcher_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
- Climber climber_;
-
aos::monotonic_clock::time_point shooting_start_time_ =
aos::monotonic_clock::min_time;
aos::monotonic_clock::time_point preloading_timeout_ =
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 2b8b39a..b34273c 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -952,8 +952,8 @@
// Give it time to stabilize.
RunFor(chrono::seconds(1));
- // Can't go backwards.
- EXPECT_EQ(superstructure_plant_.climber_voltage(), 0.0);
+ // Can go backwards.
+ EXPECT_EQ(superstructure_plant_.climber_voltage(), -10.0);
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -965,7 +965,8 @@
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(1));
- // But forwards works.
+
+ // And forwards too.
EXPECT_EQ(superstructure_plant_.climber_voltage(), 10.0);
VerifyNearGoal();
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 9c8bae5..1b061e5 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -42,7 +42,7 @@
const ButtonLocation kShootFast(3, 16);
const ButtonLocation kAutoTrack(3, 3);
const ButtonLocation kAutoNoHood(3, 5);
-const ButtonLocation kHood(3, 4);
+const ButtonLocation kHood(3, 2);
const ButtonLocation kShootSlow(4, 2);
const ButtonLocation kFixedTurret(3, 1);
const ButtonLocation kFeed(4, 1);
@@ -56,7 +56,8 @@
const ButtonLocation kLocalizerReset(3, 8);
const ButtonLocation kIntakeSlightlyOut(3, 7);
-const ButtonLocation kWinch(3, 14);
+const ButtonLocation kWinch(3, 4);
+const ButtonLocation kUnWinch(3, 6);
class Reader : public ::frc971::input::ActionJoystickInput {
public:
@@ -138,6 +139,8 @@
}
}
+ bool latched_climbing_ = false;
+
void HandleTeleop(
const ::frc971::input::driver_station::Data &data) override {
superstructure_status_fetcher_.Fetch();
@@ -157,6 +160,7 @@
double finisher_speed = 0.0;
double climber_speed = 0.0;
bool preload_intake = false;
+ bool turret_tracking = false;
const bool auto_track = data.IsPressed(kAutoTrack);
@@ -176,6 +180,10 @@
turret_pos = 0.0;
}
+ if (!data.IsPressed(kFixedTurret)) {
+ turret_tracking = true;
+ }
+
if (data.IsPressed(kAutoNoHood)) {
if (setpoint_fetcher_.get()) {
accelerator_speed = setpoint_fetcher_->accelerator();
@@ -222,6 +230,21 @@
if (data.IsPressed(kWinch)) {
climber_speed = 12.0f;
+ latched_climbing_ = true;
+ }
+
+ if (data.IsPressed(kUnWinch)) {
+ climber_speed = -12.0f;
+ latched_climbing_ = true;
+ }
+
+ if (data.IsPressed(kWinch) && data.IsPressed(kUnWinch)) {
+ latched_climbing_ = false;
+ }
+
+ if (latched_climbing_) {
+ turret_tracking = false;
+ turret_pos = -M_PI / 2.0;
}
if (data.PosEdge(kLocalizerReset)) {
@@ -272,8 +295,7 @@
data.IsPressed(kFeedDriver));
superstructure_goal_builder.add_climber_voltage(climber_speed);
- superstructure_goal_builder.add_turret_tracking(
- !data.IsPressed(kFixedTurret));
+ superstructure_goal_builder.add_turret_tracking(turret_tracking);
superstructure_goal_builder.add_hood_tracking(
!data.IsPressed(kFixedTurret) && !data.IsPressed(kAutoNoHood));
superstructure_goal_builder.add_shooter_tracking(
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index 9cc0c7d..67afb1d 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -512,7 +512,7 @@
if (climber_falcon_) {
climber_falcon_->Set(
ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- std::clamp(-output.climber_voltage(), -kMaxBringupPower,
+ std::clamp(output.climber_voltage(), -kMaxBringupPower,
kMaxBringupPower) /
12.0);
}
@@ -625,8 +625,7 @@
std::unique_ptr<frc971::wpilib::ADIS16448> old_imu;
std::unique_ptr<frc971::wpilib::ADIS16470> new_imu;
std::unique_ptr<frc::SPI> imu_spi;
- if (::aos::network::GetTeamNumber() ==
- constants::Values::kCompTeamNumber) {
+ if (::aos::network::GetTeamNumber() == constants::Values::kCompTeamNumber) {
old_imu = make_unique<frc971::wpilib::ADIS16448>(
&imu_event_loop, spi_port, imu_trigger.get());
old_imu->SetDummySPI(frc::SPI::Port::kOnboardCS2);