commit b392b741af38ae5ad5313773c20c66c7e9c9d8b1
author Maxwell Henderson <mxwhenderson@gmail.com> Sun Mar 05 07:53:51 2023 -0800
committer Maxwell Henderson <mxwhenderson@gmail.com> Sun Mar 05 12:16:08 2023 -0800
tree 06001cb9d83135e0d7a90f4740d6c1e521e11b1d
parent 5c47a46ea78b67b771d622021926c43c946acdb1

Allow pre-compiling arm trajectories

The robot takes a while before it's usable because it has to re-optimize
the arm trajectories when it runs. This commit takes the outputted c++
from graph_codegen and optimizes the trajectories before the robot
runs. It then puts them into a bfbs which is able to be placed on the
robot and read.

Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ic75f9d590d6b89bfd5eed8b804e73c8c84ecec6b

y2023/control_loops/superstructure/superstructure.h

diff --git a/y2023/control_loops/superstructure/superstructure.h b/y2023/control_loops/superstructure/superstructure.h
index 87a5395..0c8b2c0 100644
--- a/y2023/control_loops/superstructure/superstructure.h
+++ b/y2023/control_loops/superstructure/superstructure.h
@@ -2,17 +2,22 @@
 #define Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
 
 #include "aos/events/event_loop.h"
+#include "aos/json_to_flatbuffer.h"
 #include "frc971/control_loops/control_loop.h"
 #include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
 #include "y2023/constants.h"
 #include "y2023/control_loops/drivetrain/drivetrain_can_position_generated.h"
 #include "y2023/control_loops/superstructure/arm/arm.h"
+#include "y2023/control_loops/superstructure/arm/arm_trajectories_generated.h"
 #include "y2023/control_loops/superstructure/end_effector.h"
 #include "y2023/control_loops/superstructure/superstructure_goal_generated.h"
 #include "y2023/control_loops/superstructure/superstructure_output_generated.h"
 #include "y2023/control_loops/superstructure/superstructure_position_generated.h"
 #include "y2023/control_loops/superstructure/superstructure_status_generated.h"
 
+using y2023::control_loops::superstructure::arm::ArmTrajectories;
+using y2023::control_loops::superstructure::arm::TrajectoryAndParams;
+
 namespace y2023 {
 namespace control_loops {
 namespace superstructure {
@@ -35,9 +40,11 @@
           ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
           ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
 
-  explicit Superstructure(::aos::EventLoop *event_loop,
-                          std::shared_ptr<const constants::Values> values,
-                          const ::std::string &name = "/superstructure");
+  explicit Superstructure(
+      ::aos::EventLoop *event_loop,
+      std::shared_ptr<const constants::Values> values,
+      const aos::FlatbufferVector<ArmTrajectories> &arm_trajectories,
+      const ::std::string &name = "/superstructure");
 
   double robot_velocity() const;
 
@@ -45,6 +52,11 @@
   inline const EndEffector &end_effector() const { return end_effector_; }
   inline const AbsoluteEncoderSubsystem &wrist() const { return wrist_; }
 
+  static const aos::FlatbufferVector<ArmTrajectories> GetArmTrajectories(
+      const std::string &filename) {
+    return aos::FileToFlatbuffer<arm::ArmTrajectories>(filename);
+  }
+
  protected:
   virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
                             aos::Sender<Output>::Builder *output,