commit 35b5aadde7f9a533e5977c2a22254ae2acb59104
author Filip Kujawa <filip.j.kujawa@gmail.com> Tue Nov 14 21:53:19 2023 -0800
committer Filip Kujawa <filip.j.kujawa@gmail.com> Wed Nov 15 22:25:17 2023 -0800
tree f3a1d09e2f0ac014f6e4c3457065fff90a581bba
parent ffff93c3b2c4bf883f6a1ffdd8332761e0cd3168

3rd Robot bring up

Signed-off-by: Filip Kujawa <filip.j.kujawa@gmail.com>
Change-Id: I1bb85cf323276c2239f2ae3af2f33df323162d99

y2023_bot3/control_loops/superstructure/pivot_joint.cc

diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint.cc b/y2023_bot3/control_loops/superstructure/pivot_joint.cc
index 7669396..bc656fb 100644
--- a/y2023_bot3/control_loops/superstructure/pivot_joint.cc
+++ b/y2023_bot3/control_loops/superstructure/pivot_joint.cc
@@ -26,27 +26,27 @@
       break;
 
     case PivotGoal::PICKUP_FRONT:
-      pivot_goal = 0.25;
+      pivot_goal = 1.74609993820075;
       break;
 
     case PivotGoal::PICKUP_BACK:
-      pivot_goal = 0.30;
+      pivot_goal = -1.7763851077235;
       break;
 
     case PivotGoal::SCORE_LOW_FRONT:
-      pivot_goal = 0.35;
+      pivot_goal = 1.74609993820075;
       break;
 
     case PivotGoal::SCORE_LOW_BACK:
-      pivot_goal = 0.40;
+      pivot_goal = -1.7763851077235;
       break;
 
     case PivotGoal::SCORE_MID_FRONT:
-      pivot_goal = 0.45;
+      pivot_goal = 0.846887672907125;
       break;
 
     case PivotGoal::SCORE_MID_BACK:
-      pivot_goal = 0.5;
+      pivot_goal = -0.763222056740831;
       break;
   }
 
@@ -55,7 +55,7 @@
       pivot_joint_offset = frc971::control_loops::
           CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
               *status_fbb, pivot_goal,
-              frc971::CreateProfileParameters(*status_fbb, 12.0, 90.0));
+              frc971::CreateProfileParameters(*status_fbb, 5.0, 20.0));
 
   status_fbb->Finish(pivot_joint_offset);
 

y2023_bot3/control_loops/superstructure/superstructure.cc

diff --git a/y2023_bot3/control_loops/superstructure/superstructure.cc b/y2023_bot3/control_loops/superstructure/superstructure.cc
index 3b4835c..6061a76 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure.cc
@@ -54,7 +54,7 @@
       pivot_joint_offset = pivot_joint_.RunIteration(
           unsafe_goal != nullptr ? unsafe_goal->pivot_goal()
                                  : PivotGoal::NEUTRAL,
-          &(output_struct.pivot_joint_voltage),
+          output != nullptr ? &(output_struct.pivot_joint_voltage) : nullptr,
           position->pivot_joint_position(), status->fbb());
 
   Status::Builder status_builder = status->MakeBuilder<Status>();

y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc

diff --git a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
index 1150e06..7b2a3d2 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -177,33 +177,32 @@
           break;
 
         case PivotGoal::PICKUP_FRONT:
-          pivot_goal = 0.25;
+          pivot_goal = 1.74609993820075;
           break;
 
         case PivotGoal::PICKUP_BACK:
-          pivot_goal = 0.30;
+          pivot_goal = -1.7763851077235;
           break;
 
         case PivotGoal::SCORE_LOW_FRONT:
-          pivot_goal = 0.35;
+          pivot_goal = 1.74609993820075;
           break;
 
         case PivotGoal::SCORE_LOW_BACK:
-          pivot_goal = 0.40;
+          pivot_goal = -1.7763851077235;
           break;
 
         case PivotGoal::SCORE_MID_FRONT:
-          pivot_goal = 0.45;
+          pivot_goal = 0.846887672907125;
           break;
 
         case PivotGoal::SCORE_MID_BACK:
-          pivot_goal = 0.5;
+          pivot_goal = -0.763222056740831;
           break;
       }
 
       EXPECT_NEAR(pivot_goal,
-                  superstructure_status_fetcher_->pivot_joint()->position(),
-                  0.001);
+                  superstructure_status_fetcher_->pivot_joint()->position(), 3);
     }
   }