Arch math update

src/frc846/cpp/frc846/util/math.cc → src/frc846/cpp/frc846/math/collection.cc

@@ -1,6 +1,12 @@
-#include "frc846/util/math.h"
+#include "frc846/math/collection.h"
 
-namespace frc846::util {
+#include <cmath>
+
+namespace frc846::math {
+
+bool DEquals(double x, double y, double epsilon) {
+  return std::abs(x - y) < epsilon;
+}
 
 double HorizontalDeadband(double input, double x_intercept, double max,
                           double exponent, double sensitivity) {
@@ -56,4 +62,4 @@ units::degree_t CoterminalSum(units::degree_t angle,
   }
 }
 
-}  // namespace frc846::util
+}  // namespace frc846::math

src/frc846/cpp/frc846/other/sendable_callback.cc → src/frc846/cpp/frc846/ntinf/sendable_callback.cc

@@ -1,12 +1,11 @@
-#include "frc846/other/sendable_callback.h"
-
 #include <wpi/sendable/SendableBuilder.h>
 
-namespace frc846::other {
+#include "frc846/ntinf/ntaction.h"
+
+namespace frc846::ntinf {
 
-SendableCallback::SendableCallback(std::function<void()> callback)
-    : callback_(callback) {}
-void SendableCallback::InitSendable(wpi::SendableBuilder& builder) {
+NTAction::NTAction(std::function<void()> callback) : callback_(callback) {}
+void NTAction::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Command");
   builder.AddStringProperty(".name", [] { return "Run"; }, nullptr);
   builder.AddBooleanProperty(
@@ -18,4 +17,4 @@ void SendableCallback::InitSendable(wpi::SendableBuilder& builder) {
       });
 }
 
-}  // namespace frc846::other
+}  // namespace frc846::ntinf

src/frc846/cpp/frc846/other/swerve_odometry.cc

@@ -3,75 +3,47 @@
 #include <cmath>
 #include <cstdio>
 
