Speed up catapult
We got a new gear ratio. Fix some state machine transitions as well to
let us reload the catapult while grabbing the next ball, and to fire
immediately too.
Change-Id: I930af58db609815d4fa639fa37b66caa011b6b94
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
diff --git a/y2022/constants.cc b/y2022/constants.cc
index 60601bf..b00e1e8 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -70,7 +70,7 @@
turret_params->zeroing_voltage = 4.0;
turret_params->operating_voltage = 12.0;
turret_params->zeroing_profile_params = {0.5, 2.0};
- turret_params->default_profile_params = {15.0, 20.0};
+ turret_params->default_profile_params = {10.0, 20.0};
turret_params->range = Values::kTurretRange();
turret_params->make_integral_loop =
control_loops::superstructure::turret::MakeIntegralTurretLoop;
@@ -220,13 +220,13 @@
intake_back->subsystem_params.zeroing_constants
.measured_absolute_position = 0.15924088639178;
- turret->potentiometer_offset = -9.99970387166721 + 0.06415943 +
- 0.073290115367682 - 0.0634440443622909 +
- 0.213601224728352 + 0.0657973101027296 -
- 0.114726411377978 - 0.980314029089968 -
- 0.0266013159299456 + 0.0631240002215899;
+ turret->potentiometer_offset =
+ -9.99970387166721 + 0.06415943 + 0.073290115367682 -
+ 0.0634440443622909 + 0.213601224728352 + 0.0657973101027296 -
+ 0.114726411377978 - 0.980314029089968 - 0.0266013159299456 +
+ 0.0631240002215899 + 0.222882504808653;
turret->subsystem_params.zeroing_constants.measured_absolute_position =
- 1.35180753332209;
+ 1.14081767944401;
flipper_arm_left->potentiometer_offset = -6.4;
flipper_arm_right->potentiometer_offset = 5.56;
diff --git a/y2022/control_loops/python/turret.py b/y2022/control_loops/python/turret.py
index 83ba4b3..fead853 100644
--- a/y2022/control_loops/python/turret.py
+++ b/y2022/control_loops/python/turret.py
@@ -17,18 +17,18 @@
except gflags.DuplicateFlagError:
pass
-kTurret = angular_system.AngularSystemParams(
- name='Turret',
- motor=control_loop.Falcon(),
- G=0.01,
- J=2.0,
- q_pos=0.40,
- q_vel=20.0,
- kalman_q_pos=0.12,
- kalman_q_vel=2.0,
- kalman_q_voltage=4.0,
- kalman_r_position=0.05,
- radius=24 * 0.0254)
+kTurret = angular_system.AngularSystemParams(name='Turret',
+ motor=control_loop.Falcon(),
+ G=(14.0 / 66.0) * (24.0 / 58.0) *
+ (18.0 / 110.0),
+ J=2.0,
+ q_pos=0.40,
+ q_vel=20.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=4.0,
+ kalman_r_position=0.05,
+ radius=24 * 0.0254)
def main(argv):
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index cee13bf..6577bff 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -299,6 +299,12 @@
frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*turret_loading_goal_buffer.fbb(), turret_loading_position));
+ const bool catapult_near_return_position =
+ (unsafe_goal != nullptr && unsafe_goal->has_catapult() &&
+ unsafe_goal->catapult()->has_return_position() &&
+ std::abs(unsafe_goal->catapult()->return_position()->unsafe_goal() -
+ catapult_.estimated_position()) < kCatapultGoalThreshold);
+
const bool turret_near_goal =
turret_goal != nullptr &&
std::abs(turret_goal->unsafe_goal() - turret_.position()) <
@@ -354,8 +360,8 @@
const bool turret_near_goal =
std::abs(turret_.estimated_position() - turret_loading_position) <
- kTurretGoalThreshold;
- if (!turret_near_goal) {
+ kTurretGoalLoadingThreshold;
+ if (!turret_near_goal || !catapult_near_return_position) {
break; // Wait for turret to reach the chosen intake
}
@@ -418,6 +424,7 @@
// Reset opening timeout
flipper_opening_start_time_ = timestamp;
+ loading_timer_ = timestamp;
}
}
break;
@@ -486,15 +493,9 @@
fire_ = true;
}
- const bool near_return_position =
- (unsafe_goal != nullptr && unsafe_goal->has_catapult() &&
- unsafe_goal->catapult()->has_return_position() &&
- std::abs(unsafe_goal->catapult()->return_position()->unsafe_goal() -
- catapult_.estimated_position()) < kCatapultGoalThreshold);
-
// Once the shot is complete and the catapult is back to its return
// position, go back to IDLE
- if (catapult_.shot_count() > prev_shot_count_ && near_return_position) {
+ if (catapult_.shot_count() > prev_shot_count_ ) {
prev_shot_count_ = catapult_.shot_count();
fire_ = false;
discarding_ball_ = false;
@@ -509,7 +510,8 @@
{.intake_front_position = intake_front_.estimated_position(),
.intake_back_position = intake_back_.estimated_position(),
.turret_position = turret_.estimated_position(),
- .shooting = state_ == SuperstructureState::SHOOTING},
+ .shooting = (state_ == SuperstructureState::SHOOTING) ||
+ !catapult_near_return_position},
turret_goal);
turret_.set_min_position(collision_avoidance_.min_turret_goal());
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index 48d07f1..edb3831 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -33,6 +33,7 @@
::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
static constexpr double kTurretGoalThreshold = 0.05;
+ static constexpr double kTurretGoalLoadingThreshold = 0.70;
static constexpr double kCatapultGoalThreshold = 0.05;
// potentiometer will be more noisy
static constexpr double kFlipperGoalThreshold = 0.05;
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index e18c2d9..fef165b 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -764,17 +764,26 @@
SendRobotVelocity(1.0);
constexpr double kTurretGoal = 2.0;
+ constexpr double kCatapultReturnPosition = -0.87;
{
auto builder = superstructure_goal_sender_.MakeBuilder();
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), kTurretGoal);
+ const auto catapult_return_offset =
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), kCatapultReturnPosition);
+ auto catapult_builder = builder.MakeBuilder<CatapultGoal>();
+ catapult_builder.add_return_position(catapult_return_offset);
+ const auto catapult_offset = catapult_builder.Finish();
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_roller_speed_front(12.0);
goal_builder.add_roller_speed_back(12.0);
goal_builder.add_roller_speed_compensation(0.0);
goal_builder.add_turret(turret_offset);
goal_builder.add_turret_intake(RequestedIntake::kFront);
+ goal_builder.add_catapult(catapult_offset);
builder.CheckOk(builder.Send(goal_builder.Finish()));
}
RunFor(std::chrono::seconds(2));
@@ -924,8 +933,8 @@
*builder.fbb(), kTurretGoal);
const auto catapult_return_offset =
- CreateStaticZeroingSingleDOFProfiledSubsystemGoal(*builder.fbb(),
- -0.87);
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), kCatapultReturnPosition);
auto catapult_builder = builder.MakeBuilder<CatapultGoal>();
catapult_builder.add_shot_position(0.3);
catapult_builder.add_shot_velocity(15.0);
diff --git a/y2022/control_loops/superstructure/superstructure_plotter.ts b/y2022/control_loops/superstructure/superstructure_plotter.ts
index f8c3a7c..ec36dd4 100644
--- a/y2022/control_loops/superstructure/superstructure_plotter.ts
+++ b/y2022/control_loops/superstructure/superstructure_plotter.ts
@@ -32,8 +32,8 @@
.setColor(RED)
.setPointSize(4.0);
positionPlot.addMessageLine(status, ['state'])
- .setColor(CYAN)
- .setPointSize(1.0);
+ .setColor(PINK)
+ .setPointSize(4.0);
positionPlot.addMessageLine(status, ['flippers_open'])
.setColor(WHITE)
.setPointSize(1.0);
@@ -60,6 +60,9 @@
intakePlot.addMessageLine(position, ['intake_beambreak_back'])
.setColor(PINK)
.setPointSize(1.0);
+ intakePlot.addMessageLine(output, ['transfer_roller_voltage'])
+ .setColor(BROWN)
+ .setPointSize(3.0);
const otherPlot =
@@ -72,6 +75,9 @@
otherPlot.addMessageLine(status, ['catapult', 'position'])
.setColor(PINK)
.setPointSize(4.0);
+ otherPlot.addMessageLine(status, ['turret', 'position'])
+ .setColor(WHITE)
+ .setPointSize(4.0);
otherPlot.addMessageLine(position, ['flipper_arm_left', 'encoder'])
.setColor(BLUE)
.setPointSize(4.0);
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index 05f3008..7fb6706 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -298,7 +298,7 @@
if (turret_pos.has_value()) {
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), turret_pos.value(),
- CreateProfileParameters(*builder.fbb(), 12.0, 20.0));
+ CreateProfileParameters(*builder.fbb(), 10.0, 20.0));
}
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>