Swerve odometry core logic

.vscode/tasks.json

@@ -5,6 +5,7 @@
             "label": "Clear Terminal",
             "type": "shell",
             "command": "clear",
+            "windows": { "command": "cls" },
             "presentation": {
                 "reveal": "never",
                 "clear": true

Swerve-Standard/inc/robot.h

@@ -2,6 +2,7 @@
 #define ROBOT_H
 
 #include "user_math.h"
+#include "swerve_odometry.h"
 #include <stdint.h>
 
 typedef enum Robot_State_e {
@@ -18,6 +19,11 @@ typedef struct {
     float omega;
     uint8_t IS_SPINTOP_ENABLED;
 
+    float yaw;
+
+    // Estimated pose according to wheel odometry
+    pose_2d pose;
+
     // power management
     uint16_t power_index;
     uint16_t power_count;

Swerve-Standard/src/chassis_task.c

@@ -3,14 +3,20 @@
 #include "robot.h"
 #include "remote.h"
 #include "dji_motor.h"
+#include "imu_task.h"
 #include "motor.h"
 #include "referee_system.h"
 #include "swerve_locomotion.h"
+#include "swerve_odometry.h"
 #include "rate_limiter.h"
 
+#include "FreeRTOS.h"
+#include "task.h"
+
 extern Robot_State_t g_robot_state;
 extern Remote_t g_remote;
 extern Referee_System_t Referee_System;
+extern IMU_t g_imu;
 
 DJI_Motor_Handle_t *g_azimuth_motors[NUMBER_OF_MODULES];
 DJI_Motor_Handle_t *g_drive_motors[NUMBER_OF_MODULES];
@@ -25,8 +31,10 @@ float chassis_rad = WHEEL_BASE * 1.414f; //TODO init?
 
 float g_spintop_omega = SPIN_TOP_OMEGA;
 
-void Chassis_Task_Init()
-{
+float prev_yaw;
+unsigned int last_odom_update;
+
+void Chassis_Task_Init() {
     // init common PID configuration for azimuth motors
     Motor_Config_t azimuth_motor_config = {
         .control_mode = POSITION_VELOCITY_SERIES,
@@ -94,16 +102,21 @@ void Chassis_Task_Init()
         g_drive_motors[i] = DJI_Motor_Init(&drive_motor_config, M3508);
     }
 
-    // Initialize the swerve locomotion constants
+    // // Initialize the swerve locomotion constants
     g_swerve_constants = swerve_init(TRACK_WIDTH, WHEEL_BASE, WHEEL_DIAMETER, SWERVE_MAX_SPEED, SWERVE_MAX_ANGLUAR_SPEED);
 
-    // Initialize the rate limiters
+    // // Initialize the rate limiters
     for (int i = 0; i < NUMBER_OF_MODULES; i++)
     {
         rate_limiter_init(&chassis_vel_limiters[i], SWERVE_MAX_WHEEL_ACCEL);
     }
     #define SWERVE_MAX_OMEGA_ACCEL (5.0f)
     rate_limiter_init(&chassis_omega_limiter, SWERVE_MAX_OMEGA_ACCEL);
+
+    g_robot_state.chassis.pose = (pose_2d){0, 0, 0};
+
+    prev_yaw = g_imu.rad.yaw;
+    last_odom_update = xTaskGetTickCount();
 }
 
 void Chassis_Ctrl_Loop()
@@ -157,6 +170,13 @@ void Chassis_Ctrl_Loop()
     swerve_calculate_kinematics(&g_chassis_state, &g_swerve_constants);
     swerve_optimize_module_angles(&g_chassis_state, measured_angles);
     //swerve_desaturate_wheel_speeds(&g_chassis_state, &g_swerve_constants);
+
+    // Update odometry
+    unsigned int curr_time = xTaskGetTickCount();
+    float dt = (curr_time - last_odom_update) / (float)configTICK_RATE_HZ; // convert to seconds
+    swerve_odometry_update(&g_robot_state.chassis.pose, g_chassis_state.states, &g_swerve_constants, g_robot_state.chassis.yaw, prev_yaw, dt);
+    last_odom_update = curr_time;
+    prev_yaw = g_imu.rad.yaw;
 
     // rate limit the module speeds
     for (int i = 0; i < NUMBER_OF_MODULES; i++) {

Swerve-Standard/src/robot.c

@@ -2,6 +2,7 @@
 
 #include "robot_tasks.h"
 #include "chassis_task.h"
+#include "dji_motor.h"
 #include "gimbal_task.h"
 #include "launch_task.h"
 #include "gimbal_task.h"
@@ -12,11 +13,14 @@
 #include "user_math.h"
 #include "math.h"
 #include "rate_limiter.h"
+#include "imu_task.h"
 
 Robot_State_t g_robot_state = {0};
 extern Remote_t g_remote;
 extern Supercap_t g_supercap;
 
+extern IMU_t g_imu;
+
 extern DJI_Motor_Handle_t *g_yaw;
 
 Input_State_t g_input_state = {0};
@@ -115,9 +119,13 @@ void Process_Remote_Input()
     g_robot_state.input.vx = temp_x;
     g_robot_state.input.vy = temp_y;
 
+    // Update chassis yaw angle based on gimble IMU
+    // TODO: Place an IMU directly on the chassis, this is kind of jank
+    // UNTESTED!!! If odometry isn't working, it's probably this
+    float theta = DJI_Motor_Get_Absolute_Angle(g_yaw);
+    g_robot_state.chassis.yaw = g_imu.rad.yaw - theta;
 
     // Calculate Gimbal Oriented Control
-    float theta = DJI_Motor_Get_Absolute_Angle(g_yaw);
     g_robot_state.chassis.x_speed = -g_robot_state.input.vy * sin(theta) + g_robot_state.input.vx * cos(theta);
     g_robot_state.chassis.y_speed = g_robot_state.input.vy * cos(theta) + g_robot_state.input.vx * sin(theta);
 
@@ -157,12 +165,12 @@ void Process_Remote_Input()
     }
 
     if (g_remote.controller.left_switch == UP) { // Left switch high to enable spintop
-        //g_robot_state.chassis.IS_SPINTOP_ENABLED = 1;
-        //g_robtot_state.launch.IS_FIRING_ENABLED = 1;
+        // g_robot_state.chassis.IS_SPINTOP_ENABLED = 1;
+        // g_robot_state.launch.IS_FIRING_ENABLED = 1;
         g_robot_state.launch.IS_AUTO_AIMING_ENABLED = 1;
     } else {
-        //g_robot_state.chassis.IS_SPINTOP_ENABLED = 0;
-        //g_robot_state.launch.IS_FIRING_ENABLED = 0;
+        // g_robot_state.chassis.IS_SPINTOP_ENABLED = 0;
+        // g_robot_state.launch.IS_FIRING_ENABLED = 0;
         g_robot_state.launch.IS_AUTO_AIMING_ENABLED = 0;
     }
 

control-base/algo/inc/swerve_locomotion.h

@@ -1,6 +1,8 @@
 #ifndef SWERVE_LOCOMOTION_H
 #define SWERVE_LOCOMOTION_H
 
+#include "vec_utils.h"
+
 #define NUMBER_OF_MODULES 4
 
 typedef struct
@@ -25,7 +27,8 @@ typedef struct
     float wheel_diameter;
     float max_speed;
     float max_angular_speed;
-    float kinematics_matrix[8][3];
+    Mat* kinematics_matrix;
+    Mat* forward_kinematics_matrix;
 } swerve_constants_t;
 
 swerve_constants_t swerve_init(float track_width, float wheel_base, float wheel_diameter, float max_speed, float max_angular_speed);

control-base/algo/inc/swerve_odometry.h

@@ -0,0 +1,15 @@
+#ifndef SWERVE_ODOMETRY_H
+#define SWERVE_ODOMETRY_H
+
+#include "swerve_locomotion.h"
+
+typedef struct {
+    float x; // x position in meters
+    float y; // y position in meters
+    float theta; // orientation in radians
+} pose_2d;
+
+void swerve_odometry_update(pose_2d *pose, module_state_t wheel_states[NUMBER_OF_MODULES],
+    swerve_constants_t *swerve_constants, float yaw, float previous_yaw, float dr);
+
+#endif // SWERVE_ODOMETRY_H

control-base/algo/inc/vec_utils.h

@@ -0,0 +1,112 @@
+#ifndef VEC_UTILS_H
+#define VEC_UTILS_H
+
+#include <stdbool.h>
+
+
+// MACROS
+#define MAT_IDX(m, i, j) ((m)->data[(i) * (m)->cols + (j)])
+#define VEC_IDX(m, i) ((m)->data[(i)])
+
+
+/*
+-------------------------------------------------------------
+SECTION:	MATRICES
+-------------------------------------------------------------
+*/
+
+// NxN Matrix struct
+typedef struct Mat {
+    int rows;
+    int cols;
+    float* data;
+} Mat;
+
+// matrix creation functions
+Mat* new_mat(int rows, int cols);
+Mat* new_eye(int size);
+Mat* new_mat_buffer(int rows, int cols, float* buffer);
+void free_mat(Mat* m);
+Mat* mat_copy(Mat* m);
+Mat* create_temp_mat(Mat* m);
+Mat* mat_execute_and_free(Mat* (*func)(void *, ...), ...);
+
+// matrix helpers
+char* mat_to_string(Mat* m);
+void print_mat(Mat* m);
+bool mat_equal(Mat* m1, Mat* m2, float tol);
+
+// general matrix operations
+Mat* mat_mult(Mat* m1, Mat* m2);
+Mat* mat_mult_buffer(Mat* m1, Mat* m2, Mat* product);
+Mat* mat_scalar_mult(Mat* m, float scalar);
+Mat* mat_scalar_mult_buffer(Mat* m, float scalar, Mat* product);
+Mat* mat_add(Mat* m1, Mat* m2);
+Mat* mat_add_buffer(Mat* m1, Mat* m2, Mat* sum);
+Mat* mat_sub(Mat* m1, Mat* m2);
+Mat* mat_sub_buffer(Mat* m1, Mat* m2, Mat* diff);
+
+// advanced matrix operations
+float mat_determinant(Mat* m);
+Mat* mat_adjoint(Mat* m);
+Mat* mat_adjoint_buffer(Mat* m, Mat* buffer);
+float mat_cofactor(Mat *m, int i, int j);
+Mat* mat_cofactor_matrix(Mat* m);
+Mat* mat_cofactor_matrix_buffer(Mat* m, Mat* buffer);
+Mat* mat_inverse(Mat* m);
+Mat* mat_inverse_buffer(Mat* m, Mat* buffer);
+Mat* mat_transpose(Mat *m);
+Mat* mat_transpose_buffer(Mat *m, Mat* buffer);
+Mat* mat_transpose_overwrite(Mat *m);
+Mat* mat_pseudo_inverse(Mat *m);
+Mat* mat_damped_pseudo_inverse(Mat* m, float rho);
+
+// TODO: ADD PSEUDO INVERSE
+
+// TODO: If I feel like it: LU DECOMPOSITION, QR DECOMPOSITION, SVD DECOMPOSITION, EIGEN DECOMPOSITION, SOLVE LINEAR SYSTEM, SOLVE EIGENVALUE PROBLEM, SOLVE EIGENVECTOR PROBLEM, SOLVE SVD PROBLEM
+
+/*
+-------------------------------------------------------------
+SECTION:	VECTORS
+-------------------------------------------------------------
+*/
+
+// To maintain compatibility we use 1D matrix for vectors. You can simply use the matrix functions for vectors.
+#define Vec Mat
+
+// vector creation functions
+Vec* new_vec(int size);
+Vec* new_vec_buffer(int size, float* buffer);
+
+// vector helpers
+bool assert_vec(Vec* v);
+
+// general vector operations
+float vec_dot(Vec* v1, Vec* v2);
+Vec* vec_cross(Vec* v1, Vec* v2);
+Vec* vec_cross_buffer(Vec* v1, Vec* v2, Vec* buffer);
+float vec_magnitude(Vec* v);
+Vec* vec_normalize(Vec* v);
+Vec* vec_normalize_overwrite(Vec* v);
+
+
+/*
+-------------------------------------------------------------
+SECTION:	DH TRANSFORMATIONS
+-------------------------------------------------------------
+*/
+
+typedef struct DH_Params {
+    float a;
+    float alpha;
+    float d;
+    float theta;
+} DH_Params;
+
+// Denavit-Hartenberg matrix calculations
+DH_Params* new_dh_params(float a, float alpha, float d, float theta);
+void free_dh_params(DH_Params* dh);
+Mat* dh_transform(DH_Params dh);
+Mat* dh_transform_buffer(DH_Params dh, Mat* buffer);
+
+#endif // VEC_UTILS_H