Tune auto to make 5 balls for real
Change-Id: Iebfcb29259a27f2cf6c6fcbc794c022972228212
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index 8bfb2d9..ecc2a98 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -13,7 +13,7 @@
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
DEFINE_bool(spline_auto, false, "If true, define a spline autonomous mode");
-DEFINE_bool(rapid_react, false,
+DEFINE_bool(rapid_react, true,
"If true, run the main rapid react autonomous mode");
namespace y2022 {
@@ -21,7 +21,7 @@
namespace {
constexpr double kExtendIntakeGoal = -0.02;
constexpr double kRetractIntakeGoal = 1.47;
-constexpr double kIntakeRollerVoltage = 8.0;
+constexpr double kIntakeRollerVoltage = 12.0;
constexpr double kRollerVoltage = 12.0;
constexpr double kCatapultReturnPosition = -0.908;
} // namespace
@@ -209,15 +209,20 @@
if (!WaitForPreloaded()) return;
// Fire preloaded ball
- set_turret_goal(constants::Values::kTurretFrontIntakePos());
+ set_turret_goal(constants::Values::kTurretBackIntakePos());
set_fire_at_will(true);
SendSuperstructureGoal();
if (!WaitForBallsShot()) return;
+ LOG(INFO) << "Shot first ball "
+ << chrono::duration<double>(aos::monotonic_clock::now() -
+ start_time)
+ .count()
+ << 's';
set_fire_at_will(false);
SendSuperstructureGoal();
// Drive and intake the 2 balls in nearest to the starting zonei
- set_turret_goal(constants::Values::kTurretBackIntakePos());
+ set_turret_goal(constants::Values::kTurretFrontIntakePos());
ExtendBackIntake();
if (!splines[0].WaitForPlan()) return;
splines[0].Start();
@@ -228,6 +233,11 @@
set_fire_at_will(true);
SendSuperstructureGoal();
if (!WaitForBallsShot()) return;
+ LOG(INFO) << "Shot first 3 balls "
+ << chrono::duration<double>(aos::monotonic_clock::now() -
+ start_time)
+ .count()
+ << 's';
set_fire_at_will(false);
SendSuperstructureGoal();
@@ -238,6 +248,11 @@
if (!splines[1].WaitForPlan()) return;
splines[1].Start();
if (!splines[1].WaitForSplineDistanceRemaining(0.02)) return;
+ LOG(INFO) << "At balls 4/5 "
+ << chrono::duration<double>(aos::monotonic_clock::now() -
+ start_time)
+ .count()
+ << 's';
// Drive to the shooting position
if (!splines[2].WaitForPlan()) return;
@@ -246,6 +261,11 @@
RetractFrontIntake();
if (!splines[2].WaitForSplineDistanceRemaining(0.02)) return;
+ LOG(INFO) << "Shooting last balls "
+ << chrono::duration<double>(aos::monotonic_clock::now() -
+ start_time)
+ .count()
+ << 's';
// Fire the two balls once we stopped
set_fire_at_will(true);
@@ -325,7 +345,7 @@
superstructure_builder.add_intake_front(intake_front_offset);
superstructure_builder.add_intake_back(intake_back_offset);
- superstructure_builder.add_roller_speed_compensation(1.5);
+ superstructure_builder.add_roller_speed_compensation(0.0);
superstructure_builder.add_roller_speed_front(roller_front_voltage_);
superstructure_builder.add_roller_speed_back(roller_back_voltage_);
if (requested_intake_.has_value()) {
@@ -339,7 +359,7 @@
superstructure_builder.add_catapult(catapult_goal_offset);
superstructure_builder.add_fire(fire_);
superstructure_builder.add_preloaded(preloaded_);
- superstructure_builder.add_auto_aim(false);
+ superstructure_builder.add_auto_aim(true);
if (builder.Send(superstructure_builder.Finish()) !=
aos::RawSender::Error::kOk) {
@@ -384,7 +404,8 @@
}
[[nodiscard]] bool AutonomousActor::WaitForBallsShot() {
- CHECK(superstructure_status_fetcher_.Fetch());
+ superstructure_status_fetcher_.Fetch();
+ CHECK(superstructure_status_fetcher_.get());
// Don't do anything if we aren't loaded
if (superstructure_status_fetcher_->state() !=
diff --git a/y2022/actors/splines/spline_5_ball_1.json b/y2022/actors/splines/spline_5_ball_1.json
index 03b5644..91a31f6 100644
--- a/y2022/actors/splines/spline_5_ball_1.json
+++ b/y2022/actors/splines/spline_5_ball_1.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [-0.18145693702491972, -0.20576409082414582, 0.49914336935341463, 5.522928566665585, 2.880988556589501, 1.845502911614626], "spline_y": [2.346189480782648, 3.695236516639704, 4.837672745203337, 2.7973591802504263, 2.3618745632049176, 1.471550457245212], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
+{"spline_count": 1, "spline_x": [-0.18145693702491972, -0.1806686149879133, -0.05595918014581436, 5.568337297714338, 2.9263972876382542, 1.8899826868983047], "spline_y": [2.346189480782648, 3.6925675615333544, 4.41262134323365, 2.806871944572777, 2.3713873275272683, 1.4720189432649824], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.5}, {"constraint_type": "VOLTAGE", "value": 12.0}, {"constraint_type": "VELOCITY", "value": 0.8, "start_distance": 1.0, "end_distance": 1.15}]}
diff --git a/y2022/actors/splines/spline_5_ball_2.json b/y2022/actors/splines/spline_5_ball_2.json
index 975d6ed..7d7e8c6 100644
--- a/y2022/actors/splines/spline_5_ball_2.json
+++ b/y2022/actors/splines/spline_5_ball_2.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [1.845502911614626, 2.4171345421461163, 3.004385177342393, 5.2666273627598565, 5.91167736359207, 6.7133296469650885], "spline_y": [1.471550457245212, 2.0368843425108123, 1.4829129663110887, 1.7324754229926884, 1.9201953550863622, 2.6087941113170316], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
+{"spline_count": 1, "spline_x": [1.8977893624261148, 2.493489118397258, 3.0938041880159797, 6.267891145316549, 6.049173922736376, 6.833717690390506], "spline_y": [1.4682096511768088, 2.0094316068927602, 1.5053043317512498, 2.1686488722080637, 2.082692598659578, 2.8053622716611706], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 3}, {"constraint_type": "VOLTAGE", "value": 12.0}]}
diff --git a/y2022/actors/splines/spline_5_ball_3.json b/y2022/actors/splines/spline_5_ball_3.json
index e9e1a41..8d37834 100644
--- a/y2022/actors/splines/spline_5_ball_3.json
+++ b/y2022/actors/splines/spline_5_ball_3.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [6.7133296469650885, 6.343336285408311, 5.737641975476239, 3.4208648418667504, 2.51547308321015, 1.841750386467515], "spline_y": [2.6087941113170316, 2.2490783431368313, 1.931858867758041, 1.9600940870614552, 2.031396541720077, 1.4679812465169668], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
+{"spline_count": 1, "spline_x": [6.8497036863806855, 6.523770855698178, 5.901632869573503, 3.625913896364584, 2.7205221377079836, 2.0513600546151287], "spline_y": [2.7941148215833196, 2.479530267377096, 2.134117802271161, 2.01574966936852, 2.0870521240271422, 1.5304761433039502], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 4}, {"constraint_type": "LATERAL_ACCELERATION", "value": 3}, {"constraint_type": "VOLTAGE", "value": 12.0}]}
diff --git a/y2022/constants.cc b/y2022/constants.cc
index 0b4a591..acd36b4 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, 40.0};
+ turret_params->default_profile_params = {15.0, 20.0};
turret_params->range = Values::kTurretRange();
turret_params->make_integral_loop =
control_loops::superstructure::turret::MakeIntegralTurretLoop;
@@ -132,8 +132,8 @@
// Interpolation table for comp and practice robots
r.shot_interpolation_table = InterpolationTable<Values::ShotParams>({
- {2, {0.08, 8.0}},
- {5, {0.6, 10.0}},
+ {1, {0.03, 18.0}},
+ {10, {0.03, 18.0}},
});
switch (team) {
diff --git a/y2022/constants.h b/y2022/constants.h
index 35ca0ae..610ddc0 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -114,13 +114,13 @@
static constexpr ::frc971::constants::Range kTurretRange() {
return ::frc971::constants::Range{
.lower_hard = -7.0, // Back Hard
- .upper_hard = 4.0, // Front Hard
+ .upper_hard = 3.0, // Front Hard
.lower = -6.5, // Back Soft
- .upper = 3.25 // Front Soft
+ .upper = 2.5 // Front Soft
};
}
- static constexpr double kTurretBackIntakePos() { return M_PI; }
+ static constexpr double kTurretBackIntakePos() { return -M_PI; }
static constexpr double kTurretFrontIntakePos() { return 0; }
static constexpr double kTurretPotRatio() { return 27.0 / 110.0; }
diff --git a/y2022/control_loops/superstructure/catapult_plotter.ts b/y2022/control_loops/superstructure/catapult_plotter.ts
index 6c66340..5736426 100644
--- a/y2022/control_loops/superstructure/catapult_plotter.ts
+++ b/y2022/control_loops/superstructure/catapult_plotter.ts
@@ -14,6 +14,7 @@
const goal = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Goal');
const output = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Output');
const status = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Status');
+ const position = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Position');
const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
// Robot Enabled/Disabled and Mode
@@ -34,6 +35,7 @@
positionPlot.addMessageLine(status, ['catapult', 'position']).setColor(GREEN).setPointSize(4.0);
positionPlot.addMessageLine(status, ['catapult', 'velocity']).setColor(PINK).setPointSize(1.0);
positionPlot.addMessageLine(status, ['catapult', 'calculated_velocity']).setColor(BROWN).setPointSize(1.0);
+ positionPlot.addMessageLine(position, ['catapult', 'pot']).setColor(WHITE).setPointSize(1.0);
positionPlot.addMessageLine(status, ['catapult', 'estimator_state', 'position']).setColor(CYAN).setPointSize(1.0);
const voltagePlot =
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 6daea37..0e71656 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -386,11 +386,6 @@
reseating_in_catapult_ = true;
break;
}
- // If we started firing and the flippers closed a bit, estop to prevent
- // damage
- if (fire_ && !flippers_open_) {
- catapult_.Estop();
- }
// If the turret reached the aiming goal and the catapult is safe to move
// up, fire!
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 6c19052..9288da1 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -755,7 +755,7 @@
SendRobotVelocity(3.0);
- constexpr double kTurretGoal = 3.0;
+ constexpr double kTurretGoal = 2.0;
{
auto builder = superstructure_goal_sender_.MakeBuilder();
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
@@ -1015,7 +1015,7 @@
SetEnabled(true);
WaitUntilZeroed();
- constexpr double kTurretGoal = -4.0;
+ constexpr double kTurretGoal = -6.0;
{
auto builder = superstructure_goal_sender_.MakeBuilder();
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
@@ -1033,20 +1033,20 @@
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
0.001);
- superstructure_plant_.set_intake_beambreak_back(true);
+ superstructure_plant_.set_intake_beambreak_front(true);
RunFor(dt() * 2);
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::TRANSFERRING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
- IntakeState::INTAKE_BACK_BALL);
+ IntakeState::INTAKE_FRONT_BALL);
RunFor(std::chrono::seconds(3));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
- -constants::Values::kTurretBackIntakePos(), 0.001);
+ -constants::Values::kTurretFrontIntakePos() - 2.0 * M_PI, 0.001);
// it chooses -pi because -pi is closer to -4 than positive pi
}