Add steering angle tracking and dynamic loop delays

src/globals.h

@@ -14,9 +14,14 @@ extern long last_p;
 #define ANTICLOCKWISE 1
 */
 
+// Loop timing
+#define LOOP_DELAY_MS 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
+
 // Math things
 #define DEGREES_PER_RADIAN (180.0 / M_PI)
 #define RADIANS_PER_DEGREE (M_PI / 180.0)
+#define max(x,y) ( (x) > (y) ? (x) : (y) )
+#define min(x,y) ( (x) < (y) ? (x) : (y) )
 
 #define MOTOR_MAX_POWER 127.0 // Highest value accepted by motor control functions
 

src/main.cpp

@@ -36,10 +36,11 @@ byte d[] = { 0x7e, 0x30, 0x6d, 0x79, 0x33, 0x5b, 0x5f, 0x70, 0x7f, 0x7b, 0x77, 0
 PCF8574 ioex1(0x20, 20, 21);
 PCF8574 ioex2(0x21, 20, 21);
 
-PioEncoder enc1(18); // right1
+// JANK: soldered to pico or headers
-PioEncoder enc2(14); // left1
+PioEncoder enc1(18); // Front Left
-PioEncoder enc3(26); // right2
+PioEncoder enc2(14); // Front Right
-PioEncoder enc4(0); // left2 // JANK: soldered to pico or headers
+PioEncoder enc3(26); // Back Left
+PioEncoder enc4(0);  // Back Right
 
 // pins for arm servos
 #define ARM_SERVO_PIN_1 2
@@ -65,6 +66,8 @@ int stepperX_pos = 0;
 int stepperY_pos = 0;
 int stepperZ_pos = 0;
 
+uint64_t previous_loop_start_time = 0; // Track the previous loop start time, used to calculate how long to sleep for until the start of the next loop
+
 #define defMaxSpeed     8000
 #define defAcceleration 8000
 
@@ -867,6 +870,8 @@ void print_encoder_positions() { // Print encoder positions for steering motors
 }
 
 void loop() {
+  previous_loop_start_time = millis(); // Track the start time of this loop to determine how long to sleep before the next loop
+  
   rp2040.wdt_reset();
   
   comm_parse();
@@ -910,7 +915,7 @@ void loop() {
     swrv = rotationDrive(swrv, zeroed_rx);
   }
   
-  swrv = updateEncoderData(swrv, enc2.getCount(), enc1.getCount(), enc4.getCount(), enc3.getCount()); // Update encoder data in the swerve_drive struct
+  swrv = updateEncoderData(swrv, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount()); // Update encoder data in the swerve_drive struct
   swrv = updateSwerveCommand(swrv); // Calculate power for each drive and steering motor
 
   print_encoder_positions(); // DEBUG ONLY, print steering motor encoder positions
@@ -1071,7 +1076,11 @@ void loop() {
     }
   }*/
   //delay(200);
-  delay(50);
+
+  int64_t delay_time_ms = LOOP_DELAY_MS - (int64_t) (millis() - previous_loop_start_time); // Dynamically calculate delay time
+  if(delay_time_ms > 0) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS
+    delay(delay_time_ms);
+  }
   //DDYield();
 
 }

src/swerve.cpp

@@ -91,7 +91,7 @@ float calculateSteeringMotorSpeed(float steering_delta) // Calculate the speed o
     }
 }
 
-swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder) // Load new encoder data into the swerve_drive struct, calculate the speed of the steering motors
+swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder) // Process new encoder data, calculate the speed and angle of the steering motors
 {
     swerve_drive out = in;
     // Move values over 1 slot in the encoder data buffer to make room for new data, discard data at the end of the buffer
@@ -132,6 +132,13 @@ swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int fron
     out.front_right_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);
     out.back_left_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);
     out.back_right_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);
+
+    // Calculate the current orientation of each drive wheel in degrees
+    out.front_left_spin_angle = ((float) (front_left_encoder - out.front_left_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
+    out.front_right_spin_angle = ((float) (front_right_encoder - out.front_right_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
+    out.back_left_spin_angle = ((float) (back_left_encoder - out.back_left_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
+    out.back_right_spin_angle = ((float) (back_right_encoder - out.back_right_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
+
     return out;
 }
 

src/swerve.h

@@ -81,7 +81,7 @@ swerve_drive updateSwerveCommand(swerve_drive input); // This function calculate
 
 float calculateSteeringMotorSpeed(float steering_delta); // Calculate the speed of a steering motor based on its distance from its target angle
 
-swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder); // Load new encoder data into the swerve_drive struct, calculate the speed of the steering motors
+swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder); // Process new encoder data, calculate the speed and angle of the steering motors
 
 swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle); // Implementation for translation drive mode