Second Auto
Cable protector side 3 piece autonomous, no balance.
Mirrored version of current autonomous for the three
game pieces.
Signed-off-by: Nathan Leong <nathanrleong@gmail.com>
Change-Id: I9fc841b844b8cd4bb2b02fa46188a364cf58160c
diff --git a/y2023/autonomous/auto_splines.cc b/y2023/autonomous/auto_splines.cc
index c99e417..20593f9 100644
--- a/y2023/autonomous/auto_splines.cc
+++ b/y2023/autonomous/auto_splines.cc
@@ -142,6 +142,46 @@
alliance);
}
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SplineCable1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(splinecable_1_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SplineCable2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(splinecable_2_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SplineCable3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(splinecable_3_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SplineCable4(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(splinecable_4_, builder->fbb()),
+ alliance);
+}
+
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
diff --git a/y2023/autonomous/auto_splines.h b/y2023/autonomous/auto_splines.h
index 27442f3..6921e64 100644
--- a/y2023/autonomous/auto_splines.h
+++ b/y2023/autonomous/auto_splines.h
@@ -4,7 +4,6 @@
#include "aos/events/event_loop.h"
#include "aos/flatbuffer_merge.h"
#include "frc971/control_loops/control_loops_generated.h"
-#include "frc971/input/joystick_state_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
#include "frc971/input/joystick_state_generated.h"
/*
@@ -29,7 +28,15 @@
spline_3_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
"splines/spline.2.json")),
spline_4_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/spline.3.json")) {}
+ "splines/spline.3.json")),
+ splinecable_1_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/splinecable.0.json")),
+ splinecable_2_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/splinecable.1.json")),
+ splinecable_3_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/splinecable.2.json")),
+ splinecable_4_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/splinecable.3.json")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
@@ -59,6 +66,22 @@
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> SplineCable1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> SplineCable2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> SplineCable3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> SplineCable4(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
private:
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
@@ -66,6 +89,10 @@
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_2_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_3_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_4_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> splinecable_1_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> splinecable_2_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> splinecable_3_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> splinecable_4_;
};
} // namespace autonomous
diff --git a/y2023/autonomous/autonomous_actor.cc b/y2023/autonomous/autonomous_actor.cc
index 26197e3..317ff0c 100644
--- a/y2023/autonomous/autonomous_actor.cc
+++ b/y2023/autonomous/autonomous_actor.cc
@@ -14,6 +14,7 @@
DEFINE_bool(spline_auto, false, "Run simple test S-spline auto mode.");
DEFINE_bool(charged_up, true, "If true run charged up autonomous mode");
+DEFINE_bool(charged_up_cable, false, "If true run cable side autonomous mode");
namespace y2023 {
namespace autonomous {
@@ -126,11 +127,28 @@
SplineDirection::kBackward),
PlanSpline(std::bind(&AutonomousSplines::Spline4, &auto_splines_,
std::placeholders::_1, alliance_),
- SplineDirection::kForward),
- };
+ SplineDirection::kForward)};
starting_position_ = charged_up_splines_.value()[0].starting_position();
CHECK(starting_position_);
+ } else if (FLAGS_charged_up_cable) {
+ charged_up_cable_splines_ = {
+ PlanSpline(std::bind(&AutonomousSplines::SplineCable1, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kBackward),
+ PlanSpline(std::bind(&AutonomousSplines::SplineCable2, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(std::bind(&AutonomousSplines::SplineCable3, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kBackward),
+ PlanSpline(std::bind(&AutonomousSplines::SplineCable4, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward)};
+
+ starting_position_ =
+ charged_up_cable_splines_.value()[0].starting_position();
+ CHECK(starting_position_);
}
is_planned_ = true;
@@ -222,7 +240,7 @@
}
}
-// Charged Up 3 Game Object Autonomous.
+// Charged Up 3 Game Object Autonomous (non-cable side)
void AutonomousActor::ChargedUp() {
aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
@@ -401,6 +419,127 @@
}
}
+// Charged Up 3 Game Object Autonomous (cable side)
+void AutonomousActor::ChargedUpCableSide() {
+ aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
+
+ CHECK(charged_up_cable_splines_);
+
+ auto &splines = *charged_up_cable_splines_;
+
+ AOS_LOG(INFO, "Going to preload");
+
+ // Tell the superstructure a cone was preloaded
+ if (!WaitForPreloaded()) return;
+ AOS_LOG(INFO, "Moving arm");
+
+ // Place first cone on mid level
+ MidConeScore();
+
+ // Wait until the arm is at the goal to spit
+ if (!WaitForArmGoal(0.10)) return;
+ Spit();
+ if (!WaitForArmGoal(0.01)) return;
+
+ std::this_thread::sleep_for(chrono::milliseconds(100));
+
+ AOS_LOG(
+ INFO, "Placed first cone %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ // Drive and intake the cube nearest to the starting zone
+ if (!splines[0].WaitForPlan()) return;
+ splines[0].Start();
+
+ // Move arm into position to pickup a cube and start cube intake
+ PickupCube();
+
+ std::this_thread::sleep_for(chrono::milliseconds(500));
+
+ IntakeCube();
+
+ AOS_LOG(
+ INFO, "Turning on rollers %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!splines[0].WaitForSplineDistanceRemaining(0.02)) return;
+
+ AOS_LOG(
+ INFO, "Got there %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ // Drive back to grid and place cube on high level
+ if (!splines[1].WaitForPlan()) return;
+ splines[1].Start();
+
+ std::this_thread::sleep_for(chrono::milliseconds(300));
+ HighCubeScore();
+
+ if (!splines[1].WaitForSplineDistanceRemaining(0.08)) return;
+ AOS_LOG(
+ INFO, "Back for first cube %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!WaitForArmGoal(0.10)) return;
+
+ AOS_LOG(
+ INFO, "Arm in place for first cube %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ Spit();
+
+ if (!splines[1].WaitForSplineDistanceRemaining(0.08)) return;
+
+ AOS_LOG(
+ INFO, "Finished spline back %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!WaitForArmGoal(0.05)) return;
+
+ AOS_LOG(
+ INFO, "Placed first cube %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ // Drive and intake the cube second nearest to the starting zone
+ if (!splines[2].WaitForPlan()) return;
+ splines[2].Start();
+
+ std::this_thread::sleep_for(chrono::milliseconds(200));
+ PickupCube();
+
+ std::this_thread::sleep_for(chrono::milliseconds(500));
+ IntakeCube();
+
+ if (!splines[2].WaitForSplineDistanceRemaining(0.05)) return;
+ AOS_LOG(
+ INFO, "Picked up second cube %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ // Drive back to grid and place object on mid level
+ if (!splines[3].WaitForPlan()) return;
+ splines[3].Start();
+
+ AOS_LOG(
+ INFO, "Driving back %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ MidCubeScore();
+
+ if (!splines[3].WaitForSplineDistanceRemaining(0.07)) return;
+ AOS_LOG(
+ INFO, "Got back from second cube at %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!WaitForArmGoal(0.05)) return;
+ Spit();
+
+ if (!splines[3].WaitForSplineDistanceRemaining(0.02)) return;
+
+ AOS_LOG(
+ INFO, "Placed second cube %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+}
+
void AutonomousActor::SendSuperstructureGoal() {
auto builder = superstructure_goal_sender_.MakeBuilder();
diff --git a/y2023/autonomous/autonomous_actor.h b/y2023/autonomous/autonomous_actor.h
index 46c8a78..31ae4f5 100644
--- a/y2023/autonomous/autonomous_actor.h
+++ b/y2023/autonomous/autonomous_actor.h
@@ -58,6 +58,7 @@
void MaybeSendStartingPosition();
void SplineAuto();
void ChargedUp();
+ void ChargedUpCableSide();
void Replan();
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
@@ -90,6 +91,7 @@
std::optional<SplineHandle> test_spline_;
std::optional<std::array<SplineHandle, 4>> charged_up_splines_;
+ std::optional<std::array<SplineHandle, 4>> charged_up_cable_splines_;
// List of arm angles from arm::PointsList
const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
diff --git a/y2023/autonomous/splines/splinecable.0.json b/y2023/autonomous/splines/splinecable.0.json
new file mode 100644
index 0000000..3fdcfbe
--- /dev/null
+++ b/y2023/autonomous/splines/splinecable.0.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-6.468141183035714, -6.142335156250001, -5.834629464285714, -2.6851712053571433, -2.2145624999999995, -1.7620541294642855], "spline_y": [-3.493364620535714, -3.493364620535714, -3.4752642857142853, -3.0951572544642856, -3.0770569196428568, -3.0770569196428568], "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
diff --git a/y2023/autonomous/splines/splinecable.1.json b/y2023/autonomous/splines/splinecable.1.json
new file mode 100644
index 0000000..b9b4fdd
--- /dev/null
+++ b/y2023/autonomous/splines/splinecable.1.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-1.7620541294642855, -2.2145624999999995, -2.6851712053571433, -5.8165291294642865, -6.124234821428573, -6.450040848214286], "spline_y": [-3.0770569196428568, -3.0770569196428568, -3.0951572544642856, -2.9141539062499993, -2.932254241071428, -2.932254241071428], "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
diff --git a/y2023/autonomous/splines/splinecable.2.json b/y2023/autonomous/splines/splinecable.2.json
new file mode 100644
index 0000000..cef0375
--- /dev/null
+++ b/y2023/autonomous/splines/splinecable.2.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-6.450040848214286, -6.124234821428573, -5.8165291294642865, -2.9023752232142854, -2.1059604910714285, -1.6353517857142856], "spline_y": [-2.932254241071428, -2.932254241071428, -2.9141539062499993, -3.2218595982142855, -2.6607492187499995, -2.244441517857142], "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
diff --git a/y2023/autonomous/splines/splinecable.3.json b/y2023/autonomous/splines/splinecable.3.json
new file mode 100644
index 0000000..60821fe
--- /dev/null
+++ b/y2023/autonomous/splines/splinecable.3.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-1.6353517857142856, -2.1059604910714285, -2.9023752232142854, -5.8165291294642865, -6.124234821428573, -6.450040848214286], "spline_y": [-2.244441517857142, -2.6607492187499995, -3.2218595982142855, -2.9141539062499993, -2.932254241071428, -2.932254241071428], "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