Merge branch 'modulus-plus-dev'

.vscode/settings.json

@@ -4,6 +4,7 @@
         "cmath": "cpp",
         "*.tcc": "cpp",
         "string": "cpp",
-        "ranges": "cpp"
+        "ranges": "cpp",
+        "numeric": "cpp"
     }
 }

src/globals.h

@@ -58,6 +58,19 @@ extern long last_p;
 // 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 3
 
+<<<<<<< HEAD
+
+// Number of encoder ticks per full rotation of each swerve drive steering motor
+#define STEERING_ENCODER_TICKS_PER_ROTATION     (1024.0 * 8.0)
+// Number of encoder ticks per degree of rotation for the swerve drive steering motors
+#define STEERING_ENCODER_TICKS_PER_DEGREE       (STEERING_ENCODER_TICKS_PER_ROTATION / 360.0)
+
+// 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
@@ -70,6 +83,7 @@ extern long last_p;
 #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
+>>>>>>> modulus-plus-dev
 
 // Claw status
 #define CLAW_UNKNOWN        1   // Position unknown

src/main.cpp

@@ -44,8 +44,13 @@ PCF8574 ioex2(0x21, 20, 21);
 
 // JANK: soldered to pico or headers
 PioEncoder enc1(18); // Front Left
+<<<<<<< HEAD
+PioEncoder enc2(0); // Front Right // TODO WIRING AND CONFIRMATION 20230929
+PioEncoder enc3(27); // Back Left // TODO WIRING AND CONFIRMATION 20230929
+=======
 PioEncoder enc2(0); // Front Right
 PioEncoder enc3(2); // Back Left
+>>>>>>> modulus-plus-dev
 PioEncoder enc4(14);  // Back Right
 
 
@@ -998,7 +1003,11 @@ void loop() {
 
   // Claw servo control
   int new_claw_command = CLAW_COMMAND_UNSET;
+<<<<<<< HEAD
+  int claw_direction = getButton(CLAW_OPEN_BUTTON) - getButton(CLAW_CLOSE_BUTTON);
+=======
   int claw_direction = ((int) getButton(CLAW_OPEN_BUTTON)) - ((int) getButton(CLAW_CLOSE_BUTTON));
+>>>>>>> modulus-plus-dev
   switch(claw_direction) {
     case 0:
       new_claw_command = CLAW_COMMAND_STAY;
@@ -1016,9 +1025,13 @@ void loop() {
   int new_tilt_command = TILT_COMMAND_UNSET;
   clawarm = updateTiltCommand(clawarm, new_tilt_command);
 
+<<<<<<< HEAD
+  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
+=======
 
   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
 
+>>>>>>> modulus-plus-dev
 
   // update motors after calculation
   set_motor(FLDRIVE, swrv.front_left_power);
@@ -1036,6 +1049,14 @@ void loop() {
   power_data_transfer_br = swrv.back_right_spin_power;
   spinlock_release(&drive_power_command_spinlock_flag);
 
+  // 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_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);
@@ -1072,6 +1093,20 @@ void drive_control_core_1() { // Control drive motors from core 1 from loop1() f
   spinlock_release(&drive_power_command_spinlock_flag); // Release the spinlock
 
   // Set motors if the requested power is different than the previously requested power
+<<<<<<< HEAD
+  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);
+  }
+=======
   //if(local_fl != power_data_transfer_prev_fl) {
     set_motor(FLSTEER, local_fl);
   //}
@@ -1084,6 +1119,7 @@ void drive_control_core_1() { // Control drive motors from core 1 from loop1() f
   //if(local_br != power_data_transfer_prev_br) {
     set_motor(BRSTEER, local_br);
   //}
+>>>>>>> modulus-plus-dev
 
   // 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;