Add a 6 ball auto.
This has run 10 times in a row.
Change-Id: I6ffe32e99251d17a2e19c371132d1e2371f06dbb
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2020/actors/auto_splines.cc b/y2020/actors/auto_splines.cc
index 00e3bd0..77cfbb5 100644
--- a/y2020/actors/auto_splines.cc
+++ b/y2020/actors/auto_splines.cc
@@ -5,6 +5,9 @@
namespace y2020 {
namespace actors {
+constexpr double kFieldLength = 15.983;
+constexpr double kFieldWidth = 8.212;
+
void MaybeFlipSpline(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
@@ -20,6 +23,34 @@
}
}
+flatbuffers::Offset<frc971::MultiSpline> FixSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ flatbuffers::Offset<frc971::MultiSpline> spline_offset,
+ aos::Alliance alliance) {
+ frc971::MultiSpline *spline =
+ GetMutableTemporaryPointer(*builder->fbb(), spline_offset);
+ flatbuffers::Vector<float> *spline_x = spline->mutable_spline_x();
+ flatbuffers::Vector<float> *spline_y = spline->mutable_spline_y();
+
+ for (size_t ii = 0; ii < spline_x->size(); ++ii) {
+ spline_x->Mutate(ii, spline_x->Get(ii) - kFieldLength / 2.0);
+ }
+ for (size_t ii = 0; ii < spline_y->size(); ++ii) {
+ spline_y->Mutate(ii, kFieldWidth / 2.0 - spline_y->Get(ii));
+ }
+
+ if (alliance == aos::Alliance::kBlue) {
+ for (size_t ii = 0; ii < spline_x->size(); ++ii) {
+ spline_x->Mutate(ii, -spline_x->Get(ii));
+ }
+ for (size_t ii = 0; ii < spline_y->size(); ++ii) {
+ spline_y->Mutate(ii, -spline_y->Get(ii));
+ }
+ }
+ return spline_offset;
+}
+
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
@@ -90,6 +121,26 @@
return multispline_builder.Finish();
}
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TargetAligned1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(target_aligned_1_,
+ builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TargetAligned2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(target_aligned_2_,
+ builder->fbb()),
+ alliance);
+}
+
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder) {
diff --git a/y2020/actors/auto_splines.h b/y2020/actors/auto_splines.h
index 98bf0b7..bdaef75 100644
--- a/y2020/actors/auto_splines.h
+++ b/y2020/actors/auto_splines.h
@@ -47,16 +47,12 @@
flatbuffers::Offset<frc971::MultiSpline> TargetAligned1(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
- *builder) {
- return aos::CopyFlatBuffer<frc971::MultiSpline>(target_aligned_1_,
- builder->fbb());
- }
+ *builder,
+ aos::Alliance alliance);
flatbuffers::Offset<frc971::MultiSpline> TargetAligned2(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
- *builder) {
- return aos::CopyFlatBuffer<frc971::MultiSpline>(target_aligned_2_,
- builder->fbb());
- }
+ *builder,
+ aos::Alliance alliance);
flatbuffers::Offset<frc971::MultiSpline> TargetOffset1(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder) {
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index 02d68da..d25a8c3 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -11,7 +11,7 @@
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
DEFINE_bool(spline_auto, false, "If true, define a spline autonomous mode");
-DEFINE_bool(target_aligned, false,
+DEFINE_bool(target_aligned, true,
"If true, run the Infinite Recharge autonomous that starts aligned "
"with the target");
DEFINE_bool(target_offset, false,
@@ -44,7 +44,7 @@
event_loop->MakeFetcher<y2020::control_loops::superstructure::Status>(
"/superstructure")),
auto_splines_() {
- set_max_drivetrain_voltage(2.0);
+ set_max_drivetrain_voltage(12.0);
replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
event_loop->OnRun([this, event_loop]() {
replan_timer_->Setup(event_loop->monotonic_now());
@@ -72,11 +72,23 @@
void AutonomousActor::Replan() {
sent_starting_position_ = false;
+ if (alliance_ == aos::Alliance::kInvalid) {
+ return;
+ }
if (FLAGS_spline_auto) {
test_spline_ = PlanSpline(std::bind(&AutonomousSplines::TestSpline,
&auto_splines_, std::placeholders::_1),
SplineDirection::kForward);
starting_position_ = test_spline_->starting_position();
+ } else if (FLAGS_target_aligned) {
+ target_aligned_splines_ = {
+ PlanSpline(std::bind(&AutonomousSplines::TargetAligned1, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(std::bind(&AutonomousSplines::TargetAligned2, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kBackward)};
+ starting_position_ = target_aligned_splines_.value()[0].starting_position();
} else if (FLAGS_target_offset) {
target_offset_splines_ = {
PlanSpline(std::bind(&AutonomousSplines::TargetOffset1, &auto_splines_,
@@ -86,15 +98,6 @@
std::placeholders::_1),
SplineDirection::kBackward)};
starting_position_ = target_offset_splines_.value()[0].starting_position();
- } else if (FLAGS_target_aligned) {
- target_aligned_splines_ = {
- PlanSpline(std::bind(&AutonomousSplines::TargetAligned1, &auto_splines_,
- std::placeholders::_1),
- SplineDirection::kForward),
- PlanSpline(std::bind(&AutonomousSplines::TargetAligned2, &auto_splines_,
- std::placeholders::_1),
- SplineDirection::kBackward)};
- starting_position_ = target_aligned_splines_.value()[0].starting_position();
} else {
starting_position_ = Eigen::Vector3d::Zero();
}
@@ -129,14 +132,14 @@
AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
return false;
}
- if (FLAGS_target_aligned) {
+ if (FLAGS_spline_auto) {
+ SplineAuto();
+ } else if (FLAGS_target_aligned) {
TargetAligned();
} else if (FLAGS_target_offset) {
TargetOffset();
} else if (FLAGS_just_shoot) {
JustShoot();
- } else if (FLAGS_spline_auto) {
- SplineAuto();
} else {
return DriveFwd();
}
@@ -162,42 +165,64 @@
}
void AutonomousActor::TargetAligned() {
+ aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
CHECK(target_aligned_splines_);
auto &splines = *target_aligned_splines_;
- // shoot pre-loaded balls
- set_shooter_tracking(true);
+ // Spin up.
set_shooting(true);
SendSuperstructureGoal();
-
if (!WaitForBallsShot(3)) return;
set_shooting(false);
+ ExtendIntake();
SendSuperstructureGoal();
- ExtendIntake();
-
- // pickup 3 more balls
if (!splines[0].WaitForPlan()) return;
splines[0].Start();
if (!splines[0].WaitForSplineDistanceRemaining(0.02)) return;
+ std::this_thread::sleep_for(chrono::milliseconds(200));
RetractIntake();
if (!splines[1].WaitForPlan()) return;
splines[1].Start();
- if (!splines[1].WaitForSplineDistanceRemaining(0.02)) return;
+ if (!splines[1].WaitForSplineDistanceRemaining(2.0)) return;
+ // Reverse the rollers for a moment to try to unjam any jammed balls. Since
+ // we are moving here, this is free to try.
+ set_roller_voltage(-12.0);
+ std::this_thread::sleep_for(chrono::milliseconds(300));
+ set_roller_voltage(0.0);
- // shoot the new balls in front of the goal.
+ // Once we come to a stop, give the robot a moment to settle down. This makes
+ // the shot more accurate.
+ if (!splines[1].WaitForSplineDistanceRemaining(0.02)) return;
+ std::this_thread::sleep_for(chrono::milliseconds(1500));
+ set_shooting(true);
+ const int balls = Balls();
+
+ SendSuperstructureGoal();
+
+ std::this_thread::sleep_for(chrono::milliseconds(1500));
+
+ // We have been seeing balls get stuck on the intake roller. Reverse the
+ // roller again for a moment to unjam it.
+ set_shooting(false);
+ set_roller_voltage(-12.0);
+ SendSuperstructureGoal();
+ std::this_thread::sleep_for(chrono::milliseconds(250));
+
+ set_roller_voltage(0.0);
set_shooting(true);
SendSuperstructureGoal();
- if (!WaitForBallsShot(3)) return;
+ if (!WaitUntilAbsoluteBallsShot(3 + balls)) return;
- set_shooting(false);
- set_shooter_tracking(false);
- SendSuperstructureGoal();
+ LOG(INFO) << "Took "
+ << chrono::duration<double>(aos::monotonic_clock::now() -
+ start_time)
+ .count();
}
void AutonomousActor::JustShoot() {
@@ -294,7 +319,7 @@
builder.MakeBuilder<superstructure::Goal>();
superstructure_builder.add_intake(intake_offset);
- superstructure_builder.add_intake_preloading(intake_preloading_);
+ superstructure_builder.add_intake_preloading(true);
superstructure_builder.add_roller_voltage(roller_voltage_);
superstructure_builder.add_roller_speed_compensation(
kRollerSpeedCompensation);
@@ -309,26 +334,24 @@
}
void AutonomousActor::ExtendIntake() {
- set_intake_goal(1.25);
- set_roller_voltage(12.0);
- set_intake_preloading(true);
+ set_intake_goal(1.30);
+ set_roller_voltage(6.0);
SendSuperstructureGoal();
}
void AutonomousActor::RetractIntake() {
set_intake_goal(-0.89);
- set_roller_voltage(0.0);
- set_intake_preloading(false);
+ set_roller_voltage(6.0);
SendSuperstructureGoal();
}
-bool AutonomousActor::WaitForBallsShot(int num_wanted) {
+int AutonomousActor::Balls() {
superstructure_status_fetcher_.Fetch();
CHECK(superstructure_status_fetcher_.get() != nullptr);
- const int initial_balls_shot =
- superstructure_status_fetcher_->shooter()->balls_shot();
- int balls_shot = initial_balls_shot;
+ return superstructure_status_fetcher_->shooter()->balls_shot();
+}
+bool AutonomousActor::WaitUntilAbsoluteBallsShot(int absolute_balls) {
::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
frc971::controls::kLoopFrequency / 2);
@@ -338,12 +361,17 @@
}
phased_loop.SleepUntilNext();
superstructure_status_fetcher_.Fetch();
- balls_shot = superstructure_status_fetcher_->shooter()->balls_shot();
- if ((balls_shot - initial_balls_shot) >= num_wanted) {
+ CHECK(superstructure_status_fetcher_.get() != nullptr);
+ if (superstructure_status_fetcher_->shooter()->balls_shot() >=
+ absolute_balls) {
return true;
}
}
}
+bool AutonomousActor::WaitForBallsShot(int num_wanted) {
+ return WaitUntilAbsoluteBallsShot(Balls() + num_wanted);
+}
+
} // namespace actors
} // namespace y2020
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index f2c48d9..3e698f3 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -29,8 +29,12 @@
void Reset();
void set_intake_goal(double intake_goal) { intake_goal_ = intake_goal; }
- void set_intake_preloading(bool intake_preloading) {
- intake_preloading_ = intake_preloading;
+ void set_roller(bool roller) {
+ if (roller) {
+ roller_voltage_ = 6.0;
+ } else {
+ roller_voltage_ = 0.0;
+ }
}
void set_roller_voltage(double roller_voltage) {
roller_voltage_ = roller_voltage;
@@ -50,6 +54,8 @@
void JustShoot();
bool DriveFwd();
bool WaitForBallsShot(int num_shot);
+ int Balls();
+ bool WaitUntilAbsoluteBallsShot(int absolute_balls);
void MaybeSendStartingPosition();
@@ -57,9 +63,8 @@
double intake_goal_ = 0.0;
double roller_voltage_ = 0.0;
- bool intake_preloading_ = false;
bool shooting_ = false;
- bool shooter_tracking_ = false;
+ bool shooter_tracking_ = true;
const float kRollerSpeedCompensation = 2.0;
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
diff --git a/y2020/actors/splines/target_aligned_1.json b/y2020/actors/splines/target_aligned_1.json
index ff3a581..3023e64 100644
--- a/y2020/actors/splines/target_aligned_1.json
+++ b/y2020/actors/splines/target_aligned_1.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [3.0719744897959185, 3.798077551020408, 5.082721428571428, 5.473700000000001, 6.981760204081634, 7.7916443877551025], "spline_y": [2.373798469387755, 2.373798469387755, 1.368425, 0.33512448979591836, 0.7261030612244898, 0.6981760204081633], "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": [3.558808136254461, 4.612191670587221, 5.440321231581482, 4.7136396743531925, 6.871486137929907, 7.777254400492434], "spline_y": [1.7894034762792161, 1.80016906071158, 1.714597797528574, 0.5544596492306492, 0.6629242570518812, 0.6561351444944246], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 4.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 3.0}, {"constraint_type": "VOLTAGE", "value": 9.0}]}
\ No newline at end of file
diff --git a/y2020/actors/splines/target_aligned_2.json b/y2020/actors/splines/target_aligned_2.json
index 754ce52..87632a9 100644
--- a/y2020/actors/splines/target_aligned_2.json
+++ b/y2020/actors/splines/target_aligned_2.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [7.987133673469388, 5.613335204081633, 5.613335204081633, 4.719669897959184, 3.630515306122449, 3.099901530612245], "spline_y": [0.6981760204081633, 0.6423219387755102, 0.6143948979591837, 1.2846438775510205, 1.9269658163265306, 2.4017255102040815], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
+{"spline_count": 1, "spline_x": [7.7800199105347785, 6.7273880265870964, 5.286246866759816, 5.830852655684724, 4.612191670587221, 3.558808136254461], "spline_y": [0.6429818532173177, 0.6377075298650245, 0.7746133820910694, 1.4985528813730233, 1.80016906071158, 1.7894034762792161], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 5.5}, {"constraint_type": "LATERAL_ACCELERATION", "value": 4.0}, {"constraint_type": "VOLTAGE", "value": 11.0}]}
\ No newline at end of file
diff --git a/y2020/constants.h b/y2020/constants.h
index 66fb79b..d1689a7 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -120,8 +120,8 @@
::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
intake;
- static constexpr double kIntakeRollerSupplyCurrentLimit() { return 30.0; }
- static constexpr double kIntakeRollerStatorCurrentLimit() { return 40.0; }
+ static constexpr double kIntakeRollerSupplyCurrentLimit() { return 40.0; }
+ static constexpr double kIntakeRollerStatorCurrentLimit() { return 60.0; }
static constexpr double kFeederSupplyCurrentLimit() { return 40.0; }
static constexpr double kFeederStatorCurrentLimit() { return 50.0; }