Add 3 game piece auto
Change-Id: I3ee12fe6f253705e825774620d16b00c266db30d
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/autonomous/auto_splines.cc b/y2023/autonomous/auto_splines.cc
index 09cf4ea..c99e417 100644
--- a/y2023/autonomous/auto_splines.cc
+++ b/y2023/autonomous/auto_splines.cc
@@ -142,16 +142,6 @@
alliance);
}
-flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::Spline5(
- aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
- *builder,
- aos::Alliance alliance) {
- return FixSpline(
- builder,
- aos::CopyFlatBuffer<frc971::MultiSpline>(spline_5_, 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 2847957..27442f3 100644
--- a/y2023/autonomous/auto_splines.h
+++ b/y2023/autonomous/auto_splines.h
@@ -29,9 +29,7 @@
spline_3_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
"splines/spline.2.json")),
spline_4_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/spline.3.json")),
- spline_5_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/spline.4.json")) {}
+ "splines/spline.3.json")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
@@ -61,10 +59,6 @@
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
aos::Alliance alliance);
- flatbuffers::Offset<frc971::MultiSpline> Spline5(
- aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
- *builder,
- aos::Alliance alliance);
private:
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
@@ -72,7 +66,6 @@
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_2_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_3_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_4_;
- aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_5_;
};
} // namespace autonomous
diff --git a/y2023/autonomous/autonomous_actor.cc b/y2023/autonomous/autonomous_actor.cc
index 77965e4..c3ba65b 100644
--- a/y2023/autonomous/autonomous_actor.cc
+++ b/y2023/autonomous/autonomous_actor.cc
@@ -12,14 +12,27 @@
#include "y2023/control_loops/drivetrain/drivetrain_base.h"
#include "y2023/control_loops/superstructure/arm/generated_graph.h"
-DEFINE_bool(spline_auto, true, "Run simple test S-spline auto mode.");
+DEFINE_bool(spline_auto, false, "Run simple test S-spline auto mode.");
DEFINE_bool(charged_up, true, "If true run charged up autonomous mode");
namespace y2023 {
namespace autonomous {
-using ::aos::monotonic_clock;
using ::frc971::ProfileParametersT;
+
+ProfileParametersT MakeProfileParameters(float max_velocity,
+ float max_acceleration) {
+ ProfileParametersT result;
+ result.max_velocity = max_velocity;
+ result.max_acceleration = max_acceleration;
+ return result;
+}
+
+using ::aos::monotonic_clock;
+using frc971::CreateProfileParameters;
+using ::frc971::ProfileParametersT;
+using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
using frc971::control_loops::drivetrain::LocalizerControl;
namespace chrono = ::std::chrono;
@@ -34,8 +47,16 @@
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
auto_splines_(),
- arm_goal_position_(control_loops::superstructure::arm::NeutralIndex()),
+ arm_goal_position_(control_loops::superstructure::arm::StartingIndex()),
+ superstructure_goal_sender_(
+ event_loop->MakeSender<::y2023::control_loops::superstructure::Goal>(
+ "/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::y2023::control_loops::superstructure::Status>(
+ "/superstructure")),
points_(control_loops::superstructure::arm::PointList()) {
+ drivetrain_status_fetcher_.Fetch();
replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
event_loop->OnRun([this, event_loop]() {
@@ -74,6 +95,12 @@
}
void AutonomousActor::Replan() {
+ if (!drivetrain_status_fetcher_.Fetch()) {
+ replan_timer_->Setup(event_loop()->monotonic_now() + chrono::seconds(1));
+ AOS_LOG(INFO, "Drivetrain not up, replanning in 1 second");
+ return;
+ }
+
if (alliance_ == aos::Alliance::kInvalid) {
return;
}
@@ -86,6 +113,7 @@
starting_position_ = test_spline_->starting_position();
} else if (FLAGS_charged_up) {
+ AOS_LOG(INFO, "Charged up replanning!");
charged_up_splines_ = {
PlanSpline(std::bind(&AutonomousSplines::Spline1, &auto_splines_,
std::placeholders::_1, alliance_),
@@ -99,9 +127,6 @@
PlanSpline(std::bind(&AutonomousSplines::Spline4, &auto_splines_,
std::placeholders::_1, alliance_),
SplineDirection::kForward),
- PlanSpline(std::bind(&AutonomousSplines::Spline5, &auto_splines_,
- std::placeholders::_1, alliance_),
- SplineDirection::kBackward),
};
starting_position_ = charged_up_splines_.value()[0].starting_position();
@@ -129,6 +154,12 @@
CHECK(joystick_state_fetcher_.get() != nullptr)
<< "Expect at least one JoystickState message before running auto...";
alliance_ = joystick_state_fetcher_->alliance();
+
+ wrist_goal_ = 0.0;
+ roller_goal_ = control_loops::superstructure::RollerGoal::IDLE;
+ arm_goal_position_ = control_loops::superstructure::arm::StartingIndex();
+ preloaded_ = false;
+ SendSuperstructureGoal();
}
bool AutonomousActor::RunAction(
@@ -155,6 +186,8 @@
}
if (FLAGS_spline_auto) {
SplineAuto();
+ } else if (FLAGS_charged_up) {
+ ChargedUp();
} else {
AOS_LOG(WARNING, "No auto mode selected.");
}
@@ -197,15 +230,21 @@
auto &splines = *charged_up_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()) return;
+ 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",
@@ -217,21 +256,49 @@
// 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.02)) return;
+ 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()) return;
+ 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));
@@ -240,43 +307,110 @@
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.02)) return;
+ 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.02)) return;
+ if (!splines[3].WaitForSplineDistanceRemaining(0.05)) return;
+ AOS_LOG(
+ INFO, "Got back from second cube at %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
- if (!WaitForArmGoal()) return;
+ if (!WaitForArmGoal(0.05)) return;
+
+ if (!splines[3].WaitForSplineDistanceRemaining(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));
+ InitializeEncoders();
- // Drive onto charging station
- if (!splines[4].WaitForPlan()) return;
- splines[4].Start();
+ const ProfileParametersT kDrive = MakeProfileParameters(2.0, 4.0);
+ const ProfileParametersT kTurn = MakeProfileParameters(3.0, 4.5);
+ StartDrive(0.0, 0.0, kDrive, kTurn);
- if (!splines[4].WaitForSplineDistanceRemaining(0.02)) return;
+ {
+ double side_scalar = (alliance_ == aos::Alliance::kRed) ? 1.0 : -1.0;
+ StartDrive(6.33 - std::abs(X()), 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileNear(0.01)) return;
+
+ AOS_LOG(
+ INFO, "Done backing up %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ const ProfileParametersT kInPlaceTurn = MakeProfileParameters(2.0, 4.5);
+ StartDrive(0.0, aos::math::NormalizeAngle(M_PI / 2.0 - Theta()), kDrive,
+ kInPlaceTurn);
+
+ std::this_thread::sleep_for(chrono::milliseconds(400));
+ StopSpitting();
+
+ AOS_LOG(
+ INFO, "Roller off %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ Balance();
+ if (!WaitForTurnProfileNear(0.03)) return;
+
+ AOS_LOG(
+ INFO, "Done turning %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ const ProfileParametersT kDrive = MakeProfileParameters(1.4, 3.0);
+ const ProfileParametersT kFinalTurn = MakeProfileParameters(3.0, 4.5);
+ const double kDriveDistance = 3.12;
+ StartDrive(-kDriveDistance, 0.0, kDrive, kFinalTurn);
+
+ const ProfileParametersT kFastTurn = MakeProfileParameters(5.0, 8.0);
+ if (!WaitForDriveProfileNear(kDriveDistance - 0.4)) return;
+
+ AOS_LOG(
+ INFO, "Turning %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+ StartDrive(0.0, -side_scalar * M_PI / 2.0, kDrive, kFastTurn);
+ if (!WaitForDriveProfileDone()) return;
+ if (!WaitForTurnProfileDone()) return;
+ AOS_LOG(
+ INFO, "Done %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+ }
}
void AutonomousActor::SendSuperstructureGoal() {
auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), wrist_goal_,
+ CreateProfileParameters(*builder.fbb(), 12.0, 90.0));
+
control_loops::superstructure::Goal::Builder superstructure_builder =
builder.MakeBuilder<control_loops::superstructure::Goal>();
superstructure_builder.add_arm_goal_position(arm_goal_position_);
superstructure_builder.add_preloaded_with_cone(preloaded_);
superstructure_builder.add_roller_goal(roller_goal_);
- superstructure_builder.add_wrist(wrist_goal_);
+ superstructure_builder.add_wrist(wrist_offset);
if (builder.Send(superstructure_builder.Finish()) !=
aos::RawSender::Error::kOk) {
@@ -314,8 +448,21 @@
void AutonomousActor::MidConeScore() {
set_arm_goal_position(
- control_loops::superstructure::arm::ScoreFrontMidConeUpIndex());
- set_wrist_goal(0.05);
+ control_loops::superstructure::arm::ScoreFrontMidConeUpAutoIndex());
+ set_wrist_goal(0.0);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::Neutral() {
+ set_arm_goal_position(control_loops::superstructure::arm::NeutralIndex());
+ set_wrist_goal(0.0);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::Balance() {
+ set_arm_goal_position(
+ control_loops::superstructure::arm::GroundPickupFrontConeUpIndex());
+ set_wrist_goal(0.0);
SendSuperstructureGoal();
}
@@ -329,14 +476,14 @@
void AutonomousActor::MidCubeScore() {
set_arm_goal_position(
control_loops::superstructure::arm::ScoreFrontMidCubeIndex());
- set_wrist_goal(0.6);
+ set_wrist_goal(1.0);
SendSuperstructureGoal();
}
void AutonomousActor::PickupCube() {
set_arm_goal_position(
control_loops::superstructure::arm::GroundPickupBackCubeIndex());
- set_wrist_goal(0.6);
+ set_wrist_goal(1.0);
SendSuperstructureGoal();
}
@@ -345,13 +492,18 @@
SendSuperstructureGoal();
}
+void AutonomousActor::StopSpitting() {
+ set_roller_goal(control_loops::superstructure::RollerGoal::IDLE);
+ SendSuperstructureGoal();
+}
+
void AutonomousActor::IntakeCube() {
set_roller_goal(control_loops::superstructure::RollerGoal::INTAKE_CUBE);
SendSuperstructureGoal();
}
-[[nodiscard]] bool AutonomousActor::WaitForArmGoal() {
- constexpr double kEpsTheta = 0.01;
+[[nodiscard]] bool AutonomousActor::WaitForArmGoal(double distance_to_go) {
+ constexpr double kEpsTheta = 0.10;
::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
@@ -367,24 +519,15 @@
superstructure_status_fetcher_.Fetch();
CHECK(superstructure_status_fetcher_.get() != nullptr);
- // Check that the status had the right goal
- at_goal = (std::abs(points_[arm_goal_position_](0) -
- superstructure_status_fetcher_->arm()->theta0()) <
- kEpsTheta &&
- std::abs(points_[arm_goal_position_](1) -
- superstructure_status_fetcher_->arm()->theta1()) <
- kEpsTheta &&
- std::abs(points_[arm_goal_position_](2) -
- superstructure_status_fetcher_->arm()->theta2()) <
- kEpsTheta) &&
+ at_goal = (arm_goal_position_ ==
+ superstructure_status_fetcher_->arm()->current_node() &&
+ superstructure_status_fetcher_->arm()->path_distance_to_go() <
+ distance_to_go) &&
(std::abs(wrist_goal_ -
superstructure_status_fetcher_->wrist()->position()) <
kEpsTheta);
}
- set_preloaded(false);
- SendSuperstructureGoal();
-
return true;
}
diff --git a/y2023/autonomous/autonomous_actor.h b/y2023/autonomous/autonomous_actor.h
index d13003d..46c8a78 100644
--- a/y2023/autonomous/autonomous_actor.h
+++ b/y2023/autonomous/autonomous_actor.h
@@ -41,11 +41,14 @@
void HighCubeScore();
void MidCubeScore();
void MidConeScore();
+ void Neutral();
void PickupCube();
void Spit();
+ void StopSpitting();
void IntakeCube();
+ void Balance();
- [[nodiscard]] bool WaitForArmGoal();
+ [[nodiscard]] bool WaitForArmGoal(double distance_to_go = 0.01);
[[nodiscard]] bool WaitForPreloaded();
@@ -62,7 +65,7 @@
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<aos::RobotState> robot_state_fetcher_;
- double wrist_goal_ = 0.0;
+ double wrist_goal_;
control_loops::superstructure::RollerGoal roller_goal_ =
control_loops::superstructure::RollerGoal::IDLE;
@@ -86,7 +89,7 @@
superstructure_status_fetcher_;
std::optional<SplineHandle> test_spline_;
- std::optional<std::array<SplineHandle, 5>> charged_up_splines_;
+ std::optional<std::array<SplineHandle, 4>> charged_up_splines_;
// List of arm angles from arm::PointsList
const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
diff --git a/y2023/autonomous/splines/spline.0.json b/y2023/autonomous/splines/spline.0.json
index e6e24ed..b547160 100644
--- a/y2023/autonomous/splines/spline.0.json
+++ b/y2023/autonomous/splines/spline.0.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [-6.450466090790975, -6.021160733648118, -5.591855376505261, -3.3652785421474576, -2.7749836760760287, -1.7732711760760287], "spline_y": [0.9493418961252269, 0.9493418961252269, 0.9314541729109411, 0.5975544198946889, 0.5975544198946889, 0.5796666966804032], "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": [6.530466090790975, 6.021160733648118, 5.591855376505261, 3.197458857773618, 2.607163991702189, 1.6230056318540846], "spline_y": [0.9493418961252269, 0.9493418961252269, 0.9314541729109411, 0.5106030443873093, 0.5106030443873093, 0.4224994345862587], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 4.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 12.0}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/spline.1.json b/y2023/autonomous/splines/spline.1.json
index 032a081..ada494a 100644
--- a/y2023/autonomous/splines/spline.1.json
+++ b/y2023/autonomous/splines/spline.1.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [-1.7732711760760287, -2.7749836760760287, -3.3652785421474576, -5.591855376505261, -6.021160733648118, -6.450466090790975], "spline_y": [0.5796666966804032, 0.5975544198946889, 0.5975544198946889, 0.40105062588141127, 0.41893834909569705, 0.41893834909569705], "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": [1.6230056318540846, 2.595606822913154, 3.185901688984583, 5.530672564287547, 5.959977921430404, 6.389283278573261], "spline_y": [0.4224994345862587, 0.509568426046465, 0.509568426046465, 0.28192333408803366, 0.29981105730231944, 0.29981105730231944], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.5}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 12.0}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/spline.2.json b/y2023/autonomous/splines/spline.2.json
index 4ca06a8..bbefb4e 100644
--- a/y2023/autonomous/splines/spline.2.json
+++ b/y2023/autonomous/splines/spline.2.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [-6.450466090790975, -6.021160733648118, -5.591855376505261, -3.605574338541678, -3.0269522872367363, -1.6929070022836754], "spline_y": [0.41893834909569705, 0.41893834909569705, 0.40105062588141127, 0.5475210271634618, 0.515375357646521, -0.3364848845524211], "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": [6.389283278573261, 5.998153290606758, 4.236658486151713, 2.789187490908938, 2.3614674084109164, 1.571420503593167], "spline_y": [0.29981105730231944, 0.29981105730231944, 0.6546640312799279, 0.8695373532912908, -0.061240797065554964, -0.6670628799510845], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.5}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.5}, {"constraint_type": "VOLTAGE", "value": 12.0}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/spline.3.json b/y2023/autonomous/splines/spline.3.json
index a56d24e..d888718 100644
--- a/y2023/autonomous/splines/spline.3.json
+++ b/y2023/autonomous/splines/spline.3.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [-1.6929070022836754, -3.0269522872367363, -3.605574338541678, -5.591855376505261, -6.021160733648118, -6.450466090790975], "spline_y": [-0.3364848845524211, 0.515375357646521, 0.5475210271634618, 0.40105062588141127, 0.41893834909569705, 0.41893834909569705], "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": [1.571420503593167, 3.182907251060901, 3.1860855438536575, 5.26674552341491, 4.916110025955861, 6.379956328994636], "spline_y": [-0.6670628799510845, 0.568653951467349, 0.5015288933650817, 0.5325195989567781, 0.24498490138963108, 0.24723266038664643], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.5}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.5}, {"constraint_type": "VOLTAGE", "value": 12.0}, {"constraint_type": "VELOCITY", "value": 1.5, "start_distance": 4.1, "end_distance": 10.0}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/spline.4.json b/y2023/autonomous/splines/spline.4.json
deleted file mode 100644
index 4eee822..0000000
--- a/y2023/autonomous/splines/spline.4.json
+++ /dev/null
@@ -1 +0,0 @@
-{"spline_count": 1, "spline_x": [-6.450466090790975, -6.448323209188465, -6.468936183333308, -5.63485982210851, -5.224861021501398, -4.383040925048516], "spline_y": [0.41893834909569705, -0.2089748700255587, -1.0435424455861884, -0.6390449134590346, -0.8779649804883709, -0.8766708249234052], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 24b9805..557d609 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -382,17 +382,19 @@
control_alpha_rolls=[(0.40, 0.0), (.95, -np.pi / 2.0)],
))
+points['Starting'] = np.array((np.pi, -0.125053863467887, 0.0))
+
points['ScoreFrontMidConeUpAuto'] = to_theta_with_circular_index_and_roll(
0.58, 0.97, -np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
- name="NeutralToScoreFrontMidConeUpAuto",
- start=points['Neutral'],
+ name="StartingToScoreFrontMidConeUpAuto",
+ start=points['Starting'],
control1=np.array([2.99620794024176, 0.23620211875551145]),
control2=np.array([2.728197531599509, 0.5677148040671784]),
end=points['ScoreFrontMidConeUpAuto'],
- control_alpha_rolls=[(0.20, 0.0), (.90, -np.pi / 2.0)],
+ control_alpha_rolls=[(0.20, 0.0), (.85, -np.pi / 2.0)],
vmax=10.0,
alpha_unitizer=np.matrix(
f"{1.0 / 20.0} 0 0; 0 {1.0 / 25.0} 0; 0 0 {1.0 / 100.0}"),
@@ -444,17 +446,6 @@
control_alpha_rolls=[],
))
-# Auto express spline...
-named_segments.append(
- ThetaSplineSegment(
- name="GroundPickupBackCubeToScoreFrontMidCube",
- start=points['ScoreFrontMidCube'],
- control1=np.array([3.2345111429709847, 0.45338639767112277]),
- control2=np.array([3.098240119468829, -0.46161157069783254]),
- end=points['GroundPickupBackCube'],
- control_alpha_rolls=[(0.40, 0.0), (0.60, 0.0)],
- ))
-
points['ScoreFrontHighCube'] = to_theta_with_circular_index_and_roll(
0.901437, 1.16, np.pi / 2.0, circular_index=0)
@@ -656,6 +647,27 @@
control_alpha_rolls=[],
))
+# Auto express spline...
+named_segments.append(
+ ThetaSplineSegment(
+ name="GroundPickupBackCubeToScoreFrontMidCube",
+ start=points['ScoreFrontMidCube'],
+ control1=np.array([3.2345111429709847, 0.45338639767112277]),
+ control2=np.array([3.098240119468829, -0.46161157069783254]),
+ end=points['GroundPickupBackCube'],
+ control_alpha_rolls=[(0.40, 0.0), (0.60, 0.0)],
+ ))
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="GroundPickupBackCubeToScoreFrontHighCube",
+ start=points['ScoreFrontHighCube'],
+ control1=np.array([2.7074513232200186, 0.20154350392334375]),
+ control2=np.array([3.01714846217257, -0.6310437434614364]),
+ end=points['GroundPickupBackCube'],
+ control_alpha_rolls=[(0.40, 0.0), (0.60, 0.0)],
+ ))
+
front_points = []
back_points = []
unnamed_segments = []