NIU Competition code final 20230930

platformio.ini

@@ -13,7 +13,7 @@ platform = https://github.com/maxgerhardt/platform-raspberrypi.git
 board = rpipicow
 framework = arduino
 platform_packages = 
-	framework-arduinopico@https://github.com/earlephilhower/arduino-pico.git#master ; use master branch for setXXX fix
+	framework-arduinopico@https://github.com/earlephilhower/arduino-pico.git#master
 upload_port = /mnt/pico
 board_build.core = earlephilhower
 lib_deps = 
@@ -23,6 +23,7 @@ lib_deps =
 	ftjuh/I2Cwrapper@^0.5.0
 	waspinator/AccelStepper@^1.64
 	olikraus/Ucglib@^1.5.2
-	khoih-prog/RP2040_ISR_Servo@^1.1.2
+	107-systems/107-Arduino-Servo-RP2040@^0.1.0
 debug_tool = cmsis-dap
-build_flags = -O3 ; optimize build level 3
+build_flags = -O3
+; upload_protocol = cmsis-dap

src/globals.h

@@ -15,9 +15,9 @@ extern long last_p;
 */
 
 // Loop timing
-#define LOOP_DELAY_MS_CORE_0 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
-#define LOOP_DELAY_MS_CORE_1 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
-#define LOOP_DELAY_SECONDS ((float)LOOP_DELAY_MS / 1000.0f) // Previous number expressed in seconds
+#define LOOP_DELAY_MS_CORE_0    50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
+#define LOOP_DELAY_MS_CORE_1    20 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
+#define LOOP_DELAY_SECONDS ((float)LOOP_DELAY_MS_CORE_0 / 1000.0f) // Previous number expressed in seconds
 
 // Math things
 #define DEGREES_PER_RADIAN (180.0 / M_PI)
@@ -25,7 +25,8 @@ extern long last_p;
 #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
+#define MOTOR_MAX_POWER         127.0   // Highest value accepted by motor control functions
+#define DRIVE_MOTOR_MAX_POWER   64.0    // Maximum power for drive motors 
 
 // Drive modes
 #define DRIVE_STOP          0
@@ -55,16 +56,20 @@ extern long last_p;
 // Length of the buffer to monitor recent steering 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 steering motors' current speeds
 // This value must always be at least 2, otherwise the code will break due to there being an array with a zero or negative length or a division by zero
-#define ENCODER_BUFFER_ENTRY_COUNT 5
+#define ENCODER_BUFFER_ENTRY_COUNT 3
 
-// Number of encoder ticks per degree of rotation for the swerve drive steering motors
-#define STEERING_ENCODER_TICKS_PER_DEGREE (1024.0 * 4.0) / 360.0 // TODO check as of 20230927
-
-// Maximum speed allowed for the steering motors (out of 127.0)
-#define STEERING_MOTOR_SPEED_LIMIT 127.0 // TODO as of 20230927, lower this if they're spinning too fast for the robot to handle
-
-// Start decelerating the steering motors linearly when they're within this many degrees of their target angle
-#define STEERING_SLOW_DELTA 30.0
+// Steering parameters
+#define STEERING_ENCODER_TICKS_PER_ROTATION     (1024.0 * 8.0)                                  // Number of encoder ticks per full rotation of each swerve drive steering motor
+#define STEERING_ENCODER_TICKS_PER_DEGREE       (STEERING_ENCODER_TICKS_PER_ROTATION / 360.0)   // Number of encoder ticks per degree of rotation for the swerve drive steering motors
+#define STEERING_MOTOR_SPEED_LIMIT 80.0                                                         // Maximum speed allowed for the steering motors (out of 127.0)
+// Steering PID parameters
+#define STEERING_SLOW_DELTA 35.0                                                                // Start decelerating the steering motors linearly when they're within this many degrees of their target angle
+#define STEERING_ACCEL_SLOW_DELAY 0.20                                                          // Estimated acceleration delay of steering motors at low speeds (seconds)
+#define STEERING_TOLERANCE 1.0                                                                  // Steering tolerance in degrees
+#define STEERING_STALL_DETECT_ANGULAR_SPEED 5.0                                                 // Detect steering motor stall if measured angular speed is below this
+#define STEERING_SLOW_APPROACH_SPEED (0.16 * (MOTOR_MAX_POWER / STEERING_MOTOR_SPEED_LIMIT))    // Slow approach speed for steering motors
+#define STEERING_TOLERANCE_DISABLE_DRIVE 30.0                                                    // Disable the drive motors if any steering motor is off-target by more than this many degrees
+#define STEERING_HOVER_RANGE 10.0                                                               // Angular range where steering motors tend to hover around their targets
 
 // Claw status
 #define CLAW_UNKNOWN        1   // Position unknown
@@ -78,6 +83,7 @@ extern long last_p;
 // #define CLAW_COMMAND_STOP   1   // Stop immediately, no matter the location
 #define CLAW_COMMAND_CLOSE  2   // Close the claw
 #define CLAW_COMMAND_OPEN   3   // Open the claw
+#define CLAW_COMMAND_STAY   4   // Maintain the current position
 
 // Claw things
 #define CLAW_OPEN_ANGLE 90.0f       // Open position of the claw
@@ -85,18 +91,32 @@ extern long last_p;
 #define CLAW_DEFAULT_ANGLE 0.0f     // Default starting claw position
 
 // Tilt servo control parameters
-#define TILT_ANGLE_MIN_UPDATE_INTERVAL 0.2f // Update the tilt servo's target angle only after this many seconds have elapsed since the previous angle update
-#define TILT_ANGLE_MIN_UPDATE_LOOPS (TILT_ANGLE_MIN_UPDATE_INTERVAL / LOOP_DELAY_SECONDS) // Previous value expressed as a number of control loops to wait between updates
-#define TILT_ANGLE_UPDATE_DISTANCE 10.0f // Distance in degrees to shift the servo angle by each update
-#define TILT_MAX_ANGLE 90.0f // Maximum angle allowed for the tilt servo
-#define TILT_MIN_ANGLE -90.0f // Minimum angle allowed for the tilt servo
-#define TILT_FLAT_ANGLE 0.0f // Default/flat/starting angle for the tilt servo
+#define TILT_DEGREES_PER_SECOND         50.0f // Speed in degrees per second that the tilt motor will move up or down (except when resetting to a flat position)
+#define TILT_ANGLE_MIN_UPDATE_INTERVAL  0.2f // Update the tilt servo's target angle only after this many seconds have elapsed since the previous angle update
+#define TILT_ANGLE_MIN_UPDATE_LOOPS     (TILT_ANGLE_MIN_UPDATE_INTERVAL / LOOP_DELAY_SECONDS) // Previous value expressed as a number of control loops to wait between updates
+#define TILT_ANGLE_UPDATE_DISTANCE      (TILT_DEGREES_PER_SECOND * TILT_ANGLE_MIN_UPDATE_INTERVAL) // Distance in degrees to shift the servo angle by each update
+#define TILT_MAX_ANGLE                  90.0f // Maximum angle allowed for the tilt servo
+#define TILT_MIN_ANGLE                  -90.0f // Minimum angle allowed for the tilt servo
+#define TILT_FLAT_ANGLE                 0.0f // Default/flat/starting angle for the tilt servo
 
 // Tilt servo commands
