Add back catapult beambreak logic
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I9cf032eadf2b64d9e21352568063d9f7621119db
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index a0dd234..72ad0d6 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -10,18 +10,12 @@
using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
-constexpr double kMinCurrent = 20.0;
-constexpr double kMaxVelocity = 1.0;
constexpr double kCatapultActivationThreshold = 0.01;
Shooter::Shooter(aos::EventLoop *event_loop, const Constants *robot_constants)
: drivetrain_status_fetcher_(
event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
- superstructure_can_position_fetcher_(
- event_loop
- ->MakeFetcher<y2024::control_loops::superstructure::CANPosition>(
- "/superstructure/rio")),
robot_constants_(robot_constants),
catapult_(
robot_constants->common()->catapult(),
@@ -43,34 +37,18 @@
const y2024::control_loops::superstructure::ShooterGoal *shooter_goal,
double *catapult_output, double *altitude_output, double *turret_output,
double *retention_roller_output, double /*battery_voltage*/,
- aos::monotonic_clock::time_point current_timestamp,
CollisionAvoidance *collision_avoidance, const double intake_pivot_position,
double *max_intake_pivot_position, double *min_intake_pivot_position,
flatbuffers::FlatBufferBuilder *fbb) {
- superstructure_can_position_fetcher_.Fetch();
drivetrain_status_fetcher_.Fetch();
- CHECK(superstructure_can_position_fetcher_.get() != nullptr);
-
- double current_retention_position =
- superstructure_can_position_fetcher_->retention_roller()->position();
-
- double torque_current =
- superstructure_can_position_fetcher_->retention_roller()
- ->torque_current();
-
- double retention_velocity =
- (current_retention_position - last_retention_position_) /
- std::chrono::duration<double>(current_timestamp - last_timestamp_)
- .count();
// If our current is over the minimum current and our velocity is under our
// maximum velocity, then set loaded to true. If we are preloaded set it to
// true as well.
//
// TODO(austin): Debounce piece_loaded?
- bool piece_loaded =
- (torque_current > kMinCurrent && retention_velocity < kMaxVelocity) ||
- (shooter_goal != nullptr && shooter_goal->preloaded());
+ bool piece_loaded = position->catapult_beambreak() ||
+ (shooter_goal != nullptr && shooter_goal->preloaded());
aos::fbs::FixedStackAllocator<aos::fbs::Builder<
frc971::control_loops::
@@ -289,8 +267,6 @@
status_builder.add_aimer(aimer_offset);
}
- last_retention_position_ = current_retention_position;
- last_timestamp_ = current_timestamp;
return status_builder.Finish();
}
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
index 048e4b2..521fa04 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -67,7 +67,6 @@
const Position *position, const ShooterGoal *shooter_goal,
double *catapult_output, double *altitude_output, double *turret_output,
double *retention_roller_output, double battery_voltage,
- aos::monotonic_clock::time_point current_timestamp,
/* Hacky way to use collision avoidance in this class */
CollisionAvoidance *collision_avoidance,
const double intake_pivot_position, double *max_turret_intake_position,
@@ -100,11 +99,6 @@
frc971::shooter_interpolation::InterpolationTable<
y2024::constants::Values::ShotParams>
interpolation_table_;
-
- double last_retention_position_;
-
- aos::monotonic_clock::time_point last_timestamp_{
- aos::monotonic_clock::min_time};
};
} // namespace y2024::control_loops::superstructure
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index ebcbf1b..29b4cc9 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -47,6 +47,8 @@
const monotonic_clock::time_point timestamp =
event_loop()->context().monotonic_event_time;
+ (void)timestamp;
+
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
intake_pivot_.Reset();
@@ -191,7 +193,7 @@
output != nullptr ? &output_struct.altitude_voltage : nullptr,
output != nullptr ? &output_struct.turret_voltage : nullptr,
output != nullptr ? &output_struct.retention_roller_voltage : nullptr,
- robot_state().voltage_battery(), timestamp, &collision_avoidance_,
+ robot_state().voltage_battery(), &collision_avoidance_,
intake_pivot_.estimated_position(), &max_intake_pivot_position,
&min_intake_pivot_position, status->fbb());
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index eeeb731..701690b 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -64,6 +64,7 @@
superstructure_output_fetcher_(
event_loop_->MakeFetcher<Output>("/superstructure")),
transfer_beambreak_(false),
+ catapult_beambreak_(false),
intake_pivot_(
new CappedTestPlant(intake_pivot::MakeIntakePivotPlant()),
PositionSensorSimulator(simulated_robot_constants->robot()
@@ -205,7 +206,6 @@
}
first_ = false;
SendPositionMessage();
- SendCANPositionMessage();
},
dt);
}
@@ -244,6 +244,7 @@
Position::Builder position_builder = builder.MakeBuilder<Position>();
position_builder.add_transfer_beambreak(transfer_beambreak_);
+ position_builder.add_catapult_beambreak(catapult_beambreak_);
position_builder.add_intake_pivot(intake_pivot_offset);
position_builder.add_catapult(catapult_offset);
position_builder.add_altitude(altitude_offset);
@@ -254,41 +255,12 @@
aos::RawSender::Error::kOk);
}
- void SendCANPositionMessage() {
- retention_position_ =
- retention_velocity_ * std::chrono::duration<double>(dt_).count() +
- retention_position_;
-
- auto builder = superstructure_can_position_sender_.MakeBuilder();
-
- frc971::control_loops::CANTalonFX::Builder retention_builder =
- builder.MakeBuilder<frc971::control_loops::CANTalonFX>();
-
- retention_builder.add_torque_current(retention_torque_current_);
- retention_builder.add_position(retention_position_);
-
- flatbuffers::Offset<frc971::control_loops::CANTalonFX> retention_offset =
- retention_builder.Finish();
-
- CANPosition::Builder can_position_builder =
- builder.MakeBuilder<CANPosition>();
-
- can_position_builder.add_retention_roller(retention_offset);
-
- CHECK_EQ(builder.Send(can_position_builder.Finish()),
- aos::RawSender::Error::kOk);
- }
-
void set_transfer_beambreak(bool triggered) {
transfer_beambreak_ = triggered;
}
- void set_retention_velocity(double velocity) {
- retention_velocity_ = velocity;
- }
-
- void set_retention_torque_current(double torque_current) {
- retention_torque_current_ = torque_current;
+ void set_catapult_beambreak(bool triggered) {
+ catapult_beambreak_ = triggered;
}
AbsoluteEncoderSimulator *intake_pivot() { return &intake_pivot_; }
@@ -308,6 +280,7 @@
::aos::Fetcher<Output> superstructure_output_fetcher_;
bool transfer_beambreak_;
+ bool catapult_beambreak_;
AbsoluteEncoderSimulator intake_pivot_;
PotAndAbsoluteEncoderSimulator climber_;
@@ -316,10 +289,6 @@
PotAndAbsoluteEncoderSimulator turret_;
bool first_ = true;
-
- double retention_velocity_;
- double retention_position_;
- double retention_torque_current_;
};
class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
@@ -970,8 +939,7 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- superstructure_plant_.set_retention_velocity(5);
- superstructure_plant_.set_retention_torque_current(1);
+ superstructure_plant_.set_catapult_beambreak(false);
superstructure_plant_.set_transfer_beambreak(false);
RunFor(chrono::seconds(5));
@@ -1046,9 +1014,7 @@
EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
CatapultState::READY);
- // Set retention roller to show that we are loaded.
- superstructure_plant_.set_retention_velocity(0.1);
- superstructure_plant_.set_retention_torque_current(45);
+ superstructure_plant_.set_catapult_beambreak(true);
superstructure_plant_.set_transfer_beambreak(true);
RunFor(chrono::seconds(5));
@@ -1115,8 +1081,7 @@
CatapultState::FIRING);
// Wheel should spin free again.
- superstructure_plant_.set_retention_velocity(5);
- superstructure_plant_.set_retention_torque_current(1);
+ superstructure_plant_.set_catapult_beambreak(false);
RunFor(chrono::seconds(5));
@@ -1152,8 +1117,7 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- superstructure_plant_.set_retention_velocity(5);
- superstructure_plant_.set_retention_torque_current(45);
+ superstructure_plant_.set_catapult_beambreak(false);
superstructure_plant_.set_transfer_beambreak(false);
RunFor(chrono::seconds(5));
@@ -1267,8 +1231,7 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- superstructure_plant_.set_retention_velocity(0);
- superstructure_plant_.set_retention_torque_current(45);
+ superstructure_plant_.set_catapult_beambreak(true);
RunFor(chrono::seconds(5));
@@ -1281,8 +1244,6 @@
EXPECT_NEAR(-M_PI_2,
superstructure_status_fetcher_->shooter()->turret()->position(),
5e-4);
- LOG(INFO) << aos::FlatbufferToJson(superstructure_status_fetcher_.get(),
- {.multi_line = true});
EXPECT_EQ(
kDistanceFromSpeaker,
@@ -1311,8 +1272,7 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- superstructure_plant_.set_retention_velocity(0);
- superstructure_plant_.set_retention_torque_current(45);
+ superstructure_plant_.set_catapult_beambreak(true);
RunFor(chrono::seconds(5));
diff --git a/y2024/control_loops/superstructure/superstructure_position.fbs b/y2024/control_loops/superstructure/superstructure_position.fbs
index d6ca193..4f4b75b 100644
--- a/y2024/control_loops/superstructure/superstructure_position.fbs
+++ b/y2024/control_loops/superstructure/superstructure_position.fbs
@@ -29,7 +29,7 @@
climber:frc971.PotAndAbsolutePosition (id: 5);
// True if there is a game piece in the catapult
- catapult_beam_break:bool (id: 6);
+ catapult_beambreak:bool (id: 6);
// Values of the encoder and potentiometer at the extend motor
// Zero is fully retracted, positive is extended outward.