commit a7c3bc6821cf28533d5f59ed68ee22dc886d01b4
author Austin Schuh <austin.linux@gmail.com> Sun Apr 16 19:46:23 2023 -0700
committer Naman Gupta <naman.gupta@bluerivertech.com> Tue Apr 25 22:54:35 2023 -0700
tree 88aeba06f9510f72400131f81ed5d0bb7b4e9c48
parent 2812f28c0465b9474d94b106f6ceef2a820df817

Tune both autos for NASA again

Change-Id: Id70ad1d6174d320be1b07b0886d5241c82846210
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

y2023/autonomous/autonomous_actor.cc

diff --git a/y2023/autonomous/autonomous_actor.cc b/y2023/autonomous/autonomous_actor.cc
index cd4d4f1..e1b4e1d 100644
--- a/y2023/autonomous/autonomous_actor.cc
+++ b/y2023/autonomous/autonomous_actor.cc
@@ -319,6 +319,8 @@
 
   if (!WaitForArmGoal(0.05)) return;
 
+  std::this_thread::sleep_for(chrono::milliseconds(100));
+
   AOS_LOG(
       INFO, "Placed first cube %lf s\n",
       aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
@@ -378,7 +380,7 @@
         INFO, "Done backing up %lf s\n",
         aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
 
-    const ProfileParametersT kInPlaceTurn = MakeProfileParameters(2.5, 7.0);
+    const ProfileParametersT kInPlaceTurn = MakeProfileParameters(2.7, 8.0);
     StartDrive(0.0, aos::math::NormalizeAngle(M_PI / 2.0 - Theta()), kDrive,
                kInPlaceTurn);
 
@@ -556,6 +558,14 @@
 
   std::this_thread::sleep_for(chrono::milliseconds(200));
   Neutral();
+  AOS_LOG(
+      INFO, "Going to neutral %lf s\n",
+      aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+  if (!WaitForArmGoal(0.05)) return;
+  AOS_LOG(
+      INFO, "Done at neutral %lf s\n",
+      aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
 }
 
 void AutonomousActor::SendSuperstructureGoal() {
@@ -617,7 +627,7 @@
 
 void AutonomousActor::Neutral() {
   set_arm_goal_position(control_loops::superstructure::arm::NeutralIndex());
-  set_wrist_goal(0.0);
+  set_wrist_goal(1.0);
   SendSuperstructureGoal();
 }
 
@@ -631,7 +641,7 @@
 void AutonomousActor::HighCubeScore() {
   set_arm_goal_position(
       control_loops::superstructure::arm::ScoreFrontHighCubeIndex());
-  set_wrist_goal(0.6);
+  set_wrist_goal(1.0);
   SendSuperstructureGoal();
 }