diff --git a/server/.idea/workspace.xml b/server/.idea/workspace.xml
new file mode 100644
index 0000000..e30254c
--- /dev/null
+++ b/server/.idea/workspace.xml
@@ -0,0 +1,187 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/server/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdC/CMakeCCompilerId.o b/server/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdC/CMakeCCompilerId.o
new file mode 100644
index 0000000..6551d10
Binary files /dev/null and b/server/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdC/CMakeCCompilerId.o differ
diff --git a/server/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdCXX/CMakeCXXCompilerId.o b/server/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdCXX/CMakeCXXCompilerId.o
new file mode 100644
index 0000000..ded33f1
Binary files /dev/null and b/server/cmake-build-debug/CMakeFiles/3.17.3/CompilerIdCXX/CMakeCXXCompilerId.o differ
diff --git a/server/cmake-build-debug/CMakeFiles/compLib_server.dir/CompLib.pb.cc.o b/server/cmake-build-debug/CMakeFiles/compLib_server.dir/CompLib.pb.cc.o
new file mode 100644
index 0000000..e87eceb
Binary files /dev/null and b/server/cmake-build-debug/CMakeFiles/compLib_server.dir/CompLib.pb.cc.o differ
diff --git a/server/cmake-build-debug/CMakeFiles/compLib_server.dir/src/Spi.cpp.o b/server/cmake-build-debug/CMakeFiles/compLib_server.dir/src/Spi.cpp.o
new file mode 100644
index 0000000..7a98719
Binary files /dev/null and b/server/cmake-build-debug/CMakeFiles/compLib_server.dir/src/Spi.cpp.o differ
diff --git a/server/cmake-build-debug/CMakeFiles/compLib_server.dir/src/main.cpp.o b/server/cmake-build-debug/CMakeFiles/compLib_server.dir/src/main.cpp.o
new file mode 100644
index 0000000..0b913ab
Binary files /dev/null and b/server/cmake-build-debug/CMakeFiles/compLib_server.dir/src/main.cpp.o differ
diff --git a/server_v2/.gitignore b/server_v2/.gitignore
index 5b4bcb6..8e2c66b 100644
--- a/server_v2/.gitignore
+++ b/server_v2/.gitignore
@@ -103,4 +103,6 @@ Temporary Items
# End of https://www.toptal.com/developers/gitignore/api/c++,macos,linux,visualstudiocode
+
+cmake-build*
build
diff --git a/server_v2/.idea/deployment.xml b/server_v2/.idea/deployment.xml
new file mode 100644
index 0000000..4205078
--- /dev/null
+++ b/server_v2/.idea/deployment.xml
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/server_v2/.idea/other.xml b/server_v2/.idea/other.xml
new file mode 100644
index 0000000..6db1952
--- /dev/null
+++ b/server_v2/.idea/other.xml
@@ -0,0 +1,10 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/server_v2/CMakeLists.txt b/server_v2/CMakeLists.txt
index 41eedd2..8da8431 100644
--- a/server_v2/CMakeLists.txt
+++ b/server_v2/CMakeLists.txt
@@ -25,13 +25,28 @@ set(PROTO_FILES
set(SRC_FILES
src/main.cpp
src/spi.cpp
- )
+ src/IRSensors.cpp
+ src/Cache.cpp
+ src/Motors.cpp
+ src/Encoders.cpp
+ src/ClosedLoopMotorController.cpp
+ src/PID.cpp
+ src/OdometryController.cpp
+ src/Odometry.cpp)
set(HDR_FILES
include/spi.hpp
include/reset.hpp
include/mathUtils.hpp
- )
+ include/IRSensors.hpp
+ include/Robot.hpp
+ include/Cache.hpp
+ include/Motors.hpp
+ include/Encoders.hpp
+ include/ClosedLoopMotorController.hpp
+ include/PID.hpp
+ include/OdometryController.hpp
+ include/Odometry.hpp)
include_directories(third_party/asio)
diff --git a/server_v2/include/Cache.hpp b/server_v2/include/Cache.hpp
new file mode 100644
index 0000000..c4d4dc9
--- /dev/null
+++ b/server_v2/include/Cache.hpp
@@ -0,0 +1,23 @@
+#ifndef COMPLIB_SERVER_CACHE_HPP
+#define COMPLIB_SERVER_CACHE_HPP
+
+#include
+
+class Cache {
+public:
+ explicit Cache(int rate_hz);
+
+ bool is_expired();
+
+ void update();
+
+private:
+
+ typedef std::chrono::steady_clock clock;
+
+ std::chrono::microseconds cache_duration{};
+ std::chrono::time_point last_update{};
+};
+
+
+#endif //COMPLIB_SERVER_CACHE_HPP
diff --git a/server_v2/include/ClosedLoopMotorController.hpp b/server_v2/include/ClosedLoopMotorController.hpp
new file mode 100644
index 0000000..ded8143
--- /dev/null
+++ b/server_v2/include/ClosedLoopMotorController.hpp
@@ -0,0 +1,46 @@
+#ifndef COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP
+#define COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP
+
+
+#include
+#include
+#include
+
+#include "include/PID.hpp"
+#include "include/Robot.hpp"
+
+class ClosedLoopMotorController {
+public:
+ static ClosedLoopMotorController &getInstance() {
+ static ClosedLoopMotorController instance;
+ return instance;
+ }
+
+ ClosedLoopMotorController(ClosedLoopMotorController const &) = delete;
+
+ void operator=(ClosedLoopMotorController const &) = delete;
+
+ void set_power(uint8_t port, double power);
+
+ void set_speed(uint8_t port, double speed_rpm);
+
+private:
+ enum ControlMode : uint8_t {
+ NONE = 0,
+ POWER = 1,
+ SPEED = 2
+ };
+
+ ClosedLoopMotorController();
+
+ [[noreturn]] void speed_control_loop();
+
+ std::array pids;
+ std::array control_modes{ControlMode::NONE};
+ std::array speed_targets{0};
+
+ std::thread speed_control_thread;
+};
+
+
+#endif //COMPLIB_SERVER_CLOSEDLOOPMOTORCONTROLLER_HPP
diff --git a/server_v2/include/Encoders.hpp b/server_v2/include/Encoders.hpp
new file mode 100644
index 0000000..612419c
--- /dev/null
+++ b/server_v2/include/Encoders.hpp
@@ -0,0 +1,35 @@
+#ifndef COMPLIB_SERVER_ENCODERS_HPP
+#define COMPLIB_SERVER_ENCODERS_HPP
+
+#include
+#include
+#include "include/Cache.hpp"
+#include "include/Robot.hpp"
+
+class Encoders {
+public:
+ static Encoders &getInstance() {
+ static Encoders instance;
+ return instance;
+ }
+
+ Encoders(Encoders const &) = delete;
+
+ void operator=(Encoders const &) = delete;
+
+ std::array get_positions();
+
+ std::array get_velocities_rpm();
+
+private:
+ Encoders() = default;
+
+ Cache position_cache{ROBOT_ENCODER_RATE_HZ};
+ Cache velocity_cache{ROBOT_ENCODER_RATE_HZ};
+
+ std::array cached_positions = {0};
+ std::array cached_velocities_rpm = {0};
+};
+
+
+#endif //COMPLIB_SERVER_ENCODERS_HPP
diff --git a/server_v2/include/IRSensors.hpp b/server_v2/include/IRSensors.hpp
new file mode 100644
index 0000000..b41374d
--- /dev/null
+++ b/server_v2/include/IRSensors.hpp
@@ -0,0 +1,37 @@
+#ifndef COMPLIB_SERVER_IRSENSORS_HPP
+#define COMPLIB_SERVER_IRSENSORS_HPP
+
+#include
+#include
+
+#include "include/Cache.hpp"
+#include "include/Robot.hpp"
+
+class IRSensors {
+public:
+ static IRSensors &getInstance() {
+ static IRSensors instance;
+ return instance;
+ }
+
+ IRSensors(IRSensors const &) = delete;
+
+ void operator=(IRSensors const &) = delete;
+
+ static void enable();
+
+ static void disable();
+
+ std::array read();
+
+private:
+
+ IRSensors() = default;
+
+ Cache cache{ROBOT_IR_RATE_HZ};
+
+ std::array cached_values = {0};
+};
+
+
+#endif //COMPLIB_SERVER_IRSENSORS_HPP
diff --git a/server_v2/include/Motors.hpp b/server_v2/include/Motors.hpp
new file mode 100644
index 0000000..7688804
--- /dev/null
+++ b/server_v2/include/Motors.hpp
@@ -0,0 +1,26 @@
+#ifndef COMPLIB_SERVER_MOTORS_HPP
+#define COMPLIB_SERVER_MOTORS_HPP
+
+#include
+
+class Motors {
+public:
+ enum Mode : uint8_t {
+ COAST = 0,
+ FORWARD = 1,
+ BACKWARD = 2,
+ BREAK = 3,
+ SERVO = 4,
+ NONE = 5
+ };
+
+ static void set_power(uint8_t port, double percent);
+
+ static void set_pwm(uint8_t port, uint16_t pwm, Mode mode);
+
+private:
+
+ Motors() = default;
+};
+
+#endif //COMPLIB_SERVER_MOTORS_HPP
diff --git a/server_v2/include/Odometry.hpp b/server_v2/include/Odometry.hpp
new file mode 100644
index 0000000..cc51a01
--- /dev/null
+++ b/server_v2/include/Odometry.hpp
@@ -0,0 +1,24 @@
+#ifndef COMPLIB_SERVER_ODOMETRY_HPP
+#define COMPLIB_SERVER_ODOMETRY_HPP
+
+class Odometry {
+
+public:
+ Odometry();
+
+ Odometry(double x_position, double y_position, double angular_orientation);
+
+ double get_x_position() const;
+
+ double get_y_position() const;
+
+ double get_angular_orientation() const;
+
+
+private:
+ double x_position;
+ double y_position;
+ double angular_orientation;
+};
+
+#endif //COMPLIB_SERVER_ODOMETRY_HPP
diff --git a/server_v2/include/OdometryController.hpp b/server_v2/include/OdometryController.hpp
new file mode 100644
index 0000000..1be1e95
--- /dev/null
+++ b/server_v2/include/OdometryController.hpp
@@ -0,0 +1,46 @@
+#ifndef COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP
+#define COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP
+
+
+#include
+#include
+
+#include "include/Odometry.hpp"
+
+class OdometryController {
+public:
+ static OdometryController &getInstance() {
+ static OdometryController instance;
+ return instance;
+ }
+
+ OdometryController(OdometryController const &) = delete;
+
+ void operator=(OdometryController const &) = delete;
+
+ void enable();
+
+ void disable();
+
+ void reset();
+
+ Odometry get();
+
+private:
+ typedef std::chrono::steady_clock clock;
+
+ OdometryController();
+
+ std::thread odometry_thread;
+
+ [[noreturn]] void odometry_loop();
+
+ Odometry current_odometry{};
+ bool enabled = false;
+ double last_position_left{0};
+ double last_position_right{0};
+ std::chrono::time_point last_run;
+};
+
+
+#endif //COMPLIB_SERVER_ODOMETRYCONTROLLER_HPP
diff --git a/server_v2/include/PID.hpp b/server_v2/include/PID.hpp
new file mode 100644
index 0000000..2422a20
--- /dev/null
+++ b/server_v2/include/PID.hpp
@@ -0,0 +1,32 @@
+#ifndef COMPLIB_SERVER_PID_HPP
+#define COMPLIB_SERVER_PID_HPP
+
+#include
+
+class PID {
+public:
+ PID();
+
+ PID(double P, double I, double D, double ramp, double limit);
+
+ ~PID() = default;
+
+ double operator()(double setpoint, double process_variable);
+
+ double P = 1;
+ double I = 0;
+ double D = 0;
+ double setpoint_ramp = 0;
+ double limit = 0;
+
+private:
+ typedef std::chrono::steady_clock clock;
+#
+
+ double error_prev = 0;
+ double setpoint_prev = 0;
+ double integral_prev = 0;
+ clock::time_point timestamp_prev = clock::now();
+};
+
+#endif // COMPLIB_SERVER_PID_HPP
\ No newline at end of file
diff --git a/server_v2/include/Robot.hpp b/server_v2/include/Robot.hpp
new file mode 100644
index 0000000..28164eb
--- /dev/null
+++ b/server_v2/include/Robot.hpp
@@ -0,0 +1,31 @@
+#ifndef COMPLIB_SERVER_ROBOT_HPP
+#define COMPLIB_SERVER_ROBOT_HPP
+
+#include
+
+#define ROBOT_IR_SENSOR_COUNT 5
+#define ROBOT_IR_RATE_HZ 250
+
+#define ROBOT_MOTOR_COUNT 4
+#define ROBOT_MOTOR_SPEED_CONTROL_KP 0.5
+#define ROBOT_MOTOR_SPEED_CONTROL_KI 5.0
+#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.025
+#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 100.0
+#define ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ 250
+
+#define ROBOT_ODOMETRY_CONTROLLER_RATE_HZ 250
+#define ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT 3
+#define ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT 0
+#define ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT 1.0
+#define ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT -1.0
+
+#define ROBOT_ENCODER_RATE_HZ 250
+
+#define ROBOT_WHEEL_CIRCUMFERENCE_MM (71.0 * M_PI)
+#define ROBOT_TICKS_PER_TURN (27.7 * 100.0)
+#define ROBOT_ARBOR_LENGTH_MM 139.0
+#define ROBOT_ARBOR_LENGTH_M (ROBOT_ARBOR_LENGTH_MM / 1000.0)
+#define ROBOT_TICKS_PER_METER (1000.0 / ROBOT_WHEEL_CIRCUMFERENCE_MM * ROBOT_TICKS_PER_TURN)
+
+
+#endif //COMPLIB_SERVER_ROBOT_HPP
diff --git a/server_v2/include/mathUtils.hpp b/server_v2/include/mathUtils.hpp
index b4eee56..603c6fd 100644
--- a/server_v2/include/mathUtils.hpp
+++ b/server_v2/include/mathUtils.hpp
@@ -4,24 +4,36 @@
#include
namespace mathUtils {
- inline int int_from_bytes(uint8_t *data, int length) {
- int ret = 0;
+ inline int int_from_bytes(uint8_t *data, int length) {
+ int ret = 0;
- int i = 0;
- for (int j = length -1; j >= 0; j--) {
- ret = ret | (data[i] << (j * 8));
- i++;
- }
- return ret;
+ int i = 0;
+ for (int j = length - 1; j >= 0; j--) {
+ ret = ret | (data[i] << (j * 8));
+ i++;
}
+ return ret;
+ }
- inline void bytes_from_int(int data, int length, uint8_t *result) {
- int i = 0;
- for (int j = length -1; j >= 0; j--) {
- result[i] = ((data >> (j * 8)) & 0xffu);
- i++;
- }
+ template
+ inline T from_bytes(const uint8_t *data, int length) {
+ T ret = 0;
+
+ int i = 0;
+ for (int j = length - 1; j >= 0; j--) {
+ ret = ret | (data[i] << (j * 8));
+ i++;
}
+ return ret;
+ }
+
+ inline void bytes_from_int(int data, int length, uint8_t *result) {
+ int i = 0;
+ for (int j = length - 1; j >= 0; j--) {
+ result[i] = ((data >> (j * 8)) & 0xffu);
+ i++;
+ }
+ }
}
#endif // COMPLIB_SERVER_MATHUTILS_HPP
\ No newline at end of file
diff --git a/server_v2/include/spi.hpp b/server_v2/include/spi.hpp
index c48e5a4..376b222 100644
--- a/server_v2/include/spi.hpp
+++ b/server_v2/include/spi.hpp
@@ -14,126 +14,129 @@
class Spi {
public:
- static Spi& getInstance()
- {
- static Spi instance;
- return instance;
- }
+ static Spi &getInstance() {
+ static Spi instance;
+ return instance;
+ }
- Spi(Spi const&) = delete;
- void operator=(Spi const&) = delete;
+ Spi(Spi const &) = delete;
- int read(uint8_t reg, uint8_t length);
- void read_array(uint8_t reg, uint8_t length, uint8_t* data);
-
- void write(uint8_t reg, uint8_t length, int value);
- void write_array(uint8_t reg, uint8_t length, const uint8_t* data);
-
- enum Register : uint8_t {
- IDENTIFICATION_MODEL_ID = 1,
- IDENTIFICATION_MODEL_REV_MAJOR = 2,
- IDENTIFICATION_MODEL_REV_MINOR = 3,
- IDENTIFICATION_MODEL_REV_PATCH = 4,
+ void operator=(Spi const &) = delete;
- // Motor encoder positions
- MOTOR_1_POS_B3 = 10,
- MOTOR_1_POS_B2 = 11,
- MOTOR_1_POS_B1 = 12,
- MOTOR_1_POS_B0 = 13,
- MOTOR_2_POS_B3 = 14,
- MOTOR_2_POS_B2 = 15,
- MOTOR_2_POS_B1 = 16,
- MOTOR_2_POS_B0 = 17,
- MOTOR_3_POS_B3 = 18,
- MOTOR_3_POS_B2 = 19,
- MOTOR_3_POS_B1 = 20,
- MOTOR_3_POS_B0 = 21,
- MOTOR_4_POS_B3 = 22,
- MOTOR_4_POS_B2 = 23,
- MOTOR_4_POS_B1 = 24,
- MOTOR_4_POS_B0 = 25,
+ int read(uint8_t reg, uint8_t length);
- // PWM Control Modes
- PWM_1_CTRL = 26,
- PWM_2_CTRL = 27,
- PWM_3_CTRL = 28,
- PWM_4_CTRL = 29,
+ void read_array(uint8_t reg, uint8_t length, uint8_t *data);
- // Motor pwm speed
- MOTOR_1_PWM_H = 30,
- MOTOR_1_PWM_L = 31,
- MOTOR_2_PWM_H = 32,
- MOTOR_2_PWM_L = 33,
- MOTOR_3_PWM_H = 34,
- MOTOR_3_PWM_L = 35,
- MOTOR_4_PWM_H = 36,
- MOTOR_4_PWM_L = 37,
+ void write(uint8_t reg, uint8_t length, int value);
- // Servo goal position
- SERVO_1_PWM_H = 38,
- SERVO_1_PWM_L = 39,
- SERVO_2_PWM_H = 40,
- SERVO_2_PWM_L = 41,
- SERVO_3_PWM_H = 42,
- SERVO_3_PWM_L = 43,
- SERVO_4_PWM_H = 44,
- SERVO_4_PWM_L = 45,
- SERVO_5_PWM_H = 46,
- SERVO_5_PWM_L = 47,
- SERVO_6_PWM_H = 48,
- SERVO_6_PWM_L = 49,
- SERVO_7_PWM_H = 50,
- SERVO_7_PWM_L = 51,
- SERVO_8_PWM_H = 52,
- SERVO_8_PWM_L = 53,
+ void write_array(uint8_t reg, uint8_t length, const uint8_t *data);
- // IR Sensor value
- IR_1_H = 54,
- IR_1_L = 55,
- IR_2_H = 56,
- IR_2_L = 57,
- IR_3_H = 58,
- IR_3_L = 59,
- IR_4_H = 60,
- IR_4_L = 61,
- IR_5_H = 62,
- IR_5_L = 63,
- IR_1_LED = 64,
- IR_2_LED = 65,
- IR_3_LED = 66,
- IR_4_LED = 67,
- IR_5_LED = 68,
+ enum Register : uint8_t {
+ IDENTIFICATION_MODEL_ID = 1,
+ IDENTIFICATION_MODEL_REV_MAJOR = 2,
+ IDENTIFICATION_MODEL_REV_MINOR = 3,
+ IDENTIFICATION_MODEL_REV_PATCH = 4,
- // Display registers
- DISPLAY_LINE_1_C0 = 69,
- DISPLAY_LINE_2_C0 = 85,
- DISPLAY_LINE_3_C0 = 101,
- DISPLAY_LINE_4_C0 = 117,
+ // Motor encoder positions
+ MOTOR_1_POS_B3 = 10,
+ MOTOR_1_POS_B2 = 11,
+ MOTOR_1_POS_B1 = 12,
+ MOTOR_1_POS_B0 = 13,
+ MOTOR_2_POS_B3 = 14,
+ MOTOR_2_POS_B2 = 15,
+ MOTOR_2_POS_B1 = 16,
+ MOTOR_2_POS_B0 = 17,
+ MOTOR_3_POS_B3 = 18,
+ MOTOR_3_POS_B2 = 19,
+ MOTOR_3_POS_B1 = 20,
+ MOTOR_3_POS_B0 = 21,
+ MOTOR_4_POS_B3 = 22,
+ MOTOR_4_POS_B2 = 23,
+ MOTOR_4_POS_B1 = 24,
+ MOTOR_4_POS_B0 = 25,
- // 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
- };
+ // PWM Control Modes
+ PWM_1_CTRL = 26,
+ PWM_2_CTRL = 27,
+ PWM_3_CTRL = 28,
+ PWM_4_CTRL = 29,
+
+ // Motor pwm speed
+ MOTOR_1_PWM_H = 30,
+ MOTOR_1_PWM_L = 31,
+ MOTOR_2_PWM_H = 32,
+ MOTOR_2_PWM_L = 33,
+ MOTOR_3_PWM_H = 34,
+ MOTOR_3_PWM_L = 35,
+ MOTOR_4_PWM_H = 36,
+ MOTOR_4_PWM_L = 37,
+
+ // Servo goal position
+ SERVO_1_PWM_H = 38,
+ SERVO_1_PWM_L = 39,
+ SERVO_2_PWM_H = 40,
+ SERVO_2_PWM_L = 41,
+ SERVO_3_PWM_H = 42,
+ SERVO_3_PWM_L = 43,
+ SERVO_4_PWM_H = 44,
+ SERVO_4_PWM_L = 45,
+ SERVO_5_PWM_H = 46,
+ SERVO_5_PWM_L = 47,
+ SERVO_6_PWM_H = 48,
+ SERVO_6_PWM_L = 49,
+ SERVO_7_PWM_H = 50,
+ SERVO_7_PWM_L = 51,
+ SERVO_8_PWM_H = 52,
+ SERVO_8_PWM_L = 53,
+
+ // IR Sensor value
+ IR_1_H = 54,
+ IR_1_L = 55,
+ IR_2_H = 56,
+ IR_2_L = 57,
+ IR_3_H = 58,
+ IR_3_L = 59,
+ IR_4_H = 60,
+ IR_4_L = 61,
+ IR_5_H = 62,
+ IR_5_L = 63,
+ IR_1_LED = 64,
+ IR_2_LED = 65,
+ IR_3_LED = 66,
+ IR_4_LED = 67,
+ IR_5_LED = 68,
+
+ // Display registers
+ DISPLAY_LINE_1_C0 = 69,
+ DISPLAY_LINE_2_C0 = 85,
+ DISPLAY_LINE_3_C0 = 101,
+ DISPLAY_LINE_4_C0 = 117,
+
+ // Motor encoder velocities
+ MOTOR_1_VEL_H = 133,
+ MOTOR_1_VEL_L = 134,
+ MOTOR_2_VEL_H = 135,
+ MOTOR_2_VEL_L = 136,
+ MOTOR_3_VEL_H = 137,
+ MOTOR_3_VEL_L = 138,
+ MOTOR_4_VEL_H = 139,
+ MOTOR_4_VEL_L = 140
+ };
private:
- Spi();
+ Spi();
- int spi_file_descriptor{};
- uint8_t tx_buffer[SPI_BUFFER_SIZE] = {0};
- uint8_t rx_buffer[SPI_BUFFER_SIZE] = {0};
+ int spi_file_descriptor{};
+ uint8_t tx_buffer[SPI_BUFFER_SIZE] = {0};
+ uint8_t rx_buffer[SPI_BUFFER_SIZE] = {0};
- std::recursive_mutex spi_mutex;
+ std::recursive_mutex spi_mutex;
- void transfer();
- void clear_buffers();
+ void transfer();
- uint8_t calculate_checksum(uint8_t* data, uint8_t length);
+ void clear_buffers();
+
+ uint8_t calculate_checksum(uint8_t *data, uint8_t length);
};
diff --git a/server_v2/src/Cache.cpp b/server_v2/src/Cache.cpp
new file mode 100644
index 0000000..0cd9a0b
--- /dev/null
+++ b/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();
+}
diff --git a/server_v2/src/ClosedLoopMotorController.cpp b/server_v2/src/ClosedLoopMotorController.cpp
new file mode 100644
index 0000000..ea06ead
--- /dev/null
+++ b/server_v2/src/ClosedLoopMotorController.cpp
@@ -0,0 +1,49 @@
+#include
+#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);
+ }
+ }
+ }
+}
+
+
diff --git a/server_v2/src/Encoders.cpp b/server_v2/src/Encoders.cpp
new file mode 100644
index 0000000..0d8ad5f
--- /dev/null
+++ b/server_v2/src/Encoders.cpp
@@ -0,0 +1,29 @@
+#include "include/Encoders.hpp"
+#include "include/spi.hpp"
+#include "include/mathUtils.hpp"
+#include
+
+std::array 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(data + i * 4, 4);
+ }
+ position_cache.update();
+ }
+ return cached_positions;
+}
+
+std::array 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(data + i * 2, 2);
+ cached_velocities_rpm.at(i) = velocity_tps * 60.0 / ROBOT_TICKS_PER_TURN;
+ }
+ velocity_cache.update();
+ }
+ return cached_velocities_rpm;
+}
diff --git a/server_v2/src/IRSensors.cpp b/server_v2/src/IRSensors.cpp
new file mode 100644
index 0000000..9edd035
--- /dev/null
+++ b/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 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(data + i * 2, 2);
+ }
+ cache.update();
+ }
+ return cached_values;
+}
\ No newline at end of file
diff --git a/server_v2/src/Motors.cpp b/server_v2/src/Motors.cpp
new file mode 100644
index 0000000..da44582
--- /dev/null
+++ b/server_v2/src/Motors.cpp
@@ -0,0 +1,31 @@
+#include "include/Motors.hpp"
+#include "include/spi.hpp"
+#include "include/Robot.hpp"
+#include
+#include
+
+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::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);
+}
+
diff --git a/server_v2/src/Odometry.cpp b/server_v2/src/Odometry.cpp
new file mode 100644
index 0000000..7e2343b
--- /dev/null
+++ b/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;
+}
diff --git a/server_v2/src/OdometryController.cpp b/server_v2/src/OdometryController.cpp
new file mode 100644
index 0000000..aed8698
--- /dev/null
+++ b/server_v2/src/OdometryController.cpp
@@ -0,0 +1,83 @@
+#include
+
+#include "include/OdometryController.hpp"
+#include "include/Robot.hpp"
+#include "include/Encoders.hpp"
+#include
+
+#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);
+ }
+ }
+}
diff --git a/server_v2/src/PID.cpp b/server_v2/src/PID.cpp
new file mode 100644
index 0000000..8ba5277
--- /dev/null
+++ b/server_v2/src/PID.cpp
@@ -0,0 +1,50 @@
+#include "include/PID.hpp"
+#include
+#include
+
+#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>(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;
+}
diff --git a/server_v2/src/main.cpp b/server_v2/src/main.cpp
index a80d960..0dddd15 100644
--- a/server_v2/src/main.cpp
+++ b/server_v2/src/main.cpp
@@ -1,12 +1,9 @@
#include
-#include
#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(end - start).count());
- spdlog::info("Loop rate: {}hz", SAMPLES / chrono::duration_cast(end - start).count() * 1000.0);
-
return 0;
}
-