-#define TILT_COMMAND_UNSET 0
-#define TILT_COMMAND_UP 1
-#define TILT_COMMAND_DOWN 2
+#define TILT_COMMAND_UNSET  0   // Command not yet set, go to default (flat) position
+#define TILT_COMMAND_RESET  1   // Reset the tilt servo to the flat position
+#define TILT_COMMAND_UP     2   // Raise the tilt servo
+#define TILT_COMMAND_DOWN   3   // Lower the silt servo
 
+// Control parameters, specify which buttons control new_angle actions
+#define ARM_UP_BUTTON       DPAD_UP
+#define ARM_DOWN_BUTTON     DPAD_DOWN
+#define CLAW_OPEN_BUTTON    DPAD_LEFT
+#define CLAW_CLOSE_BUTTON   DPAD_RIGHT
+#define TILT_UP_BUTTON      DIAMOND_UP
+#define TILT_DOWN_BUTTON    DIAMOND_DOWN
+#define TILT_RESET_BUTTON   DIAMOND_LEFT
+
+
+
+// Button definitions
 #define SerComm Serial1   //Serial port connected to Xbee
 #define DIAMOND_LEFT 0
 #define DIAMOND_DOWN 1

src/main.cpp

@@ -6,15 +6,16 @@
 #include <SPI.h>
 #include "MCP3XXX.h"
 #include "PCF8574.h"
-#include "RP2040_ISR_Servo.h"
 #include "pio_encoder.h"
 //#include "dumbdisplay.h"
 //#include "wifidumbdisplay.h"
 #include "zserio.h"
 #include "SerialUART.h"
 #include <Sabertooth.h>
+#include <107-Arduino-Servo-RP2040.h>
 #include "swerve.h"
 #include "manipulator.h"
+#include "spinlock.h"
 
 const char* ssid = "TEST";
 const char* password = "pink4bubble";
@@ -43,14 +44,11 @@ PCF8574 ioex2(0x21, 20, 21);
 
 // JANK: soldered to pico or headers
 PioEncoder enc1(18); // Front Left
-PioEncoder enc2(14); // Front Right
-PioEncoder enc3(26); // Back Left
-PioEncoder enc4(0);  // Back Right
+PioEncoder enc2(0); // Front Right
+PioEncoder enc3(2); // Back Left
+PioEncoder enc4(14);  // Back Right
+
 
-// pins for arm servos
-#define ARM_SERVO_PIN_1 2
-#define ARM_SERVO_PIN_2 3
-#define ARM_SERVO_PIN_3 8
 
 // stepper board slave pins
 #define MOTOR_X_ENABLE_PIN 8
@@ -71,10 +69,25 @@ int stepperX_pos = 0;
 int stepperY_pos = 0;
 int stepperZ_pos = 0;
 
+// Loop management
 uint64_t previous_loop_start_time_core_0 = 0, previous_loop_start_time_core_1 = 0; // Track the previous loop start time, used to calculate how long to sleep for until the start of the next loop
 uint64_t previous_loop_processing_duration_core_0 = 0, previous_loop_processing_duration_core_1 = 0; // Processing time for the previous loop on each core
 uint64_t loop_counter_core_0 = 0, loop_counter_core_1 = 0; // Counter for loop() and loop1() respectively, incremented at the end of each loop
 
+// Motor power communication between cores
+byte drive_power_command_spinlock_flag = SPINLOCK_UNSET; // Spinlock tracking flag
+// Variables used to transfer steering motor power between cores, covered by spinlock
+int power_data_transfer_fl = 0;
+int power_data_transfer_fr = 0;
+int power_data_transfer_bl = 0;
+int power_data_transfer_br = 0;
+// Variables used to track previous requested motor power, only used by core 1, sabertooth not called if motor power unchanged since previous loop
+int power_data_transfer_prev_fl = 0;
+int power_data_transfer_prev_fr = 0;
+int power_data_transfer_prev_bl = 0;
+int power_data_transfer_prev_br = 0;
+
+
 #define defMaxSpeed     8000
 #define defAcceleration 8000
 
@@ -89,20 +102,12 @@ Sabertooth actuators(130, Serial2);
 #define TALON_PIN_2       6
 #define TALON_PIN_3       7
 #define TALON_PIN_4       9
+// pins for arm servos
+#define ARM_SERVO_PIN_1   26
+#define ARM_SERVO_PIN_2   27
+#define ARM_SERVO_PIN_3   8
 
-typedef struct
-{
-  int     servoIndex;
-  uint8_t servoPin;
-} ISR_servo_t;
-
-
-#define NUM_SERVOS            7
-
-ISR_servo_t ISR_servo[NUM_SERVOS] =
-{
-  { -1, TALON_PIN_1 }, { -1, TALON_PIN_2 }, { -1, TALON_PIN_3 }, { -1, TALON_PIN_4 }, { -1, ARM_SERVO_PIN_1 }, { -1, ARM_SERVO_PIN_2 }, { -1, ARM_SERVO_PIN_3 }
-};
+static _107_::Servo talon1, talon2, talon3, talon4, arm1, arm2, arm3;
 
 MCP3008 adc;
 int count = 0;
@@ -123,14 +128,14 @@ int right_cooldown = 0;
 int olddisplay = 99999; // guarantee a change when first value comes in
 
 // motor indeces for set_motor() function
-#define FLSTEER 1
-#define FRSTEER 2
-#define BLSTEER 3
-#define BRSTEER 4
+#define FRSTEER 1
+#define FLSTEER 2
+#define BRSTEER 3
+#define BLSTEER 4
 #define FLDRIVE 5
-#define FRDRIVE 6
+#define BRDRIVE 6
 #define BLDRIVE 7
-#define BRDRIVE 8
+#define FRDRIVE 8
 #define EXTEND1 9
 #define EXTEND2 10
 #define LIFT1 11
