Added cg calculations for tippyness

Team846 · Jan 13, 2025 · 75a6d37 · 75a6d37
1 parent 3ba2412
commit 75a6d37
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 0 deletions.
diff --git a/src/y2025/cpp/robot_cg_calculator.cc b/src/y2025/cpp/robot_cg_calculator.cc
@@ -1,5 +1,7 @@
 #include "robot_cg_calculator.h"
 
+#include <units/force.h>
+
 namespace y2025 {
 
 RobotCGCalculator::RobotCGCalculator() {
@@ -16,6 +18,11 @@ RobotCGCalculator::RobotCGCalculator() {
   base_state_.velocity = {0_fps, 0_fps, 0_fps};
 }
 
+units::feet_per_second_t RobotCGCalculator::CalculateMaxAcceleration(units::inch_t distance_from_wheels) {
+  frc846::math::VectorND<units::inch, 3> cg = CalculateRobotCG();
+  return frc846::math::constants::physics::g * distance_from_wheels / cg[2] / 2;
+}
+
 void RobotCGCalculator::SetElevatorHeight(units::inch_t height) {
   elevator_state_.position[2] = height;
 }

diff --git a/src/y2025/include/robot_cg_calculator.h b/src/y2025/include/robot_cg_calculator.h
@@ -40,6 +40,8 @@ class RobotCGCalculator {
   void SetBaseHeight(units::inch_t height);
   void SetBaseVelocity(frc846::math::VectorND<units::feet_per_second, 3> vel);
 
+  units::feet_per_second_t CalculateMaxAcceleration(units::inch_t distance_from_wheels);
+
   /*
   CalculateRobotCG()