-#include "frc846/util/math.h"
-#include "frc846/util/share_tables.h"
+#include "frc846/math/vectors.h"
 
 namespace frc846 {
 
-SwerveOdometry::SwerveOdometry(util::Position initial_pose)
-    : pose_(initial_pose) {}
+SwerveOdometry::SwerveOdometry(
+    frc846::math::VectorND<units::foot_t, 2> initial_position)
+    : position_{initial_position[0], initial_position[1]} {}
 
 void SwerveOdometry::Update(
-    std::array<util::Vector2D<units::foot_t>, kModuleCount> wheel_vecs,
+    std::array<frc846::math::VectorND<units::foot_t, 2>, kModuleCount>
+        wheel_vecs,
     units::radian_t bearing) {
   // change in distance from the last odometry update
   for (int i = 0; i < kModuleCount; i++) {
-    units::foot_t wheel_dist = wheel_vecs[i].Magnitude();
+    units::foot_t wheel_dist = wheel_vecs[i].magnitude();
     units::foot_t delta_distance = wheel_dist - prev_wheel_distances_[i];
     units::foot_t dx =
-        delta_distance * units::math::cos(wheel_vecs[i].Bearing() + bearing);
+        delta_distance * units::math::sin(wheel_vecs[i].angle(true) + bearing);
     units::foot_t dy =
-        delta_distance * units::math::sin(wheel_vecs[i].Bearing() + bearing);
+        delta_distance * units::math::cos(wheel_vecs[i].angle(true) + bearing);
 
     prev_wheel_distances_[i] = wheel_dist;
 
-    wheel_vecs[i] = {dx, dy};
+    wheel_vecs[i][0] = dx;
+    wheel_vecs[i][1] = dy;
   }
 
-  // get the distance components of each of the module, accounting for the robot
-  // bearing
-  std::array<util::Vector2D<units::foot_t>, kModuleCount> xy_comps;
+  frc846::math::VectorND<units::foot_t, 2> relative_displacement{0_ft, 0_ft};
 
   for (int i = 0; i < kModuleCount; i++) {
-    xy_comps[i] = {
-        wheel_vecs[i].Magnitude() * units::math::sin(wheel_vecs[i].Bearing()),
-        wheel_vecs[i].Magnitude() * units::math::cos(wheel_vecs[i].Bearing()),
-    };
+    relative_displacement += wheel_vecs[i] / kModuleCount;
   }
 
-  // the distance travelled by each "side" of the robot
-  units::foot_t top = (xy_comps[0].x + xy_comps[1].x) / 2;
-  units::foot_t bottom = (xy_comps[3].x + xy_comps[2].x) / 2;
-  units::foot_t left = (xy_comps[0].y + xy_comps[2].y) / 2;
-  units::foot_t right = (xy_comps[1].y + xy_comps[3].y) / 2;
-
-  units::radian_t theta = bearing - pose_.bearing;
-
-  auto sin_theta = units::math::sin(theta);
-  auto cos_theta = units::math::cos(theta);
-
-  auto top_bottom = util::Vector2D<units::foot_t>{
-      (left + right) * sin_theta,
-      (left + right) * cos_theta,
-  };
-  auto left_right = util::Vector2D<units::foot_t>{
-      (top + bottom) * cos_theta,
-      (top + bottom) * -sin_theta,
-  };
-
-  pose_.point.x += (top_bottom.y + left_right.y) / 2;
-  pose_.point.y += (top_bottom.x + left_right.x) / 2;
-
-  pose_.bearing = bearing;
+  position_ += relative_displacement.rotate(bearing, true);
 }
 
-void SwerveOdometry::SetPoint(util::Vector2D<units::foot_t> point) {
-  pose_.point = point;
+void SwerveOdometry::SetPoint(frc846::math::VectorND<units::foot_t, 2> point) {
+  position_[0] = point[0];
+  position_[1] = point[1];
 }
 
-void SwerveOdometry::Zero() {
-  SetPoint({0_ft, 0_ft});
-  pose_.bearing = 0_deg;
-}
+void SwerveOdometry::Zero() { SetPoint({0_ft, 0_ft}); }
 
 }  // namespace frc846

src/frc846/cpp/frc846/other/trajectory_generator.cc

@@ -3,27 +3,27 @@
 #include <algorithm>
 
 #include "frc846/base/loggable.h"
-#include "frc846/util/math.h"
 
 namespace frc846 {
 
-std::vector<util::Position> InterpolatePoints(
-    util::Vector2D<units::foot_t> start, util::Vector2D<units::foot_t> end,
-    units::degree_t start_bearing, units::degree_t end_bearing,
-    units::foot_t cut) {
-  auto distance = (end - start).Magnitude();
+std::vector<frc846::math::FieldPoint> InterpolatePoints(
+    frc846::math::VectorND<units::foot_t, 2> start,
+    frc846::math::VectorND<units::foot_t, 2> end, units::degree_t start_bearing,
+    units::degree_t end_bearing, units::foot_t cut) {
+  auto distance = (end - start).magnitude();
   int n = std::max(units::math::ceil(distance / cut).to<int>(), 1);
 
-  std::vector<util::Position> points(n);
+  std::vector<frc846::math::FieldPoint> points(n);
   for (int i = 0; i < n; ++i) {
     double weight = (double)(i) / n;
     double bearingWeightMultiplier = 1.15;
     points[i] = {{
-                     start.x * (1 - weight) + end.x * weight,
-                     start.y * (1 - weight) + end.y * weight,
+                     start[0] * (1 - weight) + end[0] * weight,
+                     start[1] * (1 - weight) + end[1] * weight,
                  },
                  start_bearing * (1 - (weight * bearingWeightMultiplier)) +
-                     end_bearing * (weight * bearingWeightMultiplier)};
+                     end_bearing * (weight * bearingWeightMultiplier),
+                 {0.0_fps, 0.0_fps}};
 
     if (start_bearing <= end_bearing) {
       points[i].bearing = units::math::min(end_bearing, points[i].bearing);
@@ -36,8 +36,7 @@ std::vector<util::Position> InterpolatePoints(
 }
 
 Trajectory GenerateTrajectory(
-    std::vector<InputWaypoint> input_points,
-    units::feet_per_second_t robot_max_v,
+    std::vector<Waypoint> input_points, units::feet_per_second_t robot_max_v,
     units::feet_per_second_squared_t robot_max_acceleration,
     units::feet_per_second_squared_t robot_max_deceleration,
     units::inch_t cut) {
@@ -59,63 +58,55 @@ Trajectory GenerateTrajectory(
         input_points[i].pos.bearing, input_points[i - 1].pos.bearing, cut);
     interpolated_points.erase(interpolated_points.begin());
 
-    trajectory.push_back({
-        input_points[i].pos,
-        input_points[i].v_max.value_or(robot_max_v),
-    });
+    trajectory.push_back({input_points[i].pos});
 
     for (auto point : interpolated_points) {
-      trajectory.push_back({
-          point,
-          robot_max_v,
-      });
+      point.velocity[0] = robot_max_v;
+      trajectory.push_back({point});
     }
   }
-  trajectory.push_back({
-      input_points[0].pos,
-      input_points[0].v_max.value_or(robot_max_v),
-  });
+  trajectory.push_back({input_points[0].pos});
 
   for (unsigned int i = 1; i < trajectory.size(); ++i) {
     auto delta_pos =
-        (trajectory[i].pos.point - trajectory[i - 1].pos.point).Magnitude();
+        (trajectory[i].pos.point - trajectory[i - 1].pos.point).magnitude();
 
     // v₂² = v₁² + 2aΔx
     // 2aΔx = v₂² - v₁²
     // a = (v₂² - v₁²) / (2Δx)
-    auto deceleration = (units::math::pow<2>(trajectory[i].v) -
-                         units::math::pow<2>(trajectory[i - 1].v)) /
-                        (2 * delta_pos);
+    auto deceleration =
+        (units::math::pow<2>(trajectory[i].pos.velocity.magnitude()) -
+         units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude())) /
+        (2 * delta_pos);
     if (deceleration > robot_max_deceleration) {
       // v₂² = v₁² + 2aΔx
       // v₂² = sqrt(v₁² + 2aΔx)
 
-      // this code was replaced with calculateW because it makes the incorrect
-      // assumption that the values are positive
-      trajectory[i].v =
-          units::math::sqrt(units::math::pow<2>(trajectory[i - 1].v) +
-                            2 * robot_max_deceleration * delta_pos);
+      trajectory[i].pos.velocity[0] = units::math::sqrt(
+          units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude()) +
+          2 * robot_max_deceleration * delta_pos);
     }
   }
 
   std::reverse(trajectory.begin(), trajectory.end());
 
   for (unsigned int i = 1; i < trajectory.size(); ++i) {
     auto delta_pos =
-        (trajectory[i].pos.point - trajectory[i - 1].pos.point).Magnitude();
+        (trajectory[i].pos.point - trajectory[i - 1].pos.point).magnitude();
 
     // v₂² = v₁² + 2aΔx
     // 2aΔx = v₂² - v₁²
     // a = (v₂² - v₁²) / (2Δx)
-    auto acceleration = (units::math::pow<2>(trajectory[i].v) -
-                         units::math::pow<2>(trajectory[i - 1].v)) /
-                        (2 * delta_pos);
+    auto acceleration =
+        (units::math::pow<2>(trajectory[i].pos.velocity.magnitude()) -
+         units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude())) /
+        (2 * delta_pos);
     if (acceleration > robot_max_acceleration) {
       // v₂² = v₁² + 2aΔx
       // v₂² = sqrt(v₁² + 2aΔx)
-      trajectory[i].v =
-          units::math::sqrt(units::math::pow<2>(trajectory[i - 1].v) +
-                            2 * robot_max_acceleration * delta_pos);
+      trajectory[i].pos.velocity[0] = units::math::sqrt(
+          units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude()) +
+          2 * robot_max_acceleration * delta_pos);
 
       // trajectory[i].v =
       // units::velocity::feet_per_second_t(Find_Vsub2(units::unit_cast<double>(trajectory[i].v),
@@ -126,8 +117,10 @@ Trajectory GenerateTrajectory(
 
   // If any point has 0 speed, just set it to the previous speed.
   for (unsigned int i = 0; i < trajectory.size(); ++i) {
-    if (trajectory[i].v == 0_fps) {
-      trajectory[i].v = i == 0 ? trajectory[1].v : trajectory[i - 1].v;
+    if (trajectory[i].pos.velocity.magnitude() == 0_fps) {
+      trajectory[i].pos.velocity.magnitude() =
+          i == 0 ? trajectory[1].pos.velocity.magnitude()
+                 : trajectory[i - 1].pos.velocity.magnitude();
     }
   }
 

src/frc846/cpp/frc846/robot/GenericRobot.cc

@@ -15,7 +15,7 @@
 #include "frc2/command/ParallelDeadlineGroup.h"
 #include "frc2/command/WaitCommand.h"
 #include "frc846/base/loggable.h"
-#include "frc846/other/sendable_callback.h"
+#include "frc846/ntinf/ntaction.h"
 #include "frc846/robot/GenericCommand.h"
 #include "frc846/wpilib/time.h"
 
@@ -57,7 +57,7 @@ void GenericRobot::StartCompetition() {
 
   frc::SmartDashboard::PutData(
       "verify_hardware",
-      new frc846::other::SendableCallback([this] { VerifyHardware(); }));
+      new frc846::ntinf::NTAction([this] { VerifyHardware(); }));
 
   // Verify robot hardware
   VerifyHardware();

src/frc846/cpp/frc846/robot/RobotState.cc

@@ -0,0 +1,25 @@
+#include "frc846/robot/RobotState.h"
+
+namespace frc846::robot {
+
+RSTable::RSTable(std::string table_name)
+    : frc846::base::Loggable{table_name}, table_{} {}
+
+std::vector<std::string> RSTable::ListKeys() {
+  std::vector<std::string> keys;
+  for (const auto& [key, value] : table_) {
+    keys.push_back(key);
+  }
+  return keys;
+}
+
+std::unordered_map<std::string, RSTable*> RobotState::tables_{};
+
+RSTable* RobotState::getTable(std::string table_name) {
+  if (tables_.find(table_name) == tables_.end()) {
+    tables_.insert({table_name, new RSTable{table_name}});
+  }
+  return tables_[table_name];
+}
+
+};  // namespace frc846::robot

src/frc846/include/frc846/base/fserver.h

@@ -95,6 +95,7 @@ class LoggingServer {
 
         msg_mtx.lock();
         auto msg = messages.front();
+        messages.pop();
         msg_mtx.unlock();
 
         cli_mtx.lock();

src/frc846/include/frc846/control/motion.h

@@ -100,15 +100,23 @@ class BrakingPositionDyFPID {
  public:
   BrakingPositionDyFPID(frc846::base::Loggable& parent,
                         std::function<double(X)> prop_ff_function,
-                        CurrentControlSettings current_control_settings)
+                        CurrentControlSettings current_control_settings,
+                        frc846::control::HardLimitsConfigHelper<X>* hard_limits)
       : prop_ff_function_{prop_ff_function},
-        current_control_{parent, current_control_settings} {}
+        current_control_{parent, current_control_settings},
+        hard_limits_{hard_limits} {}
 
   double calculate(X target_pos, X current_pos,
                    double current_velocity_percentage,  // Only using dyF, P, D
                    frc846::control::Gains g) {
-    double error =
-        target_pos.template to<double>() - current_pos.template to<double>();
+    auto hard_limits_vals = hard_limits_->get();
+
+    double target_pos_capped =
+        std::max(std::min(hard_limits_vals.forward.template to<double>(),
+                          target_pos.template to<double>()),
+                 hard_limits_vals.reverse.template to<double>());
+
+    double error = target_pos_capped - current_pos.template to<double>();
 
     double target_output = g.kF * prop_ff_function_(current_pos) +
                            g.kP * error + g.kD * current_velocity_percentage;
@@ -121,6 +129,8 @@ class BrakingPositionDyFPID {
   std::function<double(X)> prop_ff_function_;
 
   CurrentControl current_control_;
+
+  frc846::control::HardLimitsConfigHelper<X>* hard_limits_;
 };
 
 struct GenericMotionWaypoint {