Rework library without unix sockets for now

server_v2/src/Cache.cpp

@@ -0,0 +1,16 @@
+#include "include/Cache.hpp"
+
+#define MS_IN_S 1000
+#define US_IN_S (1000 * MS_IN_S)
+
+Cache::Cache(int rate_hz) {
+  cache_duration = std::chrono::microseconds(US_IN_S / rate_hz);
+}
+
+bool Cache::is_expired() {
+  return ((clock::now() - last_update) > cache_duration);
+}
+
+void Cache::update() {
+  last_update = clock::now();
+}

server_v2/src/ClosedLoopMotorController.cpp

@@ -0,0 +1,49 @@
+#include <algorithm>
+#include "include/ClosedLoopMotorController.hpp"
+#include "include/Motors.hpp"
+#include "include/Encoders.hpp"
+
+#define MS_IN_S 1000
+#define US_IN_S (1000 * MS_IN_S)
+
+void ClosedLoopMotorController::set_power(uint8_t port, double power) {
+  control_modes.at(port) = ControlMode::POWER;
+  Motors::set_power(port, power);
+}
+
+void ClosedLoopMotorController::set_speed(uint8_t port, double speed_rpm) {
+  speed_targets.at(port) = speed_rpm;
+  if (control_modes.at(port) != ControlMode::SPEED) {
+    pids.at(port) = PID(
+        ROBOT_MOTOR_SPEED_CONTROL_KP, ROBOT_MOTOR_SPEED_CONTROL_KI, ROBOT_MOTOR_SPEED_CONTROL_KD,
+        ROBOT_MOTOR_SPEED_CONTROL_RAMP, 100.0);
+    control_modes.at(port) = ControlMode::SPEED;
+  }
+}
+
+ClosedLoopMotorController::ClosedLoopMotorController() {
+  speed_control_thread = std::thread(&ClosedLoopMotorController::speed_control_loop, this);
+  speed_control_thread.detach();
+}
+
+[[noreturn]] void ClosedLoopMotorController::speed_control_loop() {
+  auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ);
+  while (true) {
+    std::this_thread::sleep_for(sleep_duration);
+
+    bool should_control_speed = std::find(control_modes.begin(), control_modes.end(), ControlMode::SPEED) != control_modes.end();
+    if (!should_control_speed) {
+      continue;
+    }
+
+    auto speeds = Encoders::getInstance().get_velocities_rpm();
+    for (int i = 0; i < ROBOT_MOTOR_COUNT; i++) {
+      if (control_modes.at(i) == ControlMode::SPEED) {
+        double power = pids.at(i)(speed_targets.at(i), speeds.at(i));
+        Motors::set_power(i, power);
+      }
+    }
+  }
+}
+
+

server_v2/src/Encoders.cpp

@@ -0,0 +1,29 @@
+#include "include/Encoders.hpp"
+#include "include/spi.hpp"
+#include "include/mathUtils.hpp"
+#include <spdlog/spdlog.h>
+
+std::array<int32_t, ROBOT_MOTOR_COUNT> Encoders::get_positions() {
+  if (position_cache.is_expired()) {
+    uint8_t data[ROBOT_MOTOR_COUNT * 4] = {0};
+    Spi::getInstance().read_array(Spi::MOTOR_1_POS_B3, 4 * ROBOT_MOTOR_COUNT, data);
+    for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
+      cached_positions.at(i) = mathUtils::from_bytes<int32_t>(data + i * 4, 4);
+    }
+    position_cache.update();
+  }
+  return cached_positions;
+}
+
+std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_rpm() {
+  if (velocity_cache.is_expired()) {
+    uint8_t data[ROBOT_MOTOR_COUNT * 2] = {0};
+    Spi::getInstance().read_array(Spi::MOTOR_1_VEL_H, 2 * ROBOT_MOTOR_COUNT, data);
+    for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
+      auto velocity_tps = mathUtils::from_bytes<int16_t>(data + i * 2, 2);
+      cached_velocities_rpm.at(i) = velocity_tps * 60.0 / ROBOT_TICKS_PER_TURN;
+    }
+    velocity_cache.update();
+  }
+  return cached_velocities_rpm;
+}

server_v2/src/IRSensors.cpp