@@ -395,30 +400,33 @@ void set_dec(byte num) {
 }
 
 void set_motor(byte motor, int speed) {
-  // 1 - 4   : swivel motors on Sabertooth 1-2
-  // 5 - 8   : drive motors on Talon SRX
+  // 1 - 4   : swivel motors on Sabertooth 1-2 (Clockwise: FR, FL, BR, BL)
+  // 5 - 8   : drive motors on Talon SRX (Forward: FL, BR, BL, FL)
   // 9 - 10  : actuators on Sabertooth 3
   // 11 - 13 : Steppers on slave board
   // 14      : drive 11-13 with identical position & speed
   // 15 - 17 : arm servos
   // speed is -127 to 127
+  
   Serial.print("Driving motor ");
   Serial.print(motor);
   Serial.print(" with speed ");
   Serial.println(speed);
+  
+
   if (motor <= 4) {
     // swerve controls
+    speed *= (((motor % 2) * 2) - 1); // Flip motors 2 and 4
     swivel[(motor - 1) / 2].motor((motor - 1) % 2 + 1, speed);
   }
-  else if (motor <= 8) {
-    // Talon SRX drive
-    // Note: can't use the built in arduino-pico Servo.h because it uses the PIO - which is already full
-    // Can't use PWM because that would slow the PWM frequency to 50hz, kinda need more than that
-    // So we use hardware interrupt timers instead
-    //RP2040_ISR_Servos.setPosition(ISR_servo[motor - 5].servoIndex, position);
-    uint16_t newspeed = map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET);
-    RP2040_ISR_Servos.setPulseWidth(ISR_servo[motor - 5].servoIndex, newspeed);
-  }
+  else if (motor == 5)
+    talon1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+  else if (motor == 6)
+    talon2.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+  else if (motor == 7)
+    talon3.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+  else if (motor == 8)
+    talon4.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
   else if (motor <= 10) {
     // actuator controls
     actuators.motor((motor - 9 + 1), speed);
@@ -429,7 +437,8 @@ void set_motor(byte motor, int speed) {
     //stepperX.setSpeed((float)speed);
     if (abs(speed) > 0) 
       ioex1.digitalWrite(2, LOW); // enable
-      
+    else 
+      stepperX_pos = stepperX.currentPosition();
     
     stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
     stepperX_pos = speed * 96 + stepperX.currentPosition();
@@ -441,7 +450,8 @@ void set_motor(byte motor, int speed) {
     //stepperY.setSpeed((float)speed);
     if (abs(speed) > 0) 
       ioex1.digitalWrite(2, LOW); // enable
-      
+    else 
+      stepperY_pos = stepperY.currentPosition();
 
     stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
     stepperY_pos = speed * 96 + stepperY.currentPosition();
@@ -453,6 +463,8 @@ void set_motor(byte motor, int speed) {
     //stepperY.setSpeed((float)speed);
     if (abs(speed) > 0) 
       ioex1.digitalWrite(2, LOW); // enable
+    else 
+      stepperZ_pos = stepperZ.currentPosition();
       
     stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
     stepperZ_pos = speed * 96 + stepperZ.currentPosition();
@@ -467,21 +479,36 @@ void set_motor(byte motor, int speed) {
 
       stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
       stepperX.moveTo(stepperX_pos);
-      stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
-      stepperY.moveTo(stepperX_pos);
+      //stepperY.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
+      //stepperY.moveTo(stepperX_pos);
       stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed);
       stepperZ.moveTo(stepperX_pos);
+
       stepperX.runState();
-      stepperY.runState();
+      //stepperY.runState();
       stepperZ.runState();
     } else {
       ioex1.digitalWrite(2, HIGH); // disable
+
+      // stepperX_pos = stepperX.currentPosition();
+
+      stepperX.setCurrentPosition(stepperX_pos);
+      //stepperY.setCurrentPosition(stepperX_pos);
+      stepperZ.setCurrentPosition(stepperX_pos);
+
+      //stepperX.stop();
+      //stepperY.stop();
+      //stepperZ.stop();
+      
+      
     }
   }
-  else if (motor < 18) { // arm servos
-    uint16_t newspeed = map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET);
-    RP2040_ISR_Servos.setPulseWidth(ISR_servo[motor - 15 + 4].servoIndex, newspeed);
-  }
+  else if (motor == 15)
+    arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+  else if (motor == 16)
+    arm2.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
+  else if (motor == 17)
+    arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET));
 }
 
 void setup() {
@@ -582,19 +609,11 @@ void setup() {
   ioex1.digitalWrite(3, HIGH);
   delay(2000);
 
-  digitalWrite(ALI1, LOW);
-  digitalWrite(BLI1, LOW);
-  digitalWrite(AHI1, LOW);
-  digitalWrite(BHI1, LOW);
   digitalWrite(ALI2, LOW);
   digitalWrite(BLI2, LOW);
   digitalWrite(AHI2, LOW);
   digitalWrite(BHI2, LOW);
   
-  pinMode(ALI1, OUTPUT);
-  pinMode(AHI1, OUTPUT);
-  pinMode(BLI1, OUTPUT);
-  pinMode(BHI1, OUTPUT);
   pinMode(ALI2, OUTPUT);
   pinMode(AHI2, OUTPUT);
   pinMode(BLI2, OUTPUT);
@@ -660,32 +679,22 @@ void setup() {
   actuators.setTimeout(100);
   
 
-  // Talon SRX init
-  for (int index = 0; index < NUM_SERVOS; index++)
-  {
-    digitalWrite(ISR_servo[index].servoPin, LOW);
-    pinMode(ISR_servo[index].servoPin, OUTPUT);
-    digitalWrite(ISR_servo[index].servoPin, LOW);
-  }
-
-  for (int index = 0; index < NUM_SERVOS; index++)
-  {
-    ISR_servo[index].servoIndex = RP2040_ISR_Servos.setupServo(ISR_servo[index].servoPin, MIN_MICROS, MAX_MICROS);
-
-    if (ISR_servo[index].servoIndex != -1)
-    {
-      //Serial.print(F("Setup OK Servo index = ")); Serial.println(ISR_servo[index].servoIndex);
-
-      RP2040_ISR_Servos.setPosition(ISR_servo[index].servoIndex, 0);
-    }
-    else
-    {
-      Serial.print(F("Setup Failed Servo index = ")); Serial.println(ISR_servo[index].servoIndex);
-    }
-  }
-  for (int i = 5; i < 9; i++)
+  // Servo & Talon SRX init
+  // talon0, talon1, talon2, talon3, arm1, arm2, arm3
+  talon1.attach(TALON_PIN_1);
+  talon2.attach(TALON_PIN_2);
+  talon3.attach(TALON_PIN_3);
+  talon4.attach(TALON_PIN_4);
+    for (int i = 5; i < 9; i++)
     set_motor(i, 0);
 
+  arm1.attach(ARM_SERVO_PIN_1);
+  arm2.attach(ARM_SERVO_PIN_2);
+  arm3.attach(ARM_SERVO_PIN_3);
+  for (int i = 15; i < 18; i++)
+    set_motor(i, 0);
+  
+
   Serial.println(" done");
   delay(20);
 
@@ -694,10 +703,13 @@ void setup() {
   // Initialize encoders
   Serial.print("Initializing encoders..");
   set_hex(0x5);
-  //enc1.begin();
-  //enc2.begin();
-  //enc3.begin();
-  //enc4.begin();
+  enc1.begin();
+  //enc1.flip();
+  enc2.begin();
+  //enc2.flip();
+  enc3.begin();
+  //enc3.flip();
+  enc4.begin();
   Serial.println(" done");
   delay(20);
 
@@ -878,7 +890,8 @@ void print_status() {
 }
 
 void telemetry(float zeroed_lx_float, float zeroed_ly_float, float zeroed_rx_float, float zeroed_ry_float, int drive_mode, \
-               float left_joystick_angle, float left_joystick_magnitude, float right_joystick_angle, float right_joystick_magnitude) { // Print encoder positions for steering motors
+               float left_joystick_angle, float left_joystick_magnitude, float right_joystick_angle, float right_joystick_magnitude, \
+               uint64_t processing_time) { // Print encoder positions for steering motors
   
   char buffer[300] = "\0";
 
@@ -887,10 +900,11 @@ void telemetry(float zeroed_lx_float, float zeroed_ly_float, float zeroed_rx_flo
   //SerComm.println(buffer);
 
   // Encoder data and loop number
-  sprintf(buffer, "Loop = %llu  E1 = %i  E2 = %i  E3 = %i  E4 = %i  z_lx = %f  z_ly = %f  z_rx = %f  z_ry = %f  mode = %i  mag-L = %f  ang-L = %f mag-R = %f  ang-R = %f", \
+  sprintf(buffer, "Loop = %llu  E1 = %i  E2 = %i  E3 = %i  E4 = %i  z_lx = %f  z_ly = %f  z_rx = %f  z_ry = %f  mode = %i  mag-L = %f  ang-L = %f mag-R = %f  ang-R = %f  proctime = %llu", \
   loop_counter_core_0, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount(), \
   zeroed_lx_float, zeroed_ly_float, zeroed_rx_float, zeroed_ry_float, drive_mode, \
-  left_joystick_magnitude, left_joystick_angle, right_joystick_magnitude, right_joystick_angle);
+  left_joystick_magnitude, left_joystick_angle, right_joystick_magnitude, right_joystick_angle, \
+  processing_time);
   Serial.println(buffer);
   SerComm.println(buffer);
 }
@@ -971,40 +985,65 @@ void loop() {
   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
 
-  // Arm motor control (stepper motors)
-  float arm_speed = 0.0f; // TODO as of 20230928: PLACEHOLDER for input for arm speed 
+
+  //DEBUG TESTING code:
+  Serial.printf("FL spin target %f \t\t at %f\r\n", swrv.front_left_target_spin, normalizeAngle(swrv.front_right_spin_angle));
+  Serial.printf("FR spin target %f \t\t at %f\r\n", swrv.front_right_target_spin, normalizeAngle(swrv.front_right_spin_angle));
+  Serial.printf("BL spin target %f \t\t at %f\r\n", swrv.back_left_target_spin, normalizeAngle(swrv.back_left_spin_angle));
+  Serial.printf("BR spin target %f \t\t at %f\r\n", swrv.back_right_target_spin, normalizeAngle(swrv.back_right_spin_angle));
+  
+  // Arm motor control (stepper motors), DPAD_UP to move arm up, DPAD_DOWN to move arm down, both or neither being pressed stops the arm
+  float arm_speed = (float) (((int) getButton(DPAD_UP)) - ((int) getButton(DPAD_DOWN))); // TODO 20230929 confirm speed and polarity
   clawarm = setArmSpeed(clawarm, arm_speed);
 
   // Claw servo control
   int new_claw_command = CLAW_COMMAND_UNSET;
-  /*
-    // TODO select action for claw
-  */
+  int claw_direction = ((int) getButton(CLAW_OPEN_BUTTON)) - ((int) getButton(CLAW_CLOSE_BUTTON));
+  switch(claw_direction) {
+    case 0:
+      new_claw_command = CLAW_COMMAND_STAY;
+      break;
+    case 1:
+      new_claw_command = CLAW_COMMAND_OPEN;
+      break;
+    case -1:
+      new_claw_command = CLAW_COMMAND_CLOSE;
+      break;
+  }
   clawarm = updateClawCommand(clawarm, new_claw_command);
 
   // Tilt servo control
   int new_tilt_command = TILT_COMMAND_UNSET;
-  /*
-    // TODO select action for the tilt servo
-  */
   clawarm = updateTiltCommand(clawarm, new_tilt_command);
 
-  telemetry(zeroed_lx_float, zeroed_ly_float, zeroed_rx_float, zeroed_ry_float, loop_drive_mode, left_joystick_angle, left_joystick_magnitude, right_joystick_angle, right_joystick_magnitude); // DEBUG ONLY, print steering motor encoder positions
+
+  telemetry(zeroed_lx_float, zeroed_ly_float, zeroed_rx_float, zeroed_ry_float, loop_drive_mode, left_joystick_angle, left_joystick_magnitude, right_joystick_angle, right_joystick_magnitude, previous_loop_processing_duration_core_0); // DEBUG ONLY, telemetry
+
 
   // update motors after calculation
-  set_motor(FLSTEER, swrv.front_left_spin_power);
-  set_motor(BRSTEER, swrv.back_right_spin_power);
-  set_motor(FRSTEER, swrv.front_right_spin_power);
-  set_motor(BLSTEER, swrv.back_left_spin_power);
   set_motor(FLDRIVE, swrv.front_left_power);
   set_motor(BRDRIVE, swrv.back_right_power);
   set_motor(FRDRIVE, swrv.front_right_power);
   set_motor(BLDRIVE, swrv.back_left_power);
   
+
+  // Lock the spinlock and transfer the steering motor data to core 1, which will send the data to the sabertooth motor controllers
+  spinlock_lock_core_0(&drive_power_command_spinlock_flag);
+
+  power_data_transfer_fl = swrv.front_left_spin_power;
+  power_data_transfer_fr = swrv.front_right_spin_power;
+  power_data_transfer_bl = swrv.back_left_spin_power;
+  power_data_transfer_br = swrv.back_right_spin_power;
+  spinlock_release(&drive_power_command_spinlock_flag);
+
   // update stepper motors
+  // TESTING: comment out this code to check performance impact
   set_motor(LIFTALL, clawarm.arm_set_motor_int);
 
   // update servos
+  Serial.printf("claw set motor int %i\r\n", clawarm.claw_set_motor_int);
+  set_motor(ARMSERVO1, clawarm.claw_set_motor_int);
+  set_motor(ARMSERVO2, - clawarm.claw_set_motor_int);
   /*
   // TODO: Figure out servo mapping
   set_motor(SERVOTILT, clawarm.tilt_set_motor_int);
@@ -1013,155 +1052,6 @@ void loop() {
   */
 
 
-  /////////////////////////////////////////////////////////////
-  // END OF MODULUS PLUS CODE UNTIL THE END OF THIS FUNCTION //
-  /////////////////////////////////////////////////////////////
-
-  // Goliath / 2 side arcade tank drive code below
-  /*int zeroed_power =    -1 * ((int)(astate->stickX) - 127);
-  int zeroed_turn =      ((int)(astate->stickY) - 127);
-  
-  
-  if (true) { //fb != NULL) {
-    //int x = fb->x - 127;
-    //int y = - fb->y + 127;
-    int x = zeroed_turn;
-    int y = zeroed_power;
-    //Serial.print(x);
-    //Serial.print(" ");
-    //Serial.println(y);
-    
-    double rawdriveangle = atan2(x, y);
-    double driveangle = rawdriveangle * 180 / 3.1415926;
-    target_left_power = y;
-    target_right_power = y;
-    
-    target_left_power += x;
-    target_right_power += -x;
-    target_left_power = constrain(target_left_power, -127, 127);
-    target_right_power = constrain(target_right_power, -127, 127);
-    if(turbo) {
-      target_left_power *= 2;
-      target_right_power *= 2;
-    }
-    target_left_power = target_left_power * 0.675;
-    target_right_power = target_right_power * 0.675;
-
-
-  
-
-  }
-  
-  if(turbo)
-    acceleration = 8;
-  else
-    acceleration = 3;
-
-  if(left_cooldown > 0)
-    left_cooldown --;
-
-  if(abs(target_left_power) <= 4 && abs(left_power) > 5) {
-    left_power = 0;
-    left_cooldown = 2;
-  }
-  else if(target_left_power >= left_power + acceleration && left_cooldown == 0)
-    left_power += acceleration;
-  else if(acceleration > target_left_power - left_power && left_cooldown == 0)
-    left_power = target_left_power;
-  else if(target_left_power <= left_power - acceleration && left_cooldown == 0)
-    left_power -= acceleration;
-  else if(acceleration > left_power - target_left_power && left_cooldown == 0)
-    left_power = target_left_power;
-
-  if(right_cooldown > 0)
-    right_cooldown --;
-
-  if(abs(target_right_power) <= 4 && abs(right_power) > 5) {
-    right_power = 0;
-    right_cooldown = 2;
-  }
-  else if(target_right_power >= right_power + acceleration && right_cooldown == 0)
-    right_power += acceleration;
-  else if(acceleration > target_right_power - right_power && right_cooldown == 0)
-    right_power = target_right_power;
-  else if(target_right_power <= right_power - acceleration && right_cooldown == 0)
-    right_power -= acceleration;
-  else if(acceleration > right_power - target_right_power && right_cooldown == 0)
-    right_power = target_right_power;
-
-  int avg_speed = (abs(right_power) + abs(left_power))/2;
-  //SerComm.println();
-  set_hex(avg_speed);
-
-  //drive_right(right_enabled, right_power);
-  //drive_left(left_enabled, -left_power);
-  SerComm.println(" L: " + String(left_power) + " LT: " + String(target_left_power) + " R: " + String(right_power) + " RT: " + String(target_right_power) + " MEM FREE: "+ String(rp2040.getFreeHeap()));
-  */
-  //if(left_power != target_left_power || right_power != target_right_power)
-
-  
-  
-  //delay(1000);
-  //set_digit(0, 6);
-  //set_digit(0, 10);
-  //set_digit(1, 9);
-  //set_digit(1, 10);
-  //set_digit(2, 8);
-  //set_digit(2, 10);
-  //set_digit(3, 8);
-  //set_digit(3, 10);
-  //set_digit(4, 8);
-  //set_digit(4, 10);
-  /*if (mode == 0) {
-    set_raw(count / 8, count % 8);
-    if (count < 39) {
-      count ++;
-    } else {
-      count = 0;
-      mode = 1;
-      delay(100);
-    }
-  }*/
-  //print_status();
-  //drive_right(right_enabled, 10);
-  //drive_left(left_enabled, 10);
-  /*if (millis() % 3000 > 1500) {
-    set_mosfet(0, LOW);
-    set_mosfet(1, LOW);
-    //ioex2.digitalWrite(7, LOW);
-  }
-  if (millis() % 3000 < 1500) {
-    set_mosfet(0, HIGH);
-    set_mosfet(1, HIGH);
-    //ioex2.digitalWrite(7, HIGH);
-
-  }*/
-  /*if (mode == 1) {
-    set_dec(count);
-    drive_right(right_enabled, count);
-    //set_hex(count);
-    if (count < 40) {
-      count += 5;
-    } else {
-      //count = 0;
-
-      mode = 2;
-    }
-  }
-  if (mode == 2) {
-    set_dec(count);
-    drive_right(right_enabled, count);
-    //set_hex(count);
-    if (count > 5) {
-      count -= 5;
-    } else {
-      //count = 0;
-      
-      mode = 1;
-    }
-  }*/
-  //delay(200);
-
   previous_loop_processing_duration_core_0 = millis() - previous_loop_start_time_core_0;
   int64_t delay_time_ms = LOOP_DELAY_MS_CORE_0 - (int64_t) previous_loop_processing_duration_core_0; // Dynamically calculate delay time
   if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS_CORE_0
@@ -1172,88 +1062,41 @@ void loop() {
 
 }
 
+void drive_control_core_1() { // Control drive motors from core 1 from loop1() function
+  // Lock the steering motor power data to read it
+  spinlock_lock_core_1(&drive_power_command_spinlock_flag);
+  int local_fl = power_data_transfer_fl;
+  int local_fr = power_data_transfer_fr;
+  int local_bl = power_data_transfer_bl;
+  int local_br = power_data_transfer_br;
+  spinlock_release(&drive_power_command_spinlock_flag); // Release the spinlock
+
+  // Set motors if the requested power is different than the previously requested power
+  //if(local_fl != power_data_transfer_prev_fl) {
+    set_motor(FLSTEER, local_fl);
+  //}
+  //if(local_fr != power_data_transfer_prev_fr) {
+    set_motor(FRSTEER, local_fr);
+  //}
+  //if(local_bl != power_data_transfer_prev_bl) {
+    set_motor(BLSTEER, local_bl);
+  //}
+  //if(local_br != power_data_transfer_prev_br) {
+    set_motor(BRSTEER, local_br);
+  //}
+
+  // Set the previously requested power data to the current power data, will be read in the next loop
+  power_data_transfer_prev_fl = local_fl;
+  power_data_transfer_prev_fr = local_fr;
+  power_data_transfer_prev_bl = local_bl;
+  power_data_transfer_prev_br = local_br;
+}
+
 void loop1() {
   previous_loop_start_time_core_1 = millis();
   rp2040.wdt_reset();
-  //drive_left(left_enabled, 255);
-  //digitalWrite(LED_BUILTIN, HIGH);  
-  if(loop_counter_core_1 == 20) {
-    //print_status();
-    loop_counter_core_1 = 0;
-    delay(25);
-  }
-  else {
-    delay(25);
-    //loop_counter_core_1++;
-  }
-
-  //SerComm.println("update");
-  //left_enabled = try_enable_left(left_enabled, get_voltage(1));
-  //right_enabled = try_enable_right(right_enabled, get_voltage(0));
-  //digitalWrite(LED_BUILTIN, LOW);
-
-  /*if (stepperX.distanceToGo() == 0) { // Give stepper something to do...
-    if (stepperXdir) {
-      Serial.println("Driving stepper");
-      stepperX.moveTo(stepsToGo);
-    } else {
-      Serial.println("Driving stepper");
-      stepperX.moveTo(-stepsToGo);
-    }
-    stepperX.runState();  
-    stepperXdir = !stepperXdir;
-  }
-  if (stepperY.distanceToGo() == 0) { // Give stepper something to do...
-    if (stepperYdir)
-      stepperY.moveTo(stepsToGo);
-    else
-      stepperY.moveTo(-stepsToGo);    
-    stepperY.runState();  
-    stepperYdir = !stepperYdir;
-  }
-
-  if (stepperZ.distanceToGo() == 0) { // Give stepper something to do...
-    if (stepperZdir)
-      stepperZ.moveTo(stepsToGo);
-    else
-      stepperZ.moveTo(-stepsToGo);
-    stepperZ.runState(); 
-    stepperZdir = !stepperZdir;
-  }*/
-  if (mode == 1) {
-    //set_hex(count);
-    if (count < 125) {
-      count += 1;
-    } else {
-      //count = 0;
-
-      mode = 2;
-    }
-  }
-  if (mode == 2) {
-
-    //set_hex(count);
-    if (count > -125) {
-      count -= 1;
-    } else {
-      //count = 0;
-      
-      mode = 1;
-    }
-  }
-  /*for (int x = 5; x < 9; x++) {
-    set_motor(x, count);  
-  }
-  swivel[0].motor(0, 127);
-  swivel[0].motor(1, 127);
-  swivel[1].motor(0, 127);
-  swivel[1].motor(1, 127);
-  set_motor(14, count); // drive all steppers in sync
-  digitalWrite(0, HIGH);
-  digitalWrite(1, HIGH);
-  digitalWrite(2, HIGH);
-  digitalWrite(3, HIGH);*/
 
+  drive_control_core_1();
   
   previous_loop_processing_duration_core_1 = millis() - previous_loop_start_time_core_1;
   int64_t delay_time_ms = LOOP_DELAY_MS_CORE_1 - (int64_t) previous_loop_processing_duration_core_1; // Dynamically calculate delay time

src/manipulator.cpp

@@ -39,22 +39,28 @@ manipulator_arm setTiltAngle(manipulator_arm input, float new_tilt_angle) // Int
 manipulator_arm updateTiltCommand(manipulator_arm input, int new_tilt_command) // Update the command for the tilt motor
 {
     manipulator_arm out = input;
-    if(new_tilt_command != TILT_COMMAND_UNSET && out.tilt_angle_loops_since_update >= TILT_ANGLE_MIN_UPDATE_LOOPS) { // Ignore value of 0, make sure that its been long enough since the last update
-        float tilt_angle_offset_direction = 0.0f;
+    if(new_tilt_command != out.tilt_command || out.tilt_angle_loops_since_update >= TILT_ANGLE_MIN_UPDATE_LOOPS) { // Make sure that it's been long enough since the last update to not spam the servo command
+        float new_angle = out.tilt_target_angle; // Maintain the existing angle by default
         switch(new_tilt_command) {
+            case TILT_COMMAND_UNSET:
+                new_angle = out.tilt_target_angle; // Maintain the existing angle
+            case TILT_COMMAND_RESET:
+                new_angle = TILT_FLAT_ANGLE;
             case TILT_COMMAND_UP:
-                tilt_angle_offset_direction = 1.0f;
+                new_angle = out.tilt_target_angle + TILT_ANGLE_UPDATE_DISTANCE;
                 break;
             case TILT_COMMAND_DOWN:
-                tilt_angle_offset_direction = -1.0f;
+                new_angle = out.tilt_target_angle - TILT_ANGLE_UPDATE_DISTANCE;
                 break;
         }
-        float angle_offset = TILT_ANGLE_UPDATE_DISTANCE * tilt_angle_offset_direction; // Degrees that the target angle is being changed by
-        out = setTiltAngle(out, out.tilt_target_angle + angle_offset);
+        out = setTiltAngle(out, new_angle);
+        out.tilt_angle_loops_since_update = 0; // Reset the counter tracking loops since the last update, since an update was just performed
     } else { // Increment the number of loops since the previous update, since an update was not performed during this loop
         out.tilt_angle_loops_since_update++;
     }
 
+    out.tilt_command = new_tilt_command; // Save the new command to check whether it has changed in the next loop
+
     return out;
 }
 
@@ -81,6 +87,9 @@ manipulator_arm updateClawCommand(manipulator_arm input, int new_claw_command) /
         case CLAW_COMMAND_CLOSE:
             new_claw_angle = CLAW_CLOSED_ANGLE;
             break;
+        case CLAW_COMMAND_STAY:
+            new_claw_angle = out.claw_position;
+            break;
         default:
             new_claw_angle = CLAW_DEFAULT_ANGLE;
     }
@@ -92,12 +101,12 @@ manipulator_arm updateClawCommand(manipulator_arm input, int new_claw_command) /
 
 // Arm functions (stepper motors)
 
-manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed) // Set the arm's speed
+manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed) // Set the arm's speed, must be between -1.0 and 1.0
 {
     manipulator_arm out = input;
     arm_speed = out.arm_speed_coefficient * min(out.arm_speed_limit, max(-out.arm_speed_limit, arm_speed));
     out.arm_speed = arm_speed;
-    out.arm_set_motor_int = min(127, max(-127, (int) (127.0f * arm_speed)));
+    out.arm_set_motor_int = min(127, max(-127, (int) (127.0f * arm_speed))) * 2;
     return out;
 }
 manipulator_arm setArmSpeedLimit(manipulator_arm input, float arm_speed_limit) // Set the arm's speed limit (applied before coefficient), limit must be between 0.0 and 1.0

src/manipulator.h

@@ -51,7 +51,7 @@ manipulator_arm updateClawCommand(manipulator_arm input, int new_claw_command);
 
 // Arm functions (stepper motors)
 
-manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed); // Set the arm's speed
+manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed); // Set the arm's speed, must be between -1.0 and 1.0
 
 manipulator_arm setArmSpeedLimit(manipulator_arm input, float arm_speed_limit); // Set the arm's speed limit (applied before coefficient), limit must be between 0.0 and 1.0
 

src/spinlock.cpp

@@ -0,0 +1,34 @@
+#include <Arduino.h>
+#include "globals.h"
+#include "spinlock.h"
+
+void spinlock_lock_core_0(byte* spinlock_flag) // Lock a spinlock from core 0
+{
+    while(*spinlock_flag == SPINLOCK_LOCK_CORE_1) {} // Wait until the data is not locked by the other core
+    *spinlock_flag = SPINLOCK_LOCK_CORE_0; // Try setting the flag to lock the data
+    // Wait to see if the memory was overwritten by the other core
+    delay(1);
+    if(*spinlock_flag == SPINLOCK_LOCK_CORE_1 || *spinlock_flag == SPINLOCK_OPEN) { // The other core locked the data (and currently holds a lock or has released it), recurse to try again
+        spinlock_lock_core_0(spinlock_flag);
+        return;
+    }
+    return;
+}
+
+void spinlock_lock_core_1(byte* spinlock_flag) // Lock a spinlock from core 1
+{
+    while(*spinlock_flag == SPINLOCK_LOCK_CORE_1) {} // Wait until the data is not locked by the other core
+    *spinlock_flag = SPINLOCK_LOCK_CORE_0; // Try setting the flag to lock the data
+    // Wait to see if the memory was overwritten by the other core
+    delay(1);
+    if(*spinlock_flag == SPINLOCK_LOCK_CORE_1 || *spinlock_flag == SPINLOCK_OPEN) { // The other core locked the data (and currently holds a lock or has released it), recurse to try again
+        spinlock_lock_core_0(spinlock_flag);
+        return;
+    }
+    return;
+}
+
+void spinlock_release(byte* spinlock_flag) // Release a spinlock
+{
+    *spinlock_flag = SPINLOCK_OPEN;
+}

src/spinlock.h

@@ -0,0 +1,14 @@
+#include "globals.h"
+
+
+// Spinlock flags
+#define SPINLOCK_UNSET          0
+#define SPINLOCK_OPEN           1
+#define SPINLOCK_LOCK_CORE_0    2
+#define SPINLOCK_LOCK_CORE_1    3
+
+void spinlock_lock_core_0(byte* spinlock_flag); // Lock a spinlock from core 0
+
+void spinlock_lock_core_1(byte* spinlock_flag); // Lock a spinlock from core 1
+
+void spinlock_release(byte* spinlock_flag); // Release a spinlock

src/swerve.cpp

@@ -54,18 +54,33 @@ swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encod
 swerve_drive updateSwerveCommand(swerve_drive input)
 {
     swerve_drive out = input;
-
+    float new_drive_coefficient = out.target_drive_power;
     // Set the new speed of the steering motors
-    if((out.target_drive_power != 0.0f || out.current_drive_power != 0.0f) && out.enable_steering) { // Only set the steering power if the robot is trying to move, and if steering is enabled
+    if(/*(out.target_drive_power != 0.0f || out.current_drive_power != 0.0f) &&*/ out.enable_steering) { // Only set the steering power if the robot is trying to move, and if steering is enabled
         // Calculate the distance and direction each motor needs to steer from where it is now
         float front_left_delta = closestAngle(out.front_left_spin_angle, out.front_left_target_spin);
         float front_right_delta = closestAngle(out.front_right_spin_angle, out.front_right_target_spin);
         float back_left_delta = closestAngle(out.back_left_spin_angle, out.back_left_target_spin);
         float back_right_delta = closestAngle(out.back_right_spin_angle, out.back_right_target_spin);
-        out.front_left_spin_power = calculateSteeringMotorSpeed(front_left_delta);
-        out.front_right_spin_power = calculateSteeringMotorSpeed(front_right_delta);
-        out.back_left_spin_power = calculateSteeringMotorSpeed(back_left_delta);
-        out.back_right_spin_power = calculateSteeringMotorSpeed(back_right_delta);
+        // Use the delta and speed of each steering motor to calculate the necessary speed
+        out.front_left_spin_power = calculateSteeringMotorSpeed(front_left_delta, out.front_left_measured_spin_speed);
+        out.front_right_spin_power = calculateSteeringMotorSpeed(front_right_delta, out.front_right_measured_spin_speed);
+        out.back_left_spin_power = calculateSteeringMotorSpeed(back_left_delta, out.back_left_measured_spin_speed);
+        out.back_right_spin_power = calculateSteeringMotorSpeed(back_right_delta, out.back_right_measured_spin_speed);
+
+        float max_abs_steering_delta = max(max(fabs(front_left_delta), fabs(front_right_delta)), max(fabs(back_left_delta), fabs(back_right_delta)));
+        if (max_abs_steering_delta > STEERING_TOLERANCE_DISABLE_DRIVE) {
+            new_drive_coefficient = 0;
+        }
+        Serial.printf("max_abs_steering_delta = %f\t\tndc = %f\r\n", max_abs_steering_delta, new_drive_coefficient);
+
+        // TESTING DEBUG print 20230929
+        Serial.printf("FL delta = %f\t\tFL steer = %f\r\n", front_left_delta, out.front_left_spin_power);
+        Serial.printf("FR delta = %f\t\tFR steer = %f\r\n", front_right_delta, out.front_right_spin_power);
+        Serial.printf("BL delta = %f\t\tBL steer = %f\r\n", back_left_delta, out.back_left_spin_power);
+        Serial.printf("BR delta = %f\t\tBR steer = %f\r\n", back_right_delta, out.back_right_spin_power);
+        
+
     } else { // Stop the steering motors if the robot is stopped and not trying to move, or if steering is disabled
         out.front_left_spin_power = 0.0f;
         out.front_right_spin_power = 0.0f;
@@ -74,24 +89,43 @@ swerve_drive updateSwerveCommand(swerve_drive input)
     }
 
     // Set the current drive power to the target drive power, TODO: this is TEMPORARY, add in something to slow the current (set) speed until the wheels are in the correct direction
-    out.current_drive_power = out.target_drive_power;  
-
+    //out.current_drive_power = out.target_drive_power;
     // Set the new drive motor power, apply coefficients, set between -127.0 and 127.0
-    out.front_left_power = out.current_drive_power * out.front_left_coefficient * MOTOR_MAX_POWER;
-    out.front_right_power = out.current_drive_power * out.front_right_coefficient * MOTOR_MAX_POWER;
-    out.back_left_power = out.current_drive_power * out.back_left_coefficient * MOTOR_MAX_POWER;
-    out.back_right_power = out.current_drive_power * out.back_right_coefficient * MOTOR_MAX_POWER;
+    out.current_drive_power = new_drive_coefficient;
+    out.front_left_power = new_drive_coefficient * out.front_left_coefficient * DRIVE_MOTOR_MAX_POWER;
+    out.front_right_power = new_drive_coefficient * out.front_right_coefficient * DRIVE_MOTOR_MAX_POWER;
+    out.back_left_power = new_drive_coefficient * out.back_left_coefficient * DRIVE_MOTOR_MAX_POWER;
+    out.back_right_power = new_drive_coefficient * out.back_right_coefficient * DRIVE_MOTOR_MAX_POWER;
     
     return out;
 }
 
-float calculateSteeringMotorSpeed(float steering_delta) // Calculate the speed of a steering motor based on its distance from its target angle
+float calculateSteeringMotorSpeed(float steering_delta, float current_angular_speed) // Calculate the speed of a steering motor based on its distance from its target angle and its current angular speed
 {
     float abs_steering_delta = fabs(steering_delta);
-    if(abs_steering_delta > STEERING_SLOW_DELTA) { // In full speed range, still far enough away from the target angle
-        return STEERING_MOTOR_SPEED_LIMIT;
+    if(abs_steering_delta > STEERING_SLOW_DELTA && abs_steering_delta > STEERING_TOLERANCE) { // In full speed range, still far enough away from the target angle
+        return STEERING_MOTOR_SPEED_LIMIT * (steering_delta < 0.0f ? -1.0f : 1.0f);
     } else { // Slow down the speed of the steering motor since it's close to its target angle
-        return STEERING_MOTOR_SPEED_LIMIT * (1.0f - (abs_steering_delta / STEERING_SLOW_DELTA));
+
+        float calc_steering_delta = steering_delta + (STEERING_ACCEL_SLOW_DELAY * current_angular_speed); // Modify the steering delta to the estimated delta in STEERING_ACCEL_SLOW_DELAY seconds to account for motor acceleration
+        float calc_steering_limit_signed = STEERING_MOTOR_SPEED_LIMIT * (calc_steering_delta < 0.0f ? -1.0f : 1.0f); // Update the sign to account for the future location estimation above
+        float calc_abs_steering_delta = fabs(calc_steering_delta); // Update abs_steering_delta with the new steering_delta
+        float steering_speed_fraction = powf(calc_abs_steering_delta / STEERING_SLOW_DELTA, 2.0f); // Fraction of full speed being used
+        //return steering_limit_signed * (1.0f - (abs_steering_delta / STEERING_SLOW_DELTA));
+        if(current_angular_speed < STEERING_STALL_DETECT_ANGULAR_SPEED || steering_speed_fraction < STEERING_SLOW_APPROACH_SPEED) { // Detect motor stall during approach and increase speed to allow for approach
+            steering_speed_fraction = STEERING_SLOW_APPROACH_SPEED;
+            
+            if(calc_abs_steering_delta < STEERING_HOVER_RANGE) { // Decrease speed further if the steering is extremely close to the target
+                steering_speed_fraction *= (calc_abs_steering_delta / STEERING_HOVER_RANGE);
+            } else if(abs_steering_delta < STEERING_HOVER_RANGE) {
+                steering_speed_fraction *= (abs_steering_delta / STEERING_HOVER_RANGE);
+            }
+        }
+        if(calc_abs_steering_delta < STEERING_TOLERANCE) { // Stop the steering motors if they are within the tolerance range
+            return 0.0f;
+        }
+
+        return calc_steering_limit_signed * steering_speed_fraction; // Apply the direction
     }
 }
 
@@ -146,15 +180,27 @@ swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int fron
     return out;
 }
 
-float normalizeAngle(float angle) { // Takes an input angle and normalizes it to an angle between 0.0 and 360.0 degrees, results excluding exactly 360.0 degrees
+int normalizeEncoderData(int encoder_data) // Normalize encoder data to a range of [0, STEERING_ENCODER_TICKS_PER_ROTATION)
+{
+    encoder_data %= (int) STEERING_ENCODER_TICKS_PER_ROTATION;
+    encoder_data += (STEERING_ENCODER_TICKS_PER_ROTATION * (encoder_data < 0));
+    return encoder_data;
+}
+
+float relativeAngleFromEncoder(int encoder_data_relative ) // Calculate the relative angle from the difference between 2 encoder data points
+{
+    return ((float) normalizeEncoderData(encoder_data_relative)) / ((float) STEERING_ENCODER_TICKS_PER_DEGREE);
+}
+
+float normalizeAngle(float angle) // Takes an input angle and normalizes it to an angle between 0.0 and 360.0 degrees, results excluding exactly 360.0 degrees
+{
     angle = fmod(angle, 360);
-    if(angle < 0.0) {
-        angle += 360;
-    }
+    angle += (360 * (angle < 0.0));
     return angle;
 }
 
-swerve_drive setMotorCoefficients(swerve_drive input, float front_left, float front_right, float back_left, float back_right) { // Set the motor speed coefficients for each motor
+swerve_drive setMotorCoefficients(swerve_drive input, float front_left, float front_right, float back_left, float back_right) // Set the motor speed coefficients for each motor
+{
     swerve_drive out = input;
     out.front_left_coefficient = front_left;
     out.front_right_coefficient = front_right;
@@ -163,7 +209,8 @@ swerve_drive setMotorCoefficients(swerve_drive input, float front_left, float fr
     return out;
 }
 
-swerve_drive setTargetSpin(swerve_drive input, float front_left, float front_right, float back_left, float back_right) { // Set the target spin for each wheel
+swerve_drive setTargetSpin(swerve_drive input, float front_left, float front_right, float back_left, float back_right) // Set the target spin for each wheel
+{
     swerve_drive out = input;
     out.front_left_target_spin = front_left + input.spin_offset;
     out.front_right_target_spin = front_right + input.spin_offset;
@@ -172,7 +219,8 @@ swerve_drive setTargetSpin(swerve_drive input, float front_left, float front_rig
     return out;
 }
 
-swerve_drive setSpinOffset(swerve_drive input, float new_spin_offset) { // Set a new spin offset, and maintain the current target spin on each motor relative to the robot body as the offset is changed
+swerve_drive setSpinOffset(swerve_drive input, float new_spin_offset) // Set a new spin offset, and maintain the current target spin on each motor relative to the robot body as the offset is changed
+{
     swerve_drive out = input;
     
     float delta_spin_offset = new_spin_offset - input.spin_offset;
@@ -184,7 +232,8 @@ swerve_drive setSpinOffset(swerve_drive input, float new_spin_offset) { // Set a
     return out;
 }
 
-swerve_drive setDriveTargetPower(swerve_drive input, float target_drive_power) { // Set a new drive power
+swerve_drive setDriveTargetPower(swerve_drive input, float target_drive_power) // Set a new drive power
+{
     swerve_drive out = input;
     out.target_drive_power = target_drive_power;
     return out;
@@ -219,7 +268,7 @@ swerve_drive rotationDrive(swerve_drive input, float target_speed) // Implementa
 
     //float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle
 
-    out = setTargetSpin(out, 45.0, 135.0, 225.0, 315.0); // Set the target angle for each rotation motor
+    out = setTargetSpin(out, 45.0, 135.0, 315.0, 225.0); // Set the target angle for each rotation motor
     out = setMotorCoefficients(out, 1.0, 1.0, 1.0, 1.0); // Set the motor speed coefficients to 1 for all motors
     out = setDriveTargetPower(out, target_speed); // Set the power
 

src/swerve.h

@@ -81,10 +81,14 @@ swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encod
 
 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
 
-float calculateSteeringMotorSpeed(float steering_delta); // Calculate the speed of a steering motor based on its distance from its target angle
+float calculateSteeringMotorSpeed(float steering_delta, float current_angular_speed); // Calculate the speed of a steering motor based on its distance from its target angle and its current angular speed
 
 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
 
+int normalizeEncoderData(int encoder_data); // Normalize encoder data to a range of [0, STEERING_ENCODER_TICKS_PER_ROTATION)
+
+float relativeAngleFromEncoder(int encoder_data_relative ); // Calculate the relative angle from the difference between 2 encoder data points
+
 float normalizeAngle(float angle); // Takes an input angle and normalizes it to an angle between 0.0 and 360.0 degrees, results excluding exactly 360.0 degrees
 
 swerve_drive setMotorCoefficients(swerve_drive input, float front_left, float front_right, float back_left, float back_right); // Set the motor speed coefficients for each motor