From 75a6d37903c237accb26d0a42b6772072f9dc7fb Mon Sep 17 00:00:00 2001
From: prak132 <rathoreprakhar1@gmail.com>
Date: Sun, 12 Jan 2025 17:11:41 -0800
Subject: [PATCH] Added cg calculations for tippyness

---
 src/y2025/cpp/robot_cg_calculator.cc    | 7 +++++++
 src/y2025/include/robot_cg_calculator.h | 2 ++
 2 files changed, 9 insertions(+)

diff --git a/src/y2025/cpp/robot_cg_calculator.cc b/src/y2025/cpp/robot_cg_calculator.cc
index 6170211..f217a80 100644
--- 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
index a6966e8..bf358d8 100644
--- 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()