@@ -0,0 +1,29 @@
+#include "include/IRSensors.hpp"
+#include "include/spi.hpp"
+#include "include/Robot.hpp"
+#include "include/mathUtils.hpp"
+
+void IRSensors::enable() {
+  uint8_t data[ROBOT_IR_SENSOR_COUNT] = {};
+  std::fill_n(data, ROBOT_IR_SENSOR_COUNT, 1);
+  Spi::getInstance().write_array(Spi::IR_1_LED, ROBOT_IR_SENSOR_COUNT, data);
+}
+
+void IRSensors::disable() {
+  uint8_t data[ROBOT_IR_SENSOR_COUNT] = {};
+  std::fill_n(data, ROBOT_IR_SENSOR_COUNT, 0);
+  Spi::getInstance().write_array(Spi::IR_1_LED, ROBOT_IR_SENSOR_COUNT, data);
+}
+
+
+std::array<uint16_t, ROBOT_IR_SENSOR_COUNT> IRSensors::read() {
+  if (cache.is_expired()) {
+    uint8_t data[ROBOT_IR_SENSOR_COUNT * 2] = {0};
+    Spi::getInstance().read_array(Spi::IR_1_H, 2 * ROBOT_IR_SENSOR_COUNT, data);
+    for (int i = 0; i < ROBOT_IR_SENSOR_COUNT; ++i) {
+      cached_values.at(i) = mathUtils::from_bytes<uint16_t>(data + i * 2, 2);
+    }
+    cache.update();
+  }
+  return cached_values;
+}

server_v2/src/Motors.cpp

@@ -0,0 +1,31 @@
+#include "include/Motors.hpp"
+#include "include/spi.hpp"
+#include "include/Robot.hpp"
+#include <spdlog/spdlog.h>
+#include <limits>
+
+void Motors::set_power(uint8_t port, double percent) {
+  if (fabs(percent) > 100.0) {
+    spdlog::error("Invalid motor percent specified. Should be -100 <= percent <= 100", percent);
+  }
+
+  Mode mode = Mode::COAST;
+  if (percent < 0.0) {
+    mode = Mode::BACKWARD;
+  } else if (percent > 0.0) {
+    mode = Mode::FORWARD;
+  }
+
+  auto pwm = (uint16_t) (fabs(percent) * (std::numeric_limits<uint16_t>::max() / 100.0));
+  set_pwm(port, pwm, mode);
+}
+
+void Motors::set_pwm(uint8_t port, uint16_t pwm, Mode mode) {
+  if (port >= ROBOT_MOTOR_COUNT) {
+    spdlog::error("Invalid motor port {} specified", port);
+  }
+
+  Spi::getInstance().write(Spi::MOTOR_1_PWM_H + port * 2, 2, pwm);
+  Spi::getInstance().write(Spi::PWM_1_CTRL + port, 1, mode);
+}
+

server_v2/src/Odometry.cpp

@@ -0,0 +1,21 @@
+#include "include/Odometry.hpp"
+
+Odometry::Odometry() : x_position(0), y_position(0), angular_orientation(0) {
+
+}
+
+Odometry::Odometry(double x_position, double y_position, double angular_orientation) : x_position(x_position), y_position(y_position), angular_orientation(angular_orientation) {
+
+}
+
+double Odometry::get_x_position() const {
+  return x_position;
+}
+
+double Odometry::get_y_position() const {
+  return y_position;
+}
+
+double Odometry::get_angular_orientation() const {
+  return angular_orientation;
+}

server_v2/src/OdometryController.cpp

@@ -0,0 +1,83 @@
+#include <cmath>
+
+#include "include/OdometryController.hpp"
+#include "include/Robot.hpp"
+#include "include/Encoders.hpp"
+#include <spdlog/spdlog.h>
+
+#define MS_IN_S 1000
+#define US_IN_S (1000 * MS_IN_S)
+
+void OdometryController::enable() {
+  last_run = clock::now();
+  enabled = true;
+}
+
+void OdometryController::disable() {
+  enabled = false;
+}
+
+void OdometryController::reset() {
+  current_odometry = Odometry();
+  last_position_left = 0;
+  last_position_right = 0;
+  last_run = clock::now();
+}
+
+Odometry OdometryController::get() {
+  return current_odometry;
+}
+
+
+OdometryController::OdometryController() {
+  odometry_thread = std::thread(&OdometryController::odometry_loop, this);
+  odometry_thread.detach();
+}
+
+[[noreturn]] void OdometryController::odometry_loop() {
+  auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_ODOMETRY_CONTROLLER_RATE_HZ);
+  while (true) {
+    std::this_thread::sleep_for(sleep_duration);
+
+    if (enabled) {
+      last_run = clock::now();
+      auto encoder_positions = Encoders::getInstance().get_positions();
+
+      auto current_position_left = encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
+      auto current_position_right = encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
+      auto distance_left = (current_position_left - last_position_left) / ROBOT_TICKS_PER_METER;
+      auto distance_right = (current_position_right - last_position_right) / ROBOT_TICKS_PER_METER;
+      last_position_left = current_position_left;
+      last_position_right = current_position_right;
+
+      auto distance = (distance_left + distance_right) * 2.0;
+      auto theta = (distance_left - distance_right) / ROBOT_ARBOR_LENGTH_M;
+
+      auto new_x_position = current_odometry.get_x_position();
+      auto new_y_position = current_odometry.get_y_position();
+      auto new_angular_orientation = current_odometry.get_angular_orientation();
+      if (distance != 0) {
+        auto current_orientation = current_odometry.get_angular_orientation();
+
+        auto distance_x = cos(theta) * distance;
+        auto distance_y = -sin(theta) * distance;
+
+        new_x_position = current_odometry.get_x_position() +
+                         (cos(current_orientation) * distance_x -
+                          sin(current_orientation) * distance_y);
+        new_y_position = current_odometry.get_y_position() +
+                         (sin(current_orientation) * distance_x +
+                          cos(current_orientation) * distance_y);
+      }
+      if (theta != 0) {
+        new_angular_orientation = current_odometry.get_angular_orientation() + theta;
+      }
+      current_odometry = Odometry(new_x_position, new_y_position, new_angular_orientation);
+
+//      spdlog::info("{} {} {} {} {} {}",
+//                   current_position_left, current_position_right,
+//                   distance_left, distance_right,
+//                   distance, theta);
+    }
+  }
+}

