Organize swerve code, add tank drive mode

src/globals.h

@@ -15,7 +15,8 @@ extern long last_p;
 */
 
 // Loop timing
-#define LOOP_DELAY_MS 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
+#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
 
 // Math things
@@ -31,6 +32,13 @@ extern long last_p;
 #define DRIVE_BASIC         1
 #define DRIVE_TRANSLATION   2
 #define DRIVE_ROTATION      3
+#define DRIVE_TANK          4
+
+// Default drive mode, currently default to stopping upon initialization
+#define DEFAULT_SWERVE_DRIVE_MODE DRIVE_STOP
+
+// Whether to enable swerve drive steering by default (set this to false if the robot is locked to tank drive)
+#define DEFAULT_SWERVE_ENABLE_STEERING true
 
 // Controller maximum inputs for joystick
 #define CONTROLLER_JOYSTICK_MAX 128.0

src/main.cpp

@@ -72,6 +72,7 @@ int stepperY_pos = 0;
 int stepperZ_pos = 0;
 
 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
 
 #define defMaxSpeed     8000
@@ -948,15 +949,23 @@ void loop() {
   switch(loop_drive_mode) {
     case DRIVE_STOP:
       swrv = stopSwerve(swrv);
+      swrv.enable_steering = false;
       break;
     case DRIVE_BASIC:
       swrv = basicDrive(swrv, left_joystick_magnitude, left_joystick_angle);
+      swrv.enable_steering = true;
       break;
     case DRIVE_TRANSLATION:
       swrv = translationDrive(swrv, left_joystick_magnitude, left_joystick_angle);
+      swrv.enable_steering = true;
       break;
     case DRIVE_ROTATION:
       swrv = rotationDrive(swrv, zeroed_rx_float);
+      swrv.enable_steering = true;
+      break;
+    case DRIVE_TANK:
+      swrv = tankDrive(swrv, left_joystick_angle, left_joystick_magnitude);
+      swrv.enable_steering = false;
       break;
   }
   swrv = updateEncoderData(swrv, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount()); // Update encoder data in the swerve_drive struct
@@ -1153,8 +1162,9 @@ void loop() {
   }*/
   //delay(200);
 
-  int64_t delay_time_ms = LOOP_DELAY_MS - (int64_t) (millis() - previous_loop_start_time_core_0); // Dynamically calculate delay time
+  previous_loop_processing_duration_core_0 = millis() - previous_loop_start_time_core_0;
-  if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS
+  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
     delay(delay_time_ms);
   }
   loop_counter_core_0++;
@@ -1174,7 +1184,7 @@ void loop1() {
   }
   else {
     delay(25);
-    loop_counter_core_1++;
+    //loop_counter_core_1++;
   }
 
   //SerComm.println("update");
@@ -1244,5 +1254,11 @@ void loop1() {
   digitalWrite(2, HIGH);
   digitalWrite(3, HIGH);*/
 
-  delay(25);
+  
+  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
+  if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS_CORE_0
+    delay(delay_time_ms);
+  }
+  loop_counter_core_1++;
 }

src/swerve.cpp

@@ -12,6 +12,8 @@ template <typename T> int signum(T val) {
     return (T(0) < val) - (val < T(0));
 }
 
+// Utility functions
+
 float closestAngle(float current, float target) // Calculate closest distance between current and target, set the sign to the fastest direction to go from current to target
 {
         // get direction
@@ -49,13 +51,12 @@ swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encod
 }
 
 // This function calculates the robot's current speed and attempts to modify the current state of the drive towards the target drive state
-// TODO partially complete as of 20230927
 swerve_drive updateSwerveCommand(swerve_drive input)
 {
     swerve_drive out = input;
 
     // Set the new speed of the steering motors
-    if(out.target_drive_power != 0.0f || out.current_drive_power != 0.0f) { // Only set the steering power if the robot is trying to move
+    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);
@@ -65,7 +66,7 @@ swerve_drive updateSwerveCommand(swerve_drive input)
         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);
-    } else { // Stop the steering motors if the robot is stopped and not trying to move
+    } 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;
         out.back_left_spin_power = 0.0f;
@@ -145,6 +146,52 @@ 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
+    angle = fmod(angle, 360);
+    if(angle < 0.0) {
+        angle += 360;
+    }
+    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 out = input;
+    out.front_left_coefficient = front_left;
+    out.front_right_coefficient = front_right;
+    out.back_left_coefficient = back_left;
+    out.back_right_coefficient = back_right;
+    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 out = input;
+    out.front_left_target_spin = front_left + input.spin_offset;
+    out.front_right_target_spin = front_right + input.spin_offset;
+    out.back_left_target_spin = back_left + input.spin_offset;
+    out.back_right_target_spin = back_right + input.spin_offset;
+    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 out = input;
+    
+    float delta_spin_offset = new_spin_offset - input.spin_offset;
+    
+    out.front_left_target_spin = input.front_left_target_spin - delta_spin_offset;
+    out.front_right_target_spin = input.front_left_target_spin - delta_spin_offset;
+    out.back_left_target_spin = input.front_left_target_spin - delta_spin_offset;
+    out.back_right_target_spin = input.front_left_target_spin - delta_spin_offset;
+    return out;
+}
+
+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;
+}
+
+// Drive mode functions
+
 swerve_drive stopSwerve(swerve_drive input) // Stop all motors
 {
     swerve_drive out = input;
@@ -303,51 +350,44 @@ byte identifyBasicDriveCondition(float target_speed, float target_angle) // Iden
     return drive_condition;
 }
 
