Increase shot power and range
We can now shoot from further away. This change also sets us up so we
can shoot from off center better too.
The acceleration plant wasn't matching the deceleration plant very well
since the note was going away and we were fighting efficiency the other
way. Gain schedule to a different set of gains when decelerating the
cataputl.
Change-Id: Ie1955226e7148e10b447084ec04cc5bf7e91e78a
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2024/control_loops/python/catapult.py b/y2024/control_loops/python/catapult.py
index 406f56e..6f2e9a5 100644
--- a/y2024/control_loops/python/catapult.py
+++ b/y2024/control_loops/python/catapult.py
@@ -25,14 +25,14 @@
return motor
-kCatapultWithGamePiece = angular_system.AngularSystemParams(
+kCatapultWithoutGamePiece = angular_system.AngularSystemParams(
name='Catapult',
# Add the battery series resistance to make it better match.
motor=AddResistance(control_loop.NMotor(control_loop.KrakenFOC(), 2),
0.00),
G=(14.0 / 60.0) * (12.0 / 24.0),
- # 208.7328 in^2 lb
- J=0.065 + 0.04,
+ # 135.2928 in^2 lb
+ J=0.06,
q_pos=0.80,
q_vel=15.0,
kalman_q_pos=0.12,
@@ -43,14 +43,32 @@
delayed_u=1,
dt=0.005)
-kCatapultWithoutGamePiece = angular_system.AngularSystemParams(
- name='Catapult',
+kCatapultWithGamePiece = angular_system.AngularSystemParams(
+ name='CatapultWithPiece',
+ # Add the battery series resistance to make it better match.
+ motor=AddResistance(control_loop.NMotor(control_loop.KrakenFOC(), 2),
+ 0.00),
+ G=(14.0 / 60.0) * (12.0 / 24.0),
+ # 208.7328 in^2 lb
+ J=0.065 + 0.06,
+ q_pos=0.80,
+ q_vel=15.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=0.7,
+ kalman_r_position=0.05,
+ radius=12 * 0.0254,
+ delayed_u=1,
+ dt=0.005)
+
+kCatapultWithoutGamePieceDecel = angular_system.AngularSystemParams(
+ name='CatapultWithoutPieceDecel',
# Add the battery series resistance to make it better match.
motor=AddResistance(control_loop.NMotor(control_loop.KrakenFOC(), 2),
0.00),
G=(14.0 / 60.0) * (12.0 / 24.0),
# 135.2928 in^2 lb
- J=0.06,
+ J=0.04,
q_pos=0.80,
q_vel=15.0,
kalman_q_pos=0.12,
@@ -76,9 +94,10 @@
)
else:
namespaces = ['y2024', 'control_loops', 'superstructure', 'catapult']
- angular_system.WriteAngularSystem(
- [kCatapultWithoutGamePiece, kCatapultWithGamePiece], argv[1:4],
- argv[4:7], namespaces)
+ angular_system.WriteAngularSystem([
+ kCatapultWithoutGamePiece, kCatapultWithGamePiece,
+ kCatapultWithoutGamePieceDecel
+ ], argv[1:4], argv[4:7], namespaces)
if __name__ == '__main__':
diff --git a/y2024/control_loops/superstructure/aiming.cc b/y2024/control_loops/superstructure/aiming.cc
index a21c09d..bf68527 100644
--- a/y2024/control_loops/superstructure/aiming.cc
+++ b/y2024/control_loops/superstructure/aiming.cc
@@ -8,9 +8,6 @@
using frc971::control_loops::aiming::ShotMode;
using y2024::control_loops::superstructure::Aimer;
-// When the turret is at 0 the note will be leaving the robot at PI.
-static constexpr double kTurretZeroOffset = M_PI - 0.22;
-
Aimer::Aimer(aos::EventLoop *event_loop,
const y2024::Constants *robot_constants)
: event_loop_(event_loop),
@@ -76,7 +73,7 @@
robot_constants_->common()->turret()->range()),
interpolation_table_.Get(current_goal_.target_distance)
.shot_speed_over_ground,
- /*wrap_mode=*/0.15, kTurretZeroOffset},
+ /*wrap_mode=*/0.15, M_PI - kTurretZeroOffset},
RobotState{
robot_pose, {xdot, ydot}, linear_angular(1), current_goal_.position});
diff --git a/y2024/control_loops/superstructure/aiming.h b/y2024/control_loops/superstructure/aiming.h
index 9bec187..97e319d 100644
--- a/y2024/control_loops/superstructure/aiming.h
+++ b/y2024/control_loops/superstructure/aiming.h
@@ -17,6 +17,9 @@
class Aimer {
public:
+ // When the turret is at 0 the note will be leaving the robot at PI.
+ static constexpr double kTurretZeroOffset = 0.13;
+
Aimer(aos::EventLoop *event_loop, const Constants *robot_constants);
void Update(
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 88bf390..7002657 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -248,7 +248,7 @@
//
// accel = v^2 / (2 * x)
catapult_.mutable_profile()->set_maximum_velocity(
- catapult::kFreeSpeed * catapult::kOutputRatio * 4.0 / 12.0);
+ catapult::kFreeSpeed * catapult::kOutputRatio * 5.5 / 12.0);
if (disabled) {
state_ = CatapultState::RETRACTING;
@@ -272,6 +272,7 @@
if (subsystems_in_range && shooter_goal != nullptr && fire &&
catapult_close && piece_loaded) {
state_ = CatapultState::FIRING;
+ max_catapult_goal_velocity_ = catapult_.goal(1);
} else {
catapult_.set_controller_index(0);
catapult_.mutable_profile()->set_maximum_acceleration(
@@ -294,10 +295,17 @@
robot_constants_->common()
->current_limits()
->shooting_retention_roller_stator_current_limit();
- catapult_.set_controller_index(1);
+ max_catapult_goal_velocity_ =
+ std::max(max_catapult_goal_velocity_, catapult_.goal(1));
+
+ if (max_catapult_goal_velocity_ > catapult_.goal(1) + 0.1) {
+ catapult_.set_controller_index(2);
+ } else {
+ catapult_.set_controller_index(1);
+ }
catapult_.mutable_profile()->set_maximum_acceleration(400.0);
- catapult_.mutable_profile()->set_maximum_deceleration(500.0);
- catapult_.set_unprofiled_goal(2.0, 0.0);
+ catapult_.mutable_profile()->set_maximum_deceleration(1000.0);
+ catapult_.set_unprofiled_goal(2.45, 0.0);
if (CatapultClose()) {
state_ = CatapultState::RETRACTING;
++shot_count_;
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
index 5363288..e873629 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -138,6 +138,10 @@
CatapultSubsystem catapult_;
+ // Max speed we have seen during this shot. This is used to figure out when
+ // we start decelerating and switch controllers.
+ double max_catapult_goal_velocity_ = 0.0;
+
PotAndAbsoluteEncoderSubsystem turret_;
PotAndAbsoluteEncoderSubsystem altitude_;
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 9dad4ec..52c9a6c 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -1391,11 +1391,11 @@
EXPECT_NEAR(
-M_PI_2,
superstructure_status_fetcher_->shooter()->aimer()->turret_position() -
- M_PI - 0.22,
+ M_PI - Aimer::kTurretZeroOffset,
5e-4);
EXPECT_NEAR(-M_PI_2,
superstructure_status_fetcher_->shooter()->turret()->position() -
- M_PI - 0.22,
+ M_PI - Aimer::kTurretZeroOffset,
5e-4);
EXPECT_EQ(
@@ -1437,11 +1437,11 @@
EXPECT_NEAR(
M_PI_2,
superstructure_status_fetcher_->shooter()->aimer()->turret_position() +
- M_PI - 0.22,
+ M_PI - Aimer::kTurretZeroOffset,
5e-4);
EXPECT_NEAR(M_PI_2,
superstructure_status_fetcher_->shooter()->turret()->position() +
- M_PI - 0.22,
+ M_PI - Aimer::kTurretZeroOffset,
5e-4);
EXPECT_EQ(
kDistanceFromSpeaker,