Make new climber fully work
Change-Id: Id7884fdf22a180a4a33d174638c188dc1e01ae48
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
index a167941..6b31bde 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -65,7 +65,7 @@
"potentiometer_offset": {{ -0.2574404033256 + 0.0170793439542 - 0.177097393974999 + 0.3473623911879 - 0.1577}}
},
"disable_extend": false,
- "disable_climber": true
+ "disable_climber": false
},
{% include 'y2024/constants/common.json' %}
}
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index 8fec134..409a2d5 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -100,8 +100,8 @@
"transfer_roller_stator_current_limit": 50,
"drivetrain_supply_current_limit": 50,
"drivetrain_stator_current_limit": 200,
- "climber_supply_current_limit": 30,
- "climber_stator_current_limit": 100,
+ "climber_supply_current_limit": 40,
+ "climber_stator_current_limit": 150,
"extend_supply_current_limit": 30,
"extend_stator_current_limit": 180,
"extend_roller_supply_current_limit": 50,
@@ -248,7 +248,7 @@
"shot_velocity": 0.0
},
"extend_set_points": {
- "trap": 0.40,
+ "trap": 0.46,
"amp": 0.35,
"catapult": 0.017,
"retracted": 0.017
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index b280b2f..6e31a31 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -46,7 +46,7 @@
double *min_extend_position, const double intake_pivot_position,
double *max_intake_pivot_position, double *min_intake_pivot_position,
NoteGoal requested_note_goal, flatbuffers::FlatBufferBuilder *fbb,
- aos::monotonic_clock::time_point monotonic_now) {
+ aos::monotonic_clock::time_point monotonic_now, bool climbing) {
drivetrain_status_fetcher_.Fetch();
// If our current is over the minimum current and our velocity is under our
@@ -152,11 +152,12 @@
// it if we have no goal, or no subsystem goal, or if we are auto-aiming.
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
- *turret_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
- (piece_loaded || state_ == CatapultState::FIRING) &&
- shooter_goal->has_turret_position())
- ? shooter_goal->turret_position()
- : &turret_goal_builder->AsFlatbuffer();
+ *turret_goal =
+ (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
+ (piece_loaded || state_ == CatapultState::FIRING || climbing) &&
+ shooter_goal->has_turret_position())
+ ? shooter_goal->turret_position()
+ : &turret_goal_builder->AsFlatbuffer();
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*altitude_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
index e873629..dc57507 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -107,7 +107,7 @@
double *min_extend_position, const double intake_pivot_position,
double *max_turret_intake_position, double *min_intake_pivot_position,
NoteGoal requested_note_goal, flatbuffers::FlatBufferBuilder *fbb,
- aos::monotonic_clock::time_point monotonic_now);
+ aos::monotonic_clock::time_point monotonic_now, bool climbing);
bool loaded() const { return state_ == CatapultState::LOADED; }
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 6412c27..e22dc80 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -544,6 +544,18 @@
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*extend_goal = &extend_goal_buffer.message();
+ // Ignore climber voltage goal if "disable_climber" is true
+ output_struct.climber_voltage =
+ (!robot_constants_->robot()->disable_climber() && unsafe_goal != nullptr)
+ ? unsafe_goal->climber_goal_voltage()
+ : 0.0;
+
+ if (output) {
+ if (output_struct.climber_voltage != 0.0) {
+ ++climbing_;
+ }
+ }
+
// TODO(max): Change how we handle the collision with the turret and
// intake to be clearer
const flatbuffers::Offset<ShooterStatus> shooter_status_offset =
@@ -563,7 +575,7 @@
&max_extend_position, &min_extend_position,
intake_pivot_.estimated_position(), &max_intake_pivot_position,
&min_intake_pivot_position, requested_note_goal_, status->fbb(),
- timestamp);
+ timestamp, climbing_ > 50);
intake_pivot_.set_min_position(min_intake_pivot_position);
intake_pivot_.set_max_position(max_intake_pivot_position);
@@ -595,12 +607,6 @@
output_struct.extend_voltage = 0.0;
}
- // Ignore climber voltage goal if "disable_climber" is true
- output_struct.climber_voltage =
- (!robot_constants_->robot()->disable_climber() && unsafe_goal != nullptr)
- ? unsafe_goal->climber_goal_voltage()
- : 0.0;
-
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index 6286615..06cec41 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -69,6 +69,8 @@
NoteGoal requested_note_goal_ = NoteGoal::NONE;
+ int climbing_ = 0;
+
// True if the transfer beambreak has been triggered since last intake request
bool note_in_transfer_ = false;
diff --git a/y2024/joystick_reader.cc b/y2024/joystick_reader.cc
index 5b2fc74..fcece24 100644
--- a/y2024/joystick_reader.cc
+++ b/y2024/joystick_reader.cc
@@ -94,7 +94,7 @@
superstructure_goal_builder =
superstructure_goal_sender_.MakeStaticBuilder();
- if (data.IsPressed(kIntake)) {
+ if (data.IsPressed(kIntake) || climbed_count_ > 25) {
// Intake is pressed
superstructure_goal_builder->set_intake_pivot(
superstructure::IntakePivotGoal::DOWN);
@@ -163,9 +163,16 @@
data.IsPressed(kDriverFire));
if (data.IsPressed(kRaiseClimber)) {
+ ++climbed_count_;
superstructure_goal_builder->set_climber_goal_voltage(4.0);
} else if (data.IsPressed(kRaiseFastClimber)) {
- superstructure_goal_builder->set_climber_goal_voltage(6.0);
+ ++climbed_count_;
+ superstructure_goal_builder->set_climber_goal_voltage(12.0);
+ }
+
+ if (climbed_count_ > 25) {
+ PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ shooter_goal->add_turret_position(), 0.0);
}
superstructure_goal_builder.CheckOk(superstructure_goal_builder.Send());
@@ -177,6 +184,8 @@
::aos::Fetcher<control_loops::superstructure::Status>
superstructure_status_fetcher_;
const y2024::Constants *robot_constants_;
+
+ int climbed_count_ = 0;
};
} // namespace y2024::input::joysticks
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index 0abedec..7a94595 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -581,7 +581,7 @@
current_limits->turret_stator_current_limit(),
current_limits->turret_supply_current_limit());
std::shared_ptr<TalonFX> climber = std::make_shared<TalonFX>(
- 7, true, "rio", &rio_signal_registry,
+ 7, false, "rio", &rio_signal_registry,
current_limits->climber_stator_current_limit(),
current_limits->climber_supply_current_limit());
std::shared_ptr<TalonFX> extend =