Improve infinite recharge auto
New autonomous described in y2020/actors/splines/README.md
We are getting mostly 3's now! The robot starts centered on the right
side of the target, and with the back bumper on the line.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Change-Id: Ib4dbe71551c344eeabc1912c908962a5cf8daa88
diff --git a/y2020/actors/auto_splines.cc b/y2020/actors/auto_splines.cc
index 77cfbb5..48da130 100644
--- a/y2020/actors/auto_splines.cc
+++ b/y2020/actors/auto_splines.cc
@@ -97,7 +97,7 @@
const float starty = 0.0;
std::vector<float> x_pos{0.0f + startx, 0.4f + startx, 0.4f + startx,
0.6f + startx, 0.6f + startx, 1.0f + startx};
- std::vector<float> y_pos{starty + 0.0f, starty + 0.0f, starty + 0.05f,
+ std::vector<float> y_pos{starty + 0.0f, starty + 0.0f, starty + 0.05f,
starty + 0.1f, starty + 0.15f, starty + 0.15f};
if (alliance == aos::Alliance::kRed) {
for (size_t ii = 0; ii < x_pos.size(); ++ii) {
@@ -141,6 +141,16 @@
alliance);
}
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TargetAligned3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(target_aligned_3_,
+ 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 bdaef75..2dd99b7 100644
--- a/y2020/actors/auto_splines.h
+++ b/y2020/actors/auto_splines.h
@@ -25,6 +25,8 @@
"splines/target_aligned_1.json")),
target_aligned_2_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
"splines/target_aligned_2.json")),
+ target_aligned_3_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/target_aligned_3.json")),
target_offset_1_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
"splines/target_offset_1.json")),
target_offset_2_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
@@ -53,6 +55,10 @@
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> TargetAligned3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
flatbuffers::Offset<frc971::MultiSpline> TargetOffset1(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder) {
@@ -70,6 +76,7 @@
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> target_aligned_1_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> target_aligned_2_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> target_aligned_3_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> target_offset_1_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> target_offset_2_;
};
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index 077e87e..c8f1ebe 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -89,10 +89,11 @@
}
void AutonomousActor::Replan() {
- sent_starting_position_ = false;
+ LOG(INFO) << "Alliance " << static_cast<int>(alliance_);
if (alliance_ == aos::Alliance::kInvalid) {
return;
}
+ sent_starting_position_ = false;
if (FLAGS_spline_auto) {
test_spline_ = PlanSpline(std::bind(&AutonomousSplines::TestSpline,
&auto_splines_, std::placeholders::_1),
@@ -105,8 +106,12 @@
SplineDirection::kForward),
PlanSpline(std::bind(&AutonomousSplines::TargetAligned2, &auto_splines_,
std::placeholders::_1, alliance_),
- SplineDirection::kBackward)};
+ SplineDirection::kBackward),
+ PlanSpline(std::bind(&AutonomousSplines::TargetAligned3, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward)};
starting_position_ = target_aligned_splines_.value()[0].starting_position();
+ CHECK(starting_position_);
} else if (FLAGS_target_offset) {
target_offset_splines_ = {
PlanSpline(std::bind(&AutonomousSplines::TargetOffset1, &auto_splines_,
@@ -141,6 +146,7 @@
Reset();
if (!user_indicated_safe_to_reset_) {
AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
+ CHECK(starting_position_);
SendStartingPosition(starting_position_.value());
}
// Clear this so that we don't accidentally resend things as soon as we replan
@@ -181,6 +187,8 @@
localizer_control_builder.add_y(start(1));
localizer_control_builder.add_theta(start(2));
localizer_control_builder.add_theta_uncertainty(0.00001);
+ LOG(INFO) << "User button pressed, x: " << start(0) << " y: " << start(1)
+ << " theta: " << start(2);
if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
@@ -193,9 +201,14 @@
// Spin up.
set_shooting(true);
+ set_preloading(true);
+ set_shooter_tracking(true);
SendSuperstructureGoal();
if (!WaitForBallsShot(3)) return;
+ LOG(INFO) << "Shot balls";
+ set_shooter_tracking(false);
+ // Drive and intake 3 balls in front of the trench run
set_shooting(false);
ExtendIntake();
SendSuperstructureGoal();
@@ -204,9 +217,11 @@
splines[0].Start();
if (!splines[0].WaitForSplineDistanceRemaining(0.02)) return;
+
std::this_thread::sleep_for(chrono::milliseconds(200));
RetractIntake();
+ // Drive back to shooting position
if (!splines[1].WaitForPlan()) return;
splines[1].Start();
@@ -220,27 +235,30 @@
// 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;
+ set_shooter_tracking(true);
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 (!WaitUntilAbsoluteBallsShot(3 + balls)) return;
+ set_shooting(false);
+ set_roller_voltage(0.0);
+ set_shooter_tracking(false);
+ set_preloading(false);
+ SendSuperstructureGoal();
+
+ // Drive close to the rendezvous point in the center of the field so that the
+ // driver can intake balls there right after auto ends.
+ if (!splines[2].WaitForPlan()) return;
+ splines[2].Start();
+
+ if (!splines[2].WaitForSplineDistanceRemaining(0.02)) return;
+
LOG(INFO) << "Took "
<< chrono::duration<double>(aos::monotonic_clock::now() -
start_time)
@@ -341,7 +359,7 @@
builder.MakeBuilder<superstructure::Goal>();
superstructure_builder.add_intake(intake_offset);
- superstructure_builder.add_intake_preloading(true);
+ superstructure_builder.add_intake_preloading(preloading_);
superstructure_builder.add_roller_voltage(roller_voltage_);
superstructure_builder.add_roller_speed_compensation(
kRollerSpeedCompensation);
@@ -377,6 +395,10 @@
::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
frc971::controls::kLoopFrequency / 2);
+ superstructure_status_fetcher_.Fetch();
+ CHECK(superstructure_status_fetcher_.get() != nullptr);
+ int last_balls = superstructure_status_fetcher_->shooter()->balls_shot();
+ LOG(INFO) << "Waiting for balls, started with " << absolute_balls;
while (true) {
if (ShouldCancel()) {
return false;
@@ -384,10 +406,19 @@
phased_loop.SleepUntilNext();
superstructure_status_fetcher_.Fetch();
CHECK(superstructure_status_fetcher_.get() != nullptr);
+ if (superstructure_status_fetcher_->shooter()->balls_shot() != last_balls) {
+ LOG(INFO) << "Shot "
+ << superstructure_status_fetcher_->shooter()->balls_shot() -
+ last_balls
+ << " balls, now at "
+ << superstructure_status_fetcher_->shooter()->balls_shot();
+ }
if (superstructure_status_fetcher_->shooter()->balls_shot() >=
absolute_balls) {
return true;
}
+
+ last_balls = superstructure_status_fetcher_->shooter()->balls_shot();
}
}
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index 7b60ca4..1877d58 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -43,6 +43,7 @@
shooter_tracking_ = shooter_tracking;
}
void set_shooting(bool shooting) { shooting_ = shooting; }
+ void set_preloading(bool preloading) { preloading_ = preloading; }
void SendSuperstructureGoal();
void ExtendIntake();
@@ -64,6 +65,7 @@
double intake_goal_ = 0.0;
double roller_voltage_ = 0.0;
bool shooting_ = false;
+ bool preloading_ = true;
bool shooter_tracking_ = true;
const float kRollerSpeedCompensation = 2.0;
@@ -80,7 +82,8 @@
aos::TimerHandler *button_poll_;
std::optional<std::array<SplineHandle, 2>> target_offset_splines_;
- std::optional<std::array<SplineHandle, 2>> target_aligned_splines_;
+ // Max number of splines is 5
+ std::optional<std::array<SplineHandle, 3>> target_aligned_splines_;
std::optional<SplineHandle> barrel_spline_;
std::optional<SplineHandle> slalom_spline_;
diff --git a/y2020/actors/splines/README.md b/y2020/actors/splines/README.md
index 058f5bc..2487e4e 100644
--- a/y2020/actors/splines/README.md
+++ b/y2020/actors/splines/README.md
@@ -4,21 +4,22 @@
<br>
# Target Aligned - Infinite Recharge
-## After shooting balls initially, collects power cells and returns to shooting position
### [Target Aligned 1](target_aligned_1.json)
-After shooting the pre-loaded balls, this spline directs the robot to the power cells, allowing the robot to collect all three balls available
+After shooting the pre-loaded balls, drives to the power cells in front of the trench run, allowing the robot to collect all three balls available.
### [Target Aligned 2](target_aligned_2.json)
-After collecting the 3 balls using the target_aligned_1 spline, this spline directs the robot back towards the shooter tower, setting it up to shoot the balls it collected
+After collecting the 3 balls, drives back to the shooting position, setting it up to shoot the balls it collected.
+
+### [Target Aligned 3](target_aligned_3.json)
+After shooting the second set of balls, drives to the rendezvous point in the center of the field, aligning the robot with the two balls there that the driver will intake at the start of teleop.
<br>
# Target Offset - Infinite Recharge
-## After starting with 3 balls, collects power cells and heads to primary shooting position
-
+Has not been tested.
### [Target Offset 1](target_offset_1.json)
-When we start offset from the target with the 3 pre-loaded balls in the robot, this spline directs the robot to the additional 2 power cells, allowing the robot to collect both balls available
+When we start offset from the target with the 3 pre-loaded balls in the robot, drives to the additional 2 power cells, allowing the robot to collect both balls available.
### [Target Offset 2](target_offset_2.json)
-After collecting the 2 balls using the target_offset_1 spline, this spline directs the robot towards the shooter tower, setting it up to shoot the 5 balls.
\ No newline at end of file
+After collecting the 2 balls using the target_offset_1 spline, drives the robot towards the shooter tower, setting it up to shoot the 5 balls.
diff --git a/y2020/actors/splines/target_aligned_1.json b/y2020/actors/splines/target_aligned_1.json
index 3023e64..c583555 100644
--- a/y2020/actors/splines/target_aligned_1.json
+++ b/y2020/actors/splines/target_aligned_1.json
@@ -1 +1,9 @@
-{"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
+{"spline_count": 1, "spline_x": [2.707560494479258,
+ 4.261038436854981,
+ 5.087225880825187,
+ 4.888021289429735,
+ 6.519158879183451,
+ 7.801118784873183
+], "spline_y": [
+1.79, 1.79, 1.6771446569752932, 0.22320548149895927, 0.7899924087277742, 0.7361473021539742
+], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 4.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 3.0}, {"constraint_type": "VOLTAGE", "value": 9.0}]}
diff --git a/y2020/actors/splines/target_aligned_2.json b/y2020/actors/splines/target_aligned_2.json
index 87632a9..e5da9e2 100644
--- a/y2020/actors/splines/target_aligned_2.json
+++ b/y2020/actors/splines/target_aligned_2.json
@@ -1 +1,15 @@
-{"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
+{"spline_count": 1, "spline_x": [
+ 7.801118784873183,
+ 6.519158879183451,
+ 4.888021289429735,
+ 5.087225880825187,
+ 4.261038436854981,
+2.707560494479258
+], "spline_y": [
+0.7361473021539742,
+0.7899924087277742,
+0.22320548149895927,
+1.6771446569752932,
+1.79,
+1.79
+], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 5.5}, {"constraint_type": "LATERAL_ACCELERATION", "value": 4.0}, {"constraint_type": "VOLTAGE", "value": 12.0}]}
diff --git a/y2020/actors/splines/target_aligned_3.json b/y2020/actors/splines/target_aligned_3.json
new file mode 100644
index 0000000..f6d9b81
--- /dev/null
+++ b/y2020/actors/splines/target_aligned_3.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [2.707560494479258, 3.150985033912021, 3.587658957056669, 5.119307937915229, 5.46161202948668, 5.913474803230813], "spline_y": [1.79, 1.79, 1.9673439619806328, 1.4950555659255436, 1.193199959383944, 2.1384214060453353], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 4.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 3.5}, {"constraint_type": "VOLTAGE", "value": 11.0}]}
\ No newline at end of file