Add motor speed control

server/include/motor.hpp

@@ -4,6 +4,7 @@
 #include <cstdint>
 #include <vector>
 #include <chrono>
+#include <thread>
 
 #include "include/robot.hpp"
 
@@ -18,6 +19,11 @@ public:
         NONE = 5
     };
 
+    enum Control : uint8_t {
+        POWER = 0,
+        SPEED = 1
+    };
+
     static Motor& get_instance()
     {
         static Motor instance;
@@ -27,18 +33,27 @@ public:
     Motor(Motor const&) = delete;
     void operator=(Motor const&) = delete;
 
-    static void power(uint8_t port, double percent);
+    void set_speed(uint8_t port, double rpm);
-    static void pwm(uint8_t port, uint16_t pwm, Mode mode);
+    void set_power(uint8_t port, double percent);
+    static void set_pwm(uint8_t port, uint16_t pwm, Mode mode);
 
     std::vector<double> get_speed();
+
 private:
     Motor();
 
+    // Speed calculation and speed filter
     int32_t last_encoder_values[MOTOR_COUNT] = {0};
     double filtered_speeds[MOTOR_COUNT] = {0};
-    std::chrono::system_clock::time_point last_time_read;
+    std::chrono::system_clock::time_point last_time_encoders_read;
+
+    // Speed control
+    double speed_targets[MOTOR_COUNT] = {0};
+    double motor_control_modes[MOTOR_COUNT] = {0};
+    std::thread speed_control_thread;
 
     void reset_speed();
+    void speed_control_loop();
 };
 
 #endif // COMPLIB_SERVER_MOTOR_HPP

server/include/robot.hpp

@@ -15,6 +15,11 @@
 #define MOTOR_MAX_DELTA_TIME_S 0.1L
 #define MOTOR_FILTER_ALPHA 0.05L
 #define MOTOR_FILTER_RESET_DELAY_US (1000 * 10)
+#define MOTOR_SPEED_CONTROL_RESET_DELAY_S 0.01L
+#define MOTOR_SPEED_CONTROL_LOOP_HZ 150
+#define MOTOR_SPEED_CONTROL_KP 0.5L
+#define MOTOR_SPEED_CONTROL_KI 5.0L
+#define MOTOR_SPEED_CONTROL_KD 0.025L
 
 #define LEFT_PORT 3
 #define RIGHT_PORT 0

server/include/spi.hpp

@@ -7,7 +7,7 @@
 //SPI_MODE_3 (1,1) 	CPOL = 1, CPHA = 1, Clock idle high, data is clocked in on rising, edge output data (change) on falling edge
 
 #define SPI_BUFFER_SIZE 20
-#define SPI_SPEED 4000000 // 4 MHz
+#define SPI_SPEED 2000000 // 2 MHz
 #define SPI_BITS_PER_WORD 8
 
 class Spi {

server/src/main.cpp

@@ -105,43 +105,46 @@ int main() {
   // }
   // double end_time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
 
-  double kp = 0.5;
+  // double kp = 0.5;
-  double ki = 5.0;
+  // double ki = 5.0;
-  double kd = 0.025;
+  // double kd = 0.025;
-  double setpoint = 0;
+  double setpoint = 50;
-  double errror_sum = 0;
+  // double errror_sum = 0;
-  double last_error = 0;
+  // double last_error = 0;
   // int port = 0;
   // long double accumulator = 0;
   // long double alpha = 0.2;
-  auto last_time = std::chrono::high_resolution_clock::now();
+  // auto last_time = std::chrono::high_resolution_clock::now();
-  auto start_time = last_time;
+  // auto start_time = last_time;
-  int i = 0;
+  // int i = 0;
   while (1 == 1) {
-    setpoint += 0.05;
+    setpoint += 0.5;
-    auto current_time = std::chrono::high_resolution_clock::now();
+    // auto current_time = std::chrono::high_resolution_clock::now();
-    long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_time).count() / 1000000.0;
+    // long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_time).count() / 1000000.0;
-    long double delta_seconds_total = std::chrono::duration_cast<std::chrono::microseconds>(current_time - start_time).count() / 1000000.0;
+    // long double delta_seconds_total = std::chrono::duration_cast<std::chrono::microseconds>(current_time - start_time).count() / 1000000.0;
 
-    last_time = current_time;
+    // last_time = current_time;
 
-    auto speeds = Motor::get_instance().get_speed();
+    // auto speeds = Motor::get_instance().get_speed();
-    double e = setpoint - speeds.at(0);
+    // double e = setpoint - speeds.at(0);
-    errror_sum += e * delta_seconds;
+    // errror_sum += e * delta_seconds;
-    double error_diff = (e - last_error) / delta_seconds;
+    // double error_diff = (e - last_error) / delta_seconds;
-    last_error = e;
+    // last_error = e;
-    double u = kp * e + errror_sum * ki + error_diff * kd;
+    // double u = kp * e + errror_sum * ki + error_diff * kd;
 
-    Motor::power(0, std::min(std::max(-100.0, u), 100.0));
+    // Motor::power(0, std::min(std::max(-100.0, u), 100.0));
 
-    spdlog::info("{} {:05.0f} {:05.0f}", delta_seconds_total, setpoint, speeds.at(0));
+    // spdlog::info("{} {:05.0f} {:05.0f}", delta_seconds_total, setpoint, speeds.at(0));
 
-    usleep(1000.0 * (1000.0 / 150.0));
+    usleep(1000.0 * (1000.0 / 10.0));
-
+    Motor::get_instance().set_speed(0, setpoint);
-    i += 1;
+    Motor::get_instance().set_speed(3, -setpoint);
+    // i += 1;
     // if (i % 1000 == 0) {
     //   setpoint *= -1;
     // }
+    auto speeds = Motor::get_instance().get_speed();
+    spdlog::info("Speed: {} {} Target: {}", speeds.at(0), speeds.at(3), setpoint);
   }
   return 0;
 }

