Framework for integration of swerve drive control

src/globals.h

@@ -32,18 +32,22 @@ extern long last_p;
 #define DRIVE_BASIC_BACKLEFT    5
 #define DRIVE_BASIC_BACKRIGHT   6
 
-typedef struct { 
+// Length of the buffer to monitor recent encoder positions to calculate speed
+// The buffer will track the last N states of the encoders, and the times at which they were recorded, to determine the robot's current speed
+#define ENCODER_BUFFER_ENTRY_COUNT 5
+
+typedef struct { // swerve_wheel struct, used to track and manage the state of the robot's swerve drive system
     // byte spin_direction = CLOCKWISE; // Current state, 0 = clockwise, 1 = anticlockwise
     // byte drive_direction = DRIVE_FORWARDS; // Current state, Directions: 0 = forwards, 1 = right, 2 = backwards, 3 = left
     // byte target_spin_direction = CLOCKWISE; // Target state
     // byte target_drive_direction = DRIVE_FORWARDS; // Target state
     
     byte drive_mode = DRIVE_BASIC;
-    float target_drive_power = 127.0; // 0.0 - 127.0
-    float current_drive_power = 127.0; // 0.0 - 127.0
+    float target_drive_power = 127.0; // 0.0 - 127.0 , TARGET speed: this is the speed that the robot is trying to get to
+    float current_drive_power = 127.0; // 0.0 - 127.0 : CURRENT / COMMAND speed (power) being given to drive motors (before coefficient is applied)
     
     float spin_location = 0; // 0 - 360, CURRENT tracked body orientation relative to initial (or reset) orientation
-    float spin_offset = 0; // 0 - 360, offset applied to target spin when setting
+    float spin_offset = 0; // 0 - 360, offset applied to target spin when setting, this allows modification of the robot's forward orientation relative to its body
     
     // CURRENT wheel orientations relative to robot body, read from encoders & convert
     float front_left_current_spin = 0;
@@ -51,7 +55,13 @@ typedef struct {
     float back_left_current_spin = 0;
     float back_right_current_spin = 0;
 
-    // TARGET wheel orientations relative to robot body
+    // COMMAND wheel orientations relative to robot body, this is what the motors are currently being told to do
+    float front_left_ccommand_spin = 0;
+    float front_right_command_spin = 0;
+    float back_left_command_spin = 0;
+    float back_right_command_spin = 0;
+
+    // TARGET wheel orientations relative to robot body, this is the state that the robot is trying to get to
     float front_left_target_spin = 0;
     float front_right_target_spin = 0;
     float back_left_target_spin = 0;
@@ -63,6 +73,17 @@ typedef struct {
     float front_right_coefficient = 0;
     float back_left_coefficient = 0;
     float back_right_coefficient = 0;
+
+    
+    
+    // Encoder tracking, used to track speed of the robot to determine when it is safe to reorient the wheels
+    // The 0th entry in the buffer is the most recent, the highest entry is the oldest
+    // The buffer is modified each time updateSwerveCommand() is called
+    int encoder_buffer_times_ms[ENCODER_BUFFER_ENTRY_COUNT]; // Buffer that tracks the times at which encoder states were measures, uses milliseconds (either unix millis or relative to pico start)
+    int encoder_buffer_front_left[ENCODER_BUFFER_ENTRY_COUNT];
+    int encoder_buffer_front_right[ENCODER_BUFFER_ENTRY_COUNT];
+    int encoder_buffer_back_left[ENCODER_BUFFER_ENTRY_COUNT];
+    int encoder_buffer_back_right[ENCODER_BUFFER_ENTRY_COUNT];
     
 } swerve_wheel;
 

src/swerve.cpp

@@ -29,6 +29,29 @@ float closestAngle(float current, float target)
         return dir;
 }
 
+// TODO as of 20230926
+// This function calculates the robot's current speed and attempts to modify the current state of the drive towards the target drive state
+swerve_drive updateSwerveCommand(swerve_drive input) {
+    swerve_drive out = input;
+
+    // Monitor encoder values for each wheel and place them in the encoder_buffer arrays in the swerve_drive struct
+        // TODO, get encoder values for each drive motor and place them in the arrays
+        // TODO, get time in milliseconds (either unix millis or relative millis to pico startup)
+
+    // Calculate the robot's average speed over the last few encoder samples
+        // TODO
+    
+    // Determine how much it is safe to change the orientation of the wheels, given the robot's current speed
+        // TODO
+
+    // Update every "current" variable in the swerve_drive struct with new data
+        // TODO
+    
+    // Set the new commanded orientation of the rotation motors and the new commanded drive motor speed
+        // TODO
+
+    return out;
+}
 
 /*
 // TODO as of 20230923 for setDirection() : fix to work with modifications made to swerve_drive struct on 20230922

src/swerve.h

@@ -5,6 +5,8 @@ float closestAngle(float current, float target);
 // TODO as of 20230923 for setDirection() : fix to work with modifications made to swerve_drive struct on 20230922
 // swerve_wheel setDirection(swerve_wheel input, float setpoint);
 
+swerve_drive updateSwerveCommand(swerve_drive input); // This function calculates the robot's current speed and attempts to modify the current state of the drive towards the target drive state
+
 swerve_wheel tempTranslationDrive(swerve_wheel input, float target_speed, float target_angle); // Temporary implementation for translation drive mode
 
 swerve_wheel tempRotationDrive(swerve_wheel input, float target_speed) // Temporary implementation for rotation drive mode (rotating in place), positive speed is clockwise, negative speed is counterclockwise