server_v2/src/PID.cpp

@@ -0,0 +1,50 @@
+#include "include/PID.hpp"
+#include <chrono>
+#include <algorithm>
+
+#define MS_IN_S 1000.0
+#define US_IN_S (1000.0 * MS_IN_S)
+
+using namespace std;
+
+
+PID::PID() : P(0), I(0), D(0), setpoint_ramp(0), limit(0), error_prev(0.0), setpoint_prev(0.0), integral_prev(0.0) {};
+
+PID::PID(double P, double I, double D, double ramp, double limit)
+    : P(P), I(I), D(D), setpoint_ramp(ramp), limit(limit), error_prev(0.0), setpoint_prev(0.0), integral_prev(0.0) {
+  timestamp_prev = clock::now();
+}
+
+double PID::operator()(double setpoint, double process_variable) {
+  // calculate the time from the last call
+  auto timestamp_now = clock::now();
+  double delta_seconds = chrono::duration_cast<chrono::duration<double, std::micro>>(timestamp_now - timestamp_prev).count() / US_IN_S;
+
+  if (setpoint_ramp > 0) {
+    double setpoint_rate = (setpoint - setpoint_prev) / delta_seconds;
+    if (setpoint_rate > setpoint_ramp) {
+      setpoint = setpoint_prev + setpoint_ramp * delta_seconds;
+    } else if (setpoint_rate < -setpoint_ramp) {
+      setpoint = setpoint_prev - setpoint_ramp * delta_seconds;
+    }
+  }
+
+  double error = setpoint - process_variable;
+
+  double proportional = P * error;
+  // Tustin transform of the integral part
+  double integral = integral_prev + I * delta_seconds * 0.5f * (error + error_prev);
+  double derivative = D * ((error - error_prev) / delta_seconds);
+
+  double output = proportional + integral + derivative;
+
+  // anti-windup - limit the output variable
+  output = std::min(std::max(-limit, output), limit);
+  // spdlog::info("E{} P{} I{} D{} O{} EP{} DS{}", error, proportional, integral, derivative, output, error_prev, delta_seconds);
+
+  integral_prev = integral;
+  setpoint_prev = setpoint;
+  error_prev = error;
+  timestamp_prev = timestamp_now;
+  return output;
+}

server_v2/src/main.cpp

@@ -1,12 +1,9 @@
 #include <spdlog/spdlog.h>
-#include <chrono>
 
 #include "include/reset.hpp"
-#include "include/spi.hpp"
-
-#define SOCKET_PATH "/tmp/compLib"
-#define BUFFER_SIZE 64
-#define SAMPLES 20000.0
+#include "include/Encoders.hpp"
+#include "include/OdometryController.hpp"
+#include "include/ClosedLoopMotorController.hpp"
 
 using namespace std;
 
@@ -14,15 +11,14 @@ int main() {
   Reset::reset_robot();
   spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v");
 
-  auto start = chrono::steady_clock::now();
-  for (int i = 0; i < SAMPLES; i++) {
-    Spi::getInstance().read(Spi::Register::IDENTIFICATION_MODEL_ID, 1);
+  ClosedLoopMotorController::getInstance().set_speed(0, -125);
+  ClosedLoopMotorController::getInstance().set_speed(3, 125);
+  OdometryController::getInstance().enable();
+
+  for (int i = 0; i < 10000; ++i) {
+    auto odom = OdometryController::getInstance().get();
+    spdlog::info("X {} Y {} W {}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation());
+    usleep(1000);
   }
-  auto end = chrono::steady_clock::now();
-
-  spdlog::info("Elapsed time in milliseconds: {}ms", chrono::duration_cast<chrono::milliseconds>(end - start).count());
-  spdlog::info("Loop rate: {}hz", SAMPLES / chrono::duration_cast<chrono::milliseconds>(end - start).count() * 1000.0);
-
   return 0;
 }
-