server/src/motor.cpp

@@ -1,5 +1,6 @@
 #include <stdexcept>
 #include <unistd.h>
+#include <chrono>
 
 #include "include/motor.hpp"
 #include "include/spi.hpp"
@@ -7,18 +8,34 @@
 #include "include/encoder.hpp"
 #include <spdlog/spdlog.h>
 
-Motor::Motor() {
+void check_port(uint8_t port) {
-    reset_speed();
-}
-
-void Motor::power(uint8_t port, double percent) {
     if (port >= MOTOR_COUNT) {
         throw std::invalid_argument("Invalid motor port specified");
     }
+}
+
+Motor::Motor() {
+    reset_speed();
+
+    speed_control_thread = std::thread(&Motor::speed_control_loop, this);
+    speed_control_thread.detach();
+}
+
+void Motor::set_speed(uint8_t port, double rpm) {
+    check_port(port);
+    
+    speed_targets[port] = rpm;
+    motor_control_modes[port] = Motor::Control::SPEED;
+}
+
+void Motor::set_power(uint8_t port, double percent) {
+    check_port(port);
     if (abs(percent) > 100) {
         throw std::invalid_argument("Invalid motor percent specified. Should be -100 <= percent <= 100");
     }
 
+    motor_control_modes[port] = Motor::Control::POWER;
+
     Mode mode = Motor::Mode::COAST;
     if (percent < 0) {
         mode = Motor::Mode::BACKWARD;
@@ -27,14 +44,11 @@ void Motor::power(uint8_t port, double percent) {
     }
 
     uint16_t pwm = abs(percent) * (MAX_MOTOR_SPEED / 100.0);
-    Motor::pwm(port, pwm, mode);
+    Motor::set_pwm(port, pwm, mode);
-}
-
-void Motor::pwm(uint8_t port, uint16_t pwm, Mode mode) {
-    if (port >= MOTOR_COUNT) {
-        throw std::invalid_argument("Invalid motor port specified");
 }
 
+void Motor::set_pwm(uint8_t port, uint16_t pwm, Mode mode) {
+    check_port(port);
     if (mode > Motor::Mode::BREAK) {
         throw std::invalid_argument("Invalid motor mode specified");
     }
@@ -56,9 +70,9 @@ void Motor::pwm(uint8_t port, uint16_t pwm, Mode mode) {
 
 std::vector<double> Motor::get_speed() {
     auto current_time = std::chrono::high_resolution_clock::now();
-    long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_time_read).count() / 1000000.0;
+    long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_time_encoders_read).count() / 1000000.0;
     
-    last_time_read = current_time;
+    last_time_encoders_read = current_time;
 
     if (delta_seconds > MOTOR_MAX_DELTA_TIME_S) {
         reset_speed();
@@ -82,7 +96,7 @@ std::vector<double> Motor::get_speed() {
 }
 
 void Motor::reset_speed() {
-    last_time_read = std::chrono::high_resolution_clock::now();
+    last_time_encoders_read = std::chrono::high_resolution_clock::now();
 
     auto current_ticks = Encoder::read_all();
     for (int i = 0; i < MOTOR_COUNT; i++) {
@@ -90,3 +104,60 @@ void Motor::reset_speed() {
         last_encoder_values[i] = current_ticks.at(i);
     }
 }
+
+void Motor::speed_control_loop() {
+    double errror_sums[MOTOR_COUNT] = {0};
+    double last_errors[MOTOR_COUNT] = {0};
+
+    auto last_run = std::chrono::high_resolution_clock::now();
+    while (1 == 1) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000 / MOTOR_SPEED_CONTROL_LOOP_HZ));
+
+        bool should_control_speed = false;
+        for (int i = 0; i < MOTOR_COUNT; i++) {
+            if (motor_control_modes[i] == Motor::Control::SPEED) {
+                should_control_speed = true;
+            }
+        }
+
+        if (!should_control_speed) {
+            continue;
+        }
+
+        auto current_time = std::chrono::high_resolution_clock::now();
+        long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_run).count() / 1000000.0;
+        last_run = current_time;
+
+        if (delta_seconds > MOTOR_SPEED_CONTROL_RESET_DELAY_S) {
+            std::fill_n(errror_sums, MOTOR_COUNT, 0);
+            std::fill_n(last_errors, MOTOR_COUNT, 0);
+            continue;
+        }
+
+        auto speeds = Motor::get_instance().get_speed();
+        for (int i = 0; i < MOTOR_COUNT; i++) {
+            if (motor_control_modes[i] == Motor::Control::SPEED) {
+                // P
+                double error = speed_targets[i] - speeds.at(i);
+                // I
+                errror_sums[i] += error * delta_seconds;
+                // D
+                if (last_errors[i] == 0) { // Needed to prevent spike on first loop iteration
+                    last_errors[i] = error;
+                }
+                double error_diff = (error - last_errors[i]) / delta_seconds;
+
+                double power = MOTOR_SPEED_CONTROL_KP * error + 
+                    errror_sums[i] * MOTOR_SPEED_CONTROL_KI +
+                    error_diff * MOTOR_SPEED_CONTROL_KD;
+
+                set_power(i, std::min(std::max(-100.0, power), 100.0));
+                // spdlog::info("{:03.2f} {:03.2f} {:03.2f} {:03.2f} {:03.2f} {:03.2f} {:03.2f}", 
+                //     speed_targets[i], speeds.at(i), error, delta_seconds, last_errors[i], error_diff, power);
+
+                last_errors[i] = error;
+            }
+        }
+
+    }
+}