Add catapult debouncing and tuning
Changes taken from unreviewed/austin/sequencing_bringup_2024_2_26 with
extra changes to superstructure_lib_test to make them pass.
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I2bed5a39fce12cac08a7fe005ab60866c513962f
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index 16acec6..e7c9ca4 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -38,8 +38,8 @@
"max_acceleration": 3.0
},
"default_profile_params":{
- "max_velocity": 0.5,
- "max_acceleration": 3.0
+ "max_velocity": 2.0,
+ "max_acceleration": 5.0
},
"range": {
"lower_hard": -0.2,
@@ -71,10 +71,11 @@
"turret_stator_current_limit": 40,
"altitude_supply_current_limit": 10,
"altitude_stator_current_limit": 60,
- "catapult_supply_current_limit": 10,
- "catapult_stator_current_limit": 30,
- "retention_roller_stator_current_limit": 5,
+ "catapult_supply_current_limit": 60,
+ "catapult_stator_current_limit": 250,
+ "retention_roller_stator_current_limit": 20,
"slower_retention_roller_stator_current_limit": 2,
+ "shooting_retention_roller_stator_current_limit": -20,
"retention_roller_supply_current_limit": 10
},
"transfer_roller_voltages": {
@@ -111,7 +112,7 @@
},
"catapult": {
"zeroing_voltage": 3.0,
- "operating_voltage": 3.0,
+ "operating_voltage": 12.0,
"zeroing_profile_params": {
"max_velocity": 1.0,
"max_acceleration": 6.0
@@ -137,8 +138,8 @@
"max_acceleration": 3.0
},
"default_profile_params":{
- "max_velocity": 0.25,
- "max_acceleration": 0.25
+ "max_velocity": 3.0,
+ "max_acceleration": 5.0
},
"range": {
"lower_hard": -0.01,
@@ -156,8 +157,8 @@
"max_acceleration": 3.0
},
"default_profile_params":{
- "max_velocity": 1.00,
- "max_acceleration": 1.5
+ "max_velocity": 2.0,
+ "max_acceleration": 5.0
},
"range": {
"lower_hard": -4.8,
@@ -217,7 +218,7 @@
"max_altitude_shooting_angle": 0.89,
"retention_roller_voltages": {
"retaining": 1.5,
- "spitting": -6.0
+ "spitting": 6.0
},
// TODO(Filip): Update the speaker and amp shooter setpoints
"shooter_speaker_set_point": {
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index d2e4ede..dc41ec1 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -83,6 +83,7 @@
retention_roller_supply_current_limit:double (id: 18);
retention_roller_stator_current_limit:double (id: 19);
slower_retention_roller_stator_current_limit:double (id: 20);
+ shooting_retention_roller_stator_current_limit:double (id: 23);
catapult_supply_current_limit:double (id: 21);
catapult_stator_current_limit:double (id: 22);
}
diff --git a/y2024/control_loops/python/catapult.py b/y2024/control_loops/python/catapult.py
index f7b76e6..49c6305 100644
--- a/y2024/control_loops/python/catapult.py
+++ b/y2024/control_loops/python/catapult.py
@@ -29,10 +29,10 @@
name='Catapult',
# Add the battery series resistance to make it better match.
motor=AddResistance(control_loop.NMotor(control_loop.KrakenFOC(), 2),
- 0.03),
+ 0.00),
G=(14.0 / 60.0) * (12.0 / 24.0),
# 208.7328 in^2 lb
- J=0.065,
+ J=0.065 + 0.04,
q_pos=0.80,
q_vel=15.0,
kalman_q_pos=0.12,
@@ -46,10 +46,10 @@
name='Catapult',
# Add the battery series resistance to make it better match.
motor=AddResistance(control_loop.NMotor(control_loop.KrakenFOC(), 2),
- 0.03),
+ 0.00),
G=(14.0 / 60.0) * (12.0 / 24.0),
# 135.2928 in^2 lb
- J=0.04,
+ J=0.06,
q_pos=0.80,
q_vel=15.0,
kalman_q_pos=0.12,
@@ -75,7 +75,7 @@
else:
namespaces = ['y2024', 'control_loops', 'superstructure', 'catapult']
angular_system.WriteAngularSystem(
- [kCatapultWithGamePiece, kCatapultWithoutGamePiece], argv[1:4],
+ [kCatapultWithoutGamePiece, kCatapultWithGamePiece], argv[1:4],
argv[4:7], namespaces)
diff --git a/y2024/control_loops/python/intake_pivot.py b/y2024/control_loops/python/intake_pivot.py
index 5e60311..fad5020 100644
--- a/y2024/control_loops/python/intake_pivot.py
+++ b/y2024/control_loops/python/intake_pivot.py
@@ -21,12 +21,12 @@
name='IntakePivot',
motor=control_loop.KrakenFOC(),
G=(16. / 60.) * (18. / 62.) * (18. / 62.) * (15. / 24.),
- J=0.25,
- q_pos=0.80,
- q_vel=30.0,
+ J=0.4,
+ q_pos=1.0,
+ q_vel=800.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
- kalman_q_voltage=2.0,
+ kalman_q_voltage=1.5,
kalman_r_position=0.05,
radius=6.85 * 0.0254)
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 532109e..07b7ce1 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -29,7 +29,9 @@
aimer_(event_loop, robot_constants_),
interpolation_table_(
y2024::constants::Values::InterpolationTableFromFlatbuffer(
- robot_constants_->common()->shooter_interpolation_table())) {}
+ robot_constants_->common()->shooter_interpolation_table())),
+ debouncer_(std::chrono::milliseconds(100), std::chrono::milliseconds(8)) {
+}
flatbuffers::Offset<y2024::control_loops::superstructure::ShooterStatus>
Shooter::Iterate(
@@ -40,16 +42,17 @@
double *retention_roller_stator_current_limit, double /*battery_voltage*/,
CollisionAvoidance *collision_avoidance, const double intake_pivot_position,
double *max_intake_pivot_position, double *min_intake_pivot_position,
- bool standby, flatbuffers::FlatBufferBuilder *fbb) {
+ bool standby, flatbuffers::FlatBufferBuilder *fbb,
+ aos::monotonic_clock::time_point monotonic_now) {
drivetrain_status_fetcher_.Fetch();
// 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 = position->catapult_beambreak() ||
- (shooter_goal != nullptr && shooter_goal->preloaded());
+ debouncer_.Update(position->catapult_beambreak() ||
+ (shooter_goal != nullptr && shooter_goal->preloaded()),
+ monotonic_now);
+ const bool piece_loaded = debouncer_.state();
aos::fbs::FixedStackAllocator<aos::fbs::Builder<
frc971::control_loops::
@@ -214,6 +217,9 @@
state_ = CatapultState::RETRACTING;
}
+ constexpr double kLoadingAcceleration = 20.0;
+ constexpr double kLoadingDeceleration = 10.0;
+
switch (state_) {
case CatapultState::READY:
[[fallthrough]];
@@ -231,8 +237,10 @@
state_ = CatapultState::FIRING;
} else {
catapult_.set_controller_index(0);
- catapult_.mutable_profile()->set_maximum_acceleration(100.0);
- catapult_.mutable_profile()->set_maximum_deceleration(50.0);
+ catapult_.mutable_profile()->set_maximum_acceleration(
+ kLoadingAcceleration);
+ catapult_.mutable_profile()->set_maximum_deceleration(
+ kLoadingDeceleration);
catapult_.set_unprofiled_goal(0.0, 0.0);
if (!catapult_close) {
@@ -245,9 +253,13 @@
case CatapultState::FIRING:
*retention_roller_output =
robot_constants_->common()->retention_roller_voltages()->spitting();
- catapult_.set_controller_index(0);
+ *retention_roller_stator_current_limit =
+ robot_constants_->common()
+ ->current_limits()
+ ->shooting_retention_roller_stator_current_limit();
+ catapult_.set_controller_index(1);
catapult_.mutable_profile()->set_maximum_acceleration(400.0);
- catapult_.mutable_profile()->set_maximum_deceleration(600.0);
+ catapult_.mutable_profile()->set_maximum_deceleration(500.0);
catapult_.set_unprofiled_goal(2.0, 0.0);
if (CatapultClose()) {
state_ = CatapultState::RETRACTING;
@@ -257,8 +269,11 @@
[[fallthrough]];
case CatapultState::RETRACTING:
catapult_.set_controller_index(0);
- catapult_.mutable_profile()->set_maximum_acceleration(100.0);
- catapult_.mutable_profile()->set_maximum_deceleration(50.0);
+ catapult_.mutable_profile()->set_maximum_acceleration(
+ kLoadingAcceleration);
+ catapult_.mutable_profile()->set_maximum_deceleration(
+ kLoadingDeceleration);
+ // TODO: catapult_return_position
catapult_.set_unprofiled_goal(0.0, 0.0);
if (CatapultClose()) {
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
index 87b0bc0..4ceffd4 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -17,6 +17,39 @@
namespace y2024::control_loops::superstructure {
+class Debouncer {
+ public:
+ Debouncer(std::chrono::nanoseconds rising_delay,
+ std::chrono::nanoseconds falling_delay)
+ : rising_delay_(rising_delay), falling_delay_(falling_delay) {}
+
+ void Update(bool state, aos::monotonic_clock::time_point now) {
+ if (state_transition_ != state) {
+ transition_time_ = now;
+ state_transition_ = state;
+ }
+
+ if (state != output_state_) {
+ if (state) {
+ output_state_ = now > transition_time_ + rising_delay_;
+ } else {
+ output_state_ = !(now > transition_time_ + falling_delay_);
+ }
+ }
+ }
+
+ bool state() const { return output_state_; }
+
+ private:
+ const std::chrono::nanoseconds rising_delay_;
+ const std::chrono::nanoseconds falling_delay_;
+
+ bool state_transition_ = false;
+ bool output_state_ = false;
+ aos::monotonic_clock::time_point transition_time_ =
+ aos::monotonic_clock::min_time;
+};
+
// The shooter class will control the various subsystems involved in the
// shooter- the turret, altitude, and catapult.
class Shooter {
@@ -73,7 +106,8 @@
const double intake_pivot_position, double *max_turret_intake_position,
double *min_intake_pivot_position,
/* If true, go to extend collision avoidance position */ bool standby,
- flatbuffers::FlatBufferBuilder *fbb);
+ flatbuffers::FlatBufferBuilder *fbb,
+ aos::monotonic_clock::time_point monotonic_now);
private:
CatapultState state_ = CatapultState::RETRACTING;
@@ -102,6 +136,8 @@
frc971::shooter_interpolation::InterpolationTable<
y2024::constants::Values::ShotParams>
interpolation_table_;
+
+ Debouncer debouncer_;
};
} // namespace y2024::control_loops::superstructure
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 577ac15..8f42cb7 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -533,7 +533,8 @@
: nullptr,
robot_state().voltage_battery(), &collision_avoidance_,
intake_pivot_.estimated_position(), &max_intake_pivot_position,
- &min_intake_pivot_position, move_turret_to_standby, status->fbb());
+ &min_intake_pivot_position, move_turret_to_standby, status->fbb(),
+ timestamp);
intake_pivot_.set_min_position(min_intake_pivot_position);
intake_pivot_.set_max_position(max_intake_pivot_position);
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index b223736..6c72060 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -400,7 +400,7 @@
EXPECT_NEAR(set_point,
superstructure_status_fetcher_->intake_pivot()->position(),
- 0.01);
+ 0.015);
if (superstructure_goal_fetcher_->has_shooter_goal() &&
superstructure_goal_fetcher_->note_goal() != NoteGoal::AMP &&
@@ -1198,7 +1198,7 @@
// Wait until the bot finishes auto-aiming.
WaitUntilNear(kTurretGoal, kAltitudeGoal);
- RunFor(chrono::seconds(10));
+ RunFor(chrono::milliseconds(1000));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
@@ -1281,7 +1281,7 @@
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- RunFor(dt());
+ RunFor(chrono::milliseconds(200));
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);