Updated 5 ball for SVR
Mostly same auto, but shoots while moving.
Signed-off-by: Henry Speiser <henry@speiser.net>
Change-Id: I1c8c570f5e23e75d2586c90f12b922e2da7cbaed
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index a8a2905..ca109e0 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -19,7 +19,7 @@
namespace y2022 {
namespace actors {
namespace {
-constexpr double kExtendIntakeGoal = -0.02;
+constexpr double kExtendIntakeGoal = -0.10;
constexpr double kRetractIntakeGoal = 1.47;
constexpr double kIntakeRollerVoltage = 12.0;
constexpr double kRollerVoltage = 12.0;
@@ -107,10 +107,10 @@
SplineDirection::kBackward),
PlanSpline(std::bind(&AutonomousSplines::Spline2, &auto_splines_,
std::placeholders::_1, alliance_),
- SplineDirection::kForward),
+ SplineDirection::kBackward),
PlanSpline(std::bind(&AutonomousSplines::Spline3, &auto_splines_,
std::placeholders::_1, alliance_),
- SplineDirection::kBackward)};
+ SplineDirection::kForward)};
starting_position_ = rapid_react_splines_.value()[0].starting_position();
CHECK(starting_position_);
}
@@ -208,8 +208,7 @@
// Tell the superstructure a ball was preloaded
if (!WaitForPreloaded()) return;
- // Fire preloaded ball
- set_turret_goal(constants::Values::kTurretBackIntakePos());
+ // Fire preloaded ball while driving
set_fire_at_will(true);
SendSuperstructureGoal();
if (!WaitForBallsShot()) return;
@@ -221,33 +220,44 @@
set_fire_at_will(false);
SendSuperstructureGoal();
- // Drive and intake the 2 balls in nearest to the starting zonei
- set_turret_goal(constants::Values::kTurretFrontIntakePos());
+ // Drive and intake the ball nearest to the starting zone.
+ // Fire while moving.
ExtendBackIntake();
if (!splines[0].WaitForPlan()) return;
splines[0].Start();
- if (!splines[0].WaitForSplineDistanceRemaining(0.02)) return;
+ // Distance before we don't shoot while moving.
+ if (!splines[0].WaitForSplineDistanceRemaining(0.25)) return;
- // Fire the two balls once we stopped
- RetractBackIntake();
set_fire_at_will(true);
SendSuperstructureGoal();
+
+ if (!splines[0].WaitForSplineDistanceRemaining(0.02)) return;
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+ // Fire the last ball we picked up when stopped.
+ SendSuperstructureGoal();
+ LOG(INFO) << "Close";
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();
// Drive to the human player station while intaking two balls.
// Once is already placed down,
// and one will be rolled to the robot by the human player
- ExtendFrontIntake();
if (!splines[1].WaitForPlan()) return;
splines[1].Start();
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(1500));
+
+ set_fire_at_will(false);
+ SendSuperstructureGoal();
+
if (!splines[1].WaitForSplineDistanceRemaining(0.02)) return;
+ std::this_thread::sleep_for(std::chrono::milliseconds(500));
LOG(INFO) << "At balls 4/5 "
<< chrono::duration<double>(aos::monotonic_clock::now() -
start_time)
@@ -323,11 +333,6 @@
CreateProfileParameters(*builder.fbb(), 20.0, 60.0));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
- turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), turret_goal_,
- CreateProfileParameters(*builder.fbb(), 12.0, 20.0));
-
- flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
catapult_return_position_offset =
CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), kCatapultReturnPosition,
@@ -352,7 +357,6 @@
superstructure_builder.add_turret_intake(*requested_intake_);
}
superstructure_builder.add_transfer_roller_speed(transfer_roller_voltage_);
- superstructure_builder.add_turret(turret_offset);
superstructure_builder.add_catapult(catapult_goal_offset);
superstructure_builder.add_fire(fire_);
superstructure_builder.add_preloaded(preloaded_);
@@ -400,31 +404,12 @@
superstructure_status_fetcher_.Fetch();
CHECK(superstructure_status_fetcher_.get());
- // Don't do anything if we aren't loaded
- if (superstructure_status_fetcher_->state() !=
- control_loops::superstructure::SuperstructureState::LOADED &&
- superstructure_status_fetcher_->state() !=
- control_loops::superstructure::SuperstructureState::SHOOTING) {
- LOG(WARNING) << "No balls to shoot";
- return true;
- }
-
- // Since we're loaded, there will atleast be 1 ball to shoot
- int num_wanted = 1;
-
- // If we have another ball, we will shoot 2
- if (superstructure_status_fetcher_->front_intake_has_ball() ||
- superstructure_status_fetcher_->back_intake_has_ball()) {
- num_wanted++;
- }
-
::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
ActorBase::kLoopOffset);
superstructure_status_fetcher_.Fetch();
CHECK(superstructure_status_fetcher_.get() != nullptr);
- int initial_balls = superstructure_status_fetcher_->shot_count();
- LOG(INFO) << "Waiting for balls, started with " << initial_balls;
+
while (true) {
if (ShouldCancel()) {
return false;
@@ -432,8 +417,11 @@
phased_loop.SleepUntilNext();
superstructure_status_fetcher_.Fetch();
CHECK(superstructure_status_fetcher_.get() != nullptr);
- if (superstructure_status_fetcher_->shot_count() - initial_balls >=
- num_wanted) {
+
+ if (!superstructure_status_fetcher_->front_intake_has_ball() &&
+ !superstructure_status_fetcher_->back_intake_has_ball() &&
+ superstructure_status_fetcher_->state() ==
+ control_loops::superstructure::SuperstructureState::IDLE) {
return true;
}
}
diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index ec66fb3..0cf8c6d 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -47,7 +47,6 @@
void set_requested_intake(std::optional<RequestedIntake> requested_intake) {
requested_intake_ = requested_intake;
}
- void set_turret_goal(double turret_goal) { turret_goal_ = turret_goal; }
void set_fire_at_will(bool fire) { fire_ = fire; }
void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
@@ -77,7 +76,6 @@
double roller_back_voltage_ = 0.0;
double transfer_roller_voltage_ = 0.0;
std::optional<RequestedIntake> requested_intake_ = std::nullopt;
- double turret_goal_ = 0.0;
bool fire_ = false;
bool preloaded_ = false;
diff --git a/y2022/actors/splines/spline_5_ball_1.json b/y2022/actors/splines/spline_5_ball_1.json
index da3e4cf..46fddbb 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.1806686149879133, -0.05595918014581436, 5.762204620882601, 2.7805678460726355, 1.6146169804687496], "spline_y": [2.346189480782648, 3.6925675615333544, 4.41262134323365, 2.4753395126953124, 2.2341888067461992, 1.3005395681218328], "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}]}
\ No newline at end of file
+{"spline_count": 1, "spline_x": [0.009329753853116074, -0.014583556392633312, 1.086141950245409, 1.3463506181539948, 1.8252560302734366, 2.7940085985321357], "spline_y": [2.2499321755598816, 3.695204931543886, 3.9907963594941256, 3.2671894020316525, 2.3428532547468994, 2.267936657588998], "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.5, "start_distance": 1.0, "end_distance": 1.20}, {"constraint_type": "VELOCITY", "value": 1.0, "start_distance": 2.8, "end_distance": 10}]}
diff --git a/y2022/actors/splines/spline_5_ball_2.json b/y2022/actors/splines/spline_5_ball_2.json
index 3efd1ee..3ccf4d4 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.6037446032516893, 2.2055167265625, 2.8212725389450344, 6.148134261553881, 5.92062789622044, 6.7046250148859805], "spline_y": [1.2861465107685808, 1.7993420469805743, 1.286805497714088, 2.0935212995201415, 1.9849658141364017, 2.755576908889358], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 3.0}, {"constraint_type": "VOLTAGE", "value": 12.0}]}
\ No newline at end of file
+{"spline_count": 1, "spline_x": [2.8029000394337356, 3.598701010385372, 3.592928864987311, 5.716241105891047, 6.058409698241228, 6.836921351984797], "spline_y": [2.2659592486204163, 2.2235586475607194, 1.3732945972518653, 1.187231336623733, 1.965522252857657, 2.7153394202517944], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 3.0}, {"constraint_type": "VOLTAGE", "value": 12.0}]}
\ No newline at end of file
diff --git a/y2022/actors/splines/spline_5_ball_3.json b/y2022/actors/splines/spline_5_ball_3.json
index 6b239fc..7c288ca 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.702231375950168, 6.373881457031249, 5.758688966174009, 3.1788453508620487, 2.273453592205448, 1.6114305300886826], "spline_y": [2.7438724869219806, 2.4293757261929896, 2.0768880836927197, 1.7809922274871859, 1.852294682145808, 1.2821724076488596], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 4.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 3}, {"constraint_type": "VOLTAGE", "value": 12.0}]}
\ No newline at end of file
+{"spline_count": 1, "spline_x": [6.853097963576857, 6.385098954905726, 4.82969339985708, 2.83784620575764, 1.5541577889609748, 1.0363839414488067], "spline_y": [2.714615301031989, 2.2383835427652135, 1.708398624422522, 2.517585062352501, 2.5751182100225627, 2.1689745582854774], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 4.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 3.0}, {"constraint_type": "VOLTAGE", "value": 12.0}]}
\ No newline at end of file
diff --git a/y2022/constants.cc b/y2022/constants.cc
index a6b9a28..dd7a8ac 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -137,9 +137,9 @@
{1.9, {0.1, 19.0}},
{2.12, {0.15, 18.8}},
{2.9, {0.25, 19.2}},
- {3.2, {0.28, 20.5}},
+ {3.2, {0.28, 20.3}},
- {3.86, {0.35, 20.9}},
+ {3.60, {0.33, 20.3}},
{4.9, {0.4, 21.9}},
{5.4, {0.4, 23.9}},
{6.0, {0.40, 25.0}},
diff --git a/y2022/control_loops/drivetrain/localizer.cc b/y2022/control_loops/drivetrain/localizer.cc
index 65df654..79ced2d 100644
--- a/y2022/control_loops/drivetrain/localizer.cc
+++ b/y2022/control_loops/drivetrain/localizer.cc
@@ -48,7 +48,7 @@
joystick_state_fetcher_->autonomous()) {
// TODO(james): This is an inelegant way to avoid having the localizer mess
// up splines. Do better.
- return;
+ //return;
}
if (localizer_output_fetcher_.Fetch()) {
clock_offset_fetcher_.Fetch();