Tune and zero altitude/turret
Change-Id: I45d1756f9717c2b48e3bcb859379c6c943d6be69
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/control_loops/python/altitude.py b/y2024/control_loops/python/altitude.py
index a06bda7..0864301 100644
--- a/y2024/control_loops/python/altitude.py
+++ b/y2024/control_loops/python/altitude.py
@@ -25,11 +25,11 @@
G=(16.0 / 60.0) * (16.0 / 162.0),
# 4340 in^ lb
J=1.2,
- q_pos=0.60,
+ q_pos=0.40,
q_vel=8.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
- kalman_q_voltage=2.0,
+ kalman_q_voltage=3.0,
kalman_r_position=0.05,
radius=10.5 * 0.0254,
dt=0.005)
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 6e31a31..1cd0191 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -166,14 +166,18 @@
? shooter_goal->altitude_position()
: &altitude_goal_builder->AsFlatbuffer();
+ // TODO(austin): goal limit...
const bool turret_in_range =
(std::abs(turret_.estimated_position() - turret_goal->unsafe_goal()) <
kCatapultActivationTurretThreshold) &&
- (std::abs(turret_.estimated_velocity()) < 0.2);
+ (std::abs(turret_goal->goal_velocity()) < 0.2) &&
+ (std::abs(turret_.estimated_velocity()) < 1.0);
const bool altitude_in_range =
(std::abs(altitude_.estimated_position() - altitude_goal->unsafe_goal()) <
- kCatapultActivationAltitudeThreshold) &&
- (std::abs(altitude_.estimated_velocity()) < 0.1);
+ (altitude_goal->unsafe_goal() > 0.75
+ ? (5 * kCatapultActivationAltitudeThreshold)
+ : kCatapultActivationAltitudeThreshold)) &&
+ (std::abs(altitude_.estimated_velocity()) < 0.4);
const bool altitude_above_min_angle =
(altitude_.estimated_position() >
robot_constants_->common()->min_altitude_shooting_angle());