Rework PID

server/include/motor.hpp

@@ -54,6 +54,8 @@ private:
 
     void reset_speed();
     void speed_control_loop();
+
+    void _set_power(uint8_t port, double percent);
 };
 
 #endif // COMPLIB_SERVER_MOTOR_HPP

server/include/pid.hpp

@@ -0,0 +1,30 @@
+#ifndef COMPLIB_SERVER_PID_HPP
+#define COMPLIB_SERVER_PID_HPP
+
+#include <chrono>
+
+#define PID_RESET_DELAY_S 0.1L
+
+class PID
+{
+public:
+    PID(float P, float I, float D, float ramp, float limit);
+    ~PID() = default;
+
+    float operator() (float setpoint, float process_variable);
+    void reset();
+
+    float P = 1;
+    float I = 0;
+    float D = 0;
+    float setpoint_ramp = 0;
+    float limit = 0;
+
+protected:
+    float error_prev = 0;
+    float setpoint_prev = 0;
+    float integral_prev = 0;
+    std::chrono::system_clock::time_point timestamp_prev = std::chrono::high_resolution_clock::now();
+};
+
+#endif // COMPLIB_SERVER_PID_HPP

server/include/spi.hpp

@@ -1,6 +1,8 @@
 #ifndef COMPLIB_SERVER_SPI_HPP
 #define COMPLIB_SERVER_SPI_HPP
 
+#include <mutex>
+
 //SPI_MODE_0 (0,0) 	CPOL = 0, CPHA = 0, Clock idle low, data is clocked in on rising edge, output data (change) on falling edge
 //SPI_MODE_1 (0,1) 	CPOL = 0, CPHA = 1, Clock idle low, data is clocked in on falling edge, output data (change) on rising edge
 //SPI_MODE_2 (1,0) 	CPOL = 1, CPHA = 0, Clock idle high, data is clocked in on falling edge, output data (change) on rising edge
@@ -106,7 +108,17 @@ public:
         DISPLAY_LINE_1_C0 = 69,
         DISPLAY_LINE_2_C0 = 85,
         DISPLAY_LINE_3_C0 = 101,
-        DISPLAY_LINE_4_C0 = 117
+        DISPLAY_LINE_4_C0 = 117,
+
+        // Motor encoder velocities
+        MOTOR_1_VEL_H = 118,
+        MOTOR_1_VEL_L = 119,
+        MOTOR_2_VEL_H = 120,
+        MOTOR_2_VEL_L = 121,
+        MOTOR_3_VEL_H = 122,
+        MOTOR_3_VEL_L = 123,
+        MOTOR_4_VEL_H = 124,
+        MOTOR_4_VEL_L = 125
     };
 
 private:
@@ -116,6 +128,8 @@ private:
     uint8_t tx_buffer[SPI_BUFFER_SIZE] = {0};
     uint8_t rx_buffer[SPI_BUFFER_SIZE] = {0};
 
+    std::recursive_mutex spi_mutex;
+
     void transfer();
     void clear_buffers();