-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 tankDrive(swerve_drive input, float target_speed, float turn_angle) // Implementation for tank drive mode, positive angle is left, negative angle is right
-    angle = fmod(angle, 360);
+{
-    if(angle < 0.0) {
+    swerve_drive out = input;
-        angle += 360;
+
+    float normalized_turn_angle = normalizeAngle(turn_angle); // Normalize the turn angle
+
+    // Calculate the speed of each motor
+    float angle_distance_from_y_axis = 90.0f - fabs(fmod(normalized_turn_angle, 180.0f) - 90.0f); // Distance in degrees that the joystick is from the y-axis
+    float slow_side_coefficient = angle_distance_from_y_axis / 90.0f; // Coefficient applied to whichever motor is not going full speed in the tank drive
+    int turn_angle_quadrant = ((int) normalized_turn_angle) / 4;
+    float left_side_power, right_side_power;
+    
+    // Determine the motor speeds based on the quadrant that the joystick is in
+    switch(turn_angle_quadrant) {
+        case 0: // Going forward and turning right, Northeast quadrant
+            left_side_power = 1.0f;
+            right_side_power = slow_side_coefficient;
+            break;
+        case 1: // Going backward and turning left, Southeast quadrant 
+            left_side_power = -1.0f;
+            right_side_power = -slow_side_coefficient;
+            break;
+        case 2: // Going backward and turning right, Southwest quadrant
+            left_side_power = -slow_side_coefficient;
+            right_side_power = -1.0f;
+            break;
+        case 3: // Going forward and turning left, Northwest quadrant
+            left_side_power = slow_side_coefficient;
+            right_side_power = 1.0f;
+            break;
     }
-    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
+    out = setMotorCoefficients(out, left_side_power, right_side_power, left_side_power, right_side_power); // Set the motor speed coefficients
-    swerve_drive out = input;
+    out = setDriveTargetPower(out, target_speed); // Set the power
-    out.front_left_coefficient = front_left;
+
-    out.front_right_coefficient = front_right;
-    out.back_left_coefficient = back_left;
-    out.back_right_coefficient = back_right;
     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 out = input;
-    out.front_left_target_spin = front_left + input.spin_offset;
-    out.front_right_target_spin = front_right + input.spin_offset;
-    out.back_left_target_spin = back_left + input.spin_offset;
-    out.back_right_target_spin = back_right + input.spin_offset;
-    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 out = input;
-    
-    float delta_spin_offset = new_spin_offset - input.spin_offset;
-    
-    out.front_left_target_spin = input.front_left_target_spin - delta_spin_offset;
-    out.front_right_target_spin = input.front_left_target_spin - delta_spin_offset;
-    out.back_left_target_spin = input.front_left_target_spin - delta_spin_offset;
-    out.back_right_target_spin = input.front_left_target_spin - delta_spin_offset;
-    return out;
-}
-
-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;
-}
-
-
 
 // Modulus Plus: Drive control planning: v1 on 20230922
 /*

src/swerve.h

@@ -7,7 +7,8 @@ typedef struct { // swerve_drive struct, used to track and manage the state of t
     // byte target_spin_direction = CLOCKWISE; // Target state
     // byte target_drive_direction = DRIVE_FORWARDS; // Target state
     
-    int drive_mode = DRIVE_BASIC;
+    int drive_mode = DEFAULT_SWERVE_DRIVE_MODE;
+    bool enable_steering = DEFAULT_SWERVE_ENABLE_STEERING;
     
     float target_drive_power = 0.0f; // -127.0 to 127.0 : TARGET power that the robot is trying to get to
     float current_drive_power = 0.0f; // -127.0 to 127.0 : CURRENT power being given to drive motors (before coefficient is applied)
@@ -58,7 +59,6 @@ typedef struct { // swerve_drive struct, used to track and manage the state of t
     float back_right_coefficient = 0.0f;
 
     
-    
     // Encoder tracking, used to track speed of the steering motors
     // The 0th entry in the buffer is the most recent, the highest index entry is the oldest
     // The buffer is modified each time updateEncoderData() is called
@@ -70,6 +70,8 @@ typedef struct { // swerve_drive struct, used to track and manage the state of t
     
 } swerve_drive;
 
+// Utility functions
+
 float closestAngle(float current, float target); // Calculate closest distance between current and target, set the sign to the fastest direction to go from current to target
 
 // TODO as of 20230923 for setDirection() : fix to work with modifications made to swerve_drive struct on 20230922
@@ -83,6 +85,19 @@ float calculateSteeringMotorSpeed(float steering_delta); // Calculate the 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
 
+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
+
+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 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 setDriveTargetPower(swerve_drive input, float target_drive_power); // Set a new drive power
+
+
+// Drive mode functions
+
 swerve_drive stopSwerve(swerve_drive input); // Stop all motors
 
 swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle); // Implementation for translation drive mode
@@ -93,12 +108,4 @@ swerve_drive basicDrive(swerve_drive input, float target_speed, float target_ang
 
 byte identifyBasicDriveCondition(float target_speed, float target_angle); // Identify the condition in which the basic drive mode will be operating
 
-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 tankDrive(swerve_drive input, float target_speed, float turn_angle); // Implementation for tank drive mode, positive angle is left, negative angle is right
-
-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 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 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 setDriveTargetPower(swerve_drive input, float target_drive_power); // Set a new drive power