Merge branch 'modulus-plus' of https://git.myitr.org/adeck/rib-test-code into modulus-plus

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

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
@@ -947,7 +948,7 @@ void loop() {
       swrv.enable_steering = true;
       break;
     case DRIVE_TANK:
-      swrv = tankDrive(swrv, zeroed_ly_float, zeroed_lx_float);
+      swrv = tankDrive(swrv, left_joystick_angle, left_joystick_magnitude);
       swrv.enable_steering = false;
       break;
   }
@@ -1145,8 +1146,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
-  if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS
+  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
     delay(delay_time_ms);
   }
   loop_counter_core_0++;
@@ -1166,7 +1168,7 @@ void loop1() {
   }
   else {
     delay(25);
-    loop_counter_core_1++;
+    //loop_counter_core_1++;
   }
 
   //SerComm.println("update");
@@ -1236,5 +1238,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

@@ -354,10 +354,35 @@ swerve_drive tankDrive(swerve_drive input, float target_speed, float turn_angle)
 {
     swerve_drive out = input;
 
-    //float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle
+    float normalized_turn_angle = normalizeAngle(turn_angle); // Normalize the turn angle
 
-    out = setTargetSpin(out, 45.0, 135.0, 225.0, 315.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
+    // 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;
+    }
+
+    out = setMotorCoefficients(out, left_side_power, right_side_power, left_side_power, right_side_power); // Set the motor speed coefficients
     out = setDriveTargetPower(out, target_speed); // Set the power
 
     return out;