Tune auto for MBR to be more centered and reliable
Change-Id: If953f15262d8f931031acce47c01a41a267dfdb1
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/autonomous/autonomous_actor.cc b/y2023/autonomous/autonomous_actor.cc
index c3ba65b..26197e3 100644
--- a/y2023/autonomous/autonomous_actor.cc
+++ b/y2023/autonomous/autonomous_actor.cc
@@ -328,14 +328,12 @@
MidCubeScore();
- if (!splines[3].WaitForSplineDistanceRemaining(0.05)) return;
+ 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;
-
- if (!splines[3].WaitForSplineDistanceRemaining(0.05)) return;
Spit();
if (!splines[3].WaitForSplineDistanceRemaining(0.02)) return;
@@ -349,6 +347,8 @@
const ProfileParametersT kTurn = MakeProfileParameters(3.0, 4.5);
StartDrive(0.0, 0.0, kDrive, kTurn);
+ std::this_thread::sleep_for(chrono::milliseconds(100));
+
{
double side_scalar = (alliance_ == aos::Alliance::kRed) ? 1.0 : -1.0;
StartDrive(6.33 - std::abs(X()), 0.0, kDrive, kTurn);
@@ -358,7 +358,7 @@
INFO, "Done backing up %lf s\n",
aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
- const ProfileParametersT kInPlaceTurn = MakeProfileParameters(2.0, 4.5);
+ const ProfileParametersT kInPlaceTurn = MakeProfileParameters(2.5, 7.0);
StartDrive(0.0, aos::math::NormalizeAngle(M_PI / 2.0 - Theta()), kDrive,
kInPlaceTurn);
@@ -369,8 +369,13 @@
INFO, "Roller off %lf s\n",
aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+ if (!WaitForTurnProfileNear(0.6)) return;
+ AOS_LOG(
+ INFO, "Balance arm %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
Balance();
- if (!WaitForTurnProfileNear(0.03)) return;
+ if (!WaitForTurnProfileNear(0.001)) return;
AOS_LOG(
INFO, "Done turning %lf s\n",
@@ -378,7 +383,7 @@
const ProfileParametersT kDrive = MakeProfileParameters(1.4, 3.0);
const ProfileParametersT kFinalTurn = MakeProfileParameters(3.0, 4.5);
- const double kDriveDistance = 3.12;
+ const double kDriveDistance = 3.11;
StartDrive(-kDriveDistance, 0.0, kDrive, kFinalTurn);
const ProfileParametersT kFastTurn = MakeProfileParameters(5.0, 8.0);
@@ -461,7 +466,7 @@
void AutonomousActor::Balance() {
set_arm_goal_position(
- control_loops::superstructure::arm::GroundPickupFrontConeUpIndex());
+ control_loops::superstructure::arm::ScoreFrontLowCubeIndex());
set_wrist_goal(0.0);
SendSuperstructureGoal();
}
diff --git a/y2023/autonomous/splines/spline.1.json b/y2023/autonomous/splines/spline.1.json
index ada494a..22e739f 100644
--- a/y2023/autonomous/splines/spline.1.json
+++ b/y2023/autonomous/splines/spline.1.json
@@ -1 +1 @@
-{"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
+{"spline_count": 1, "spline_x": [1.6230056318540846, 2.595606822913154, 3.5201381785640087, 5.572187770143549, 6.006069035460717, 6.435374392603574], "spline_y": [0.4224994345862587, 0.509568426046465, 0.8395233993107027, 0.4247693876940553, 0.4460951327369448, 0.4460951327369448], "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 bbefb4e..4a24ffc 100644
--- a/y2023/autonomous/splines/spline.2.json
+++ b/y2023/autonomous/splines/spline.2.json
@@ -1 +1 @@
-{"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
+{"spline_count": 1, "spline_x": [6.435374392603574, 6.0442444046370705, 4.96865071295785, 2.8506937641800936, 2.4156145282058796, 1.571420503593167], "spline_y": [0.4460951327369448, 0.4460951327369448, 0.3376418389076542, 1.2164153540583742, -0.13930255865735708, -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 d888718..bce0c4d 100644
--- a/y2023/autonomous/splines/spline.3.json
+++ b/y2023/autonomous/splines/spline.3.json
@@ -1 +1 @@
-{"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
+{"spline_count": 1, "spline_x": [1.571420503593167, 3.533311283576554, 3.1860855438536575, 5.263533825984673, 4.912898328525625, 6.376744631564399], "spline_y": [-0.6670628799510845, 0.5594420208177215, 0.5015288933650817, 0.5641404302951452, 0.27660573272799804, 0.2788534917250134], "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/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 9aecb49..93134c7 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -386,7 +386,7 @@
control_alpha_rolls=[(.95, -np.pi / 2.0)],
))
-points['Starting'] = np.array((np.pi, -0.125053863467887, 0.0))
+points['Starting'] = np.array((np.pi, -0.15, 0.0))
points['ScoreFrontMidConeUpAuto'] = to_theta_with_circular_index_and_roll(
0.58, 0.97, -np.pi / 2.0, circular_index=0)
@@ -398,7 +398,7 @@
control1=np.array([2.99620794024176, 0.23620211875551145]),
control2=np.array([2.728197531599509, 0.5677148040671784]),
end=points['ScoreFrontMidConeUpAuto'],
- control_alpha_rolls=[(0.20, 0.0), (.85, -np.pi / 2.0)],
+ control_alpha_rolls=[(.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}"),