From 3cb21d195f55272dd1213644a1cc346078bf2e00 Mon Sep 17 00:00:00 2001 From: evlryah Date: Fri, 29 Sep 2023 15:22:19 -0500 Subject: [PATCH 1/6] Encoder testing, motor identification --- src/globals.h | 9 ++++++--- src/main.cpp | 13 ++++++++++++- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/globals.h b/src/globals.h index dfb3052..bdfeb55 100644 --- a/src/globals.h +++ b/src/globals.h @@ -15,9 +15,12 @@ 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 +// TODO TEMPORARY CHANGED TO 1000, NORMAL 50 +#define LOOP_DELAY_MS_CORE_0 5000 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop +#define LOOP_DELAY_SECONDS_CORE_0 ((float)LOOP_DELAY_MS_CORE_0 / 1000.0f) // Previous number expressed in seconds + #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_CORE_0 / 1000.0f) // Previous number expressed in seconds +#define LOOP_DELAY_SECONDS_CORE_1 ((float)LOOP_DELAY_MS_CORE_1 / 1000.0f) // Previous number expressed in seconds // Math things #define DEGREES_PER_RADIAN (180.0 / M_PI) @@ -86,7 +89,7 @@ extern long last_p; // 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_MIN_UPDATE_LOOPS (TILT_ANGLE_MIN_UPDATE_INTERVAL / LOOP_DELAY_SECONDS_CORE_0) // 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 diff --git a/src/main.cpp b/src/main.cpp index dd5dd2e..2c16ed0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -973,6 +973,7 @@ void loop() { 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 // 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); @@ -981,6 +982,16 @@ void loop() { set_motor(BRDRIVE, swrv.back_right_power); set_motor(FRDRIVE, swrv.front_right_power); set_motor(BLDRIVE, swrv.back_left_power); + */ + for(int i = 0; i < 8; i++) { + if(i == loop_counter_core_0 % 8) { + set_motor(i + 1, 127); + Serial.print("Powering motor"); + Serial.println(i + 1); + } else { + set_motor(i + 1, 0); + } + } // update stepper motors set_motor(LIFTALL, clawarm.arm_set_motor_int); @@ -1145,7 +1156,7 @@ void loop() { 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 + 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++; From 2e95e6afdcf0acb3f0e39d347a4603c4f2bbdb5c Mon Sep 17 00:00:00 2001 From: evlryah Date: Fri, 29 Sep 2023 18:22:46 -0500 Subject: [PATCH 2/6] Motor identification and testing --- src/globals.h | 2 +- src/main.cpp | 20 ++++++++++++-------- src/zserio.cpp | 4 +++- 3 files changed, 16 insertions(+), 10 deletions(-) diff --git a/src/globals.h b/src/globals.h index bdfeb55..f321209 100644 --- a/src/globals.h +++ b/src/globals.h @@ -16,7 +16,7 @@ extern long last_p; // Loop timing // TODO TEMPORARY CHANGED TO 1000, NORMAL 50 -#define LOOP_DELAY_MS_CORE_0 5000 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop +#define LOOP_DELAY_MS_CORE_0 500 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop #define LOOP_DELAY_SECONDS_CORE_0 ((float)LOOP_DELAY_MS_CORE_0 / 1000.0f) // Previous number expressed in seconds #define LOOP_DELAY_MS_CORE_1 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop diff --git a/src/main.cpp b/src/main.cpp index 2c16ed0..bf02f63 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -983,18 +983,22 @@ void loop() { set_motor(FRDRIVE, swrv.front_right_power); set_motor(BLDRIVE, swrv.back_left_power); */ - for(int i = 0; i < 8; i++) { - if(i == loop_counter_core_0 % 8) { - set_motor(i + 1, 127); - Serial.print("Powering motor"); - Serial.println(i + 1); - } else { - set_motor(i + 1, 0); + int loop_motor_change_interval = 8; + if(loop_counter_core_0 % loop_motor_change_interval == 0) { // Only change motors every 20 loops, or 2 seconds + int target_active_motor = ((loop_counter_core_0 / loop_motor_change_interval) % 4) + 5; + + for(int i = 5; i <= 8; i++) { + // 20 for drive motors, 120 for steering motors + set_motor(i, (20 + (100 * (i < 4))) * (i == target_active_motor)); } + + Serial.print("Powering motor"); + Serial.println(target_active_motor); } // update stepper motors - set_motor(LIFTALL, clawarm.arm_set_motor_int); + // TESTING temporarily disabled for motor testing above on 20230929 + //set_motor(LIFTALL, clawarm.arm_set_motor_int); // update servos /* diff --git a/src/zserio.cpp b/src/zserio.cpp index 66769d7..8315073 100644 --- a/src/zserio.cpp +++ b/src/zserio.cpp @@ -81,7 +81,8 @@ void comm_parse() { } } } - + // TESTING: temporarily disabled for motor testing on 20230929 + /* if(millis()-ptime > FAILTIME){ //digitalWrite(13,LOW); SerCommDbg.println("No input recieved, sending safe inputs"); @@ -89,4 +90,5 @@ void comm_parse() { memcpy(astate,&safe,sizeof(packet_t)); comm_ok=0; } + */ } From e601e5a57c6c9e9057c202e7ab75cf68ae29dc2f Mon Sep 17 00:00:00 2001 From: evlryah Date: Fri, 29 Sep 2023 22:45:24 -0500 Subject: [PATCH 3/6] Steering testing --- src/globals.h | 54 ++++--- src/main.cpp | 373 ++++++++++++-------------------------------- src/manipulator.cpp | 25 ++- src/manipulator.h | 2 +- src/spinlock.cpp | 34 ++++ src/spinlock.h | 14 ++ src/swerve.cpp | 43 +++-- src/swerve.h | 4 + src/zserio.cpp | 4 +- 9 files changed, 243 insertions(+), 310 deletions(-) create mode 100644 src/spinlock.cpp create mode 100644 src/spinlock.h diff --git a/src/globals.h b/src/globals.h index f321209..b3a27da 100644 --- a/src/globals.h +++ b/src/globals.h @@ -15,12 +15,9 @@ extern long last_p; */ // Loop timing -// TODO TEMPORARY CHANGED TO 1000, NORMAL 50 -#define LOOP_DELAY_MS_CORE_0 500 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop -#define LOOP_DELAY_SECONDS_CORE_0 ((float)LOOP_DELAY_MS_CORE_0 / 1000.0f) // Previous number expressed in seconds - -#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_CORE_1 ((float)LOOP_DELAY_MS_CORE_1 / 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) @@ -60,14 +57,20 @@ 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 5 + +// 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 (1024.0 * 4.0) / 360.0 // TODO check as of 20230927 +#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 +#define STEERING_MOTOR_SPEED_LIMIT 15.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 +#define STEERING_SLOW_DELTA 5.0 + +// Estimated acceleration delay of steering motors at low speeds (seconds) +#define STEERING_ACCEL_SLOW_DELAY 0.20 // Claw status #define CLAW_UNKNOWN 1 // Position unknown @@ -81,6 +84,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 @@ -88,18 +92,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_CORE_0) // 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 diff --git a/src/main.cpp b/src/main.cpp index bf02f63..a7fdaac 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -15,6 +15,7 @@ #include <107-Arduino-Servo-RP2040.h> #include "swerve.h" #include "manipulator.h" +#include "spinlock.h" const char* ssid = "TEST"; const char* password = "pink4bubble"; @@ -43,9 +44,9 @@ PCF8574 ioex2(0x21, 20, 21); // JANK: soldered to pico or headers PioEncoder enc1(18); // Front Left -PioEncoder enc2(14); // Front Right -PioEncoder enc3(27); // Back Left -PioEncoder enc4(0); // Back Right +PioEncoder enc2(0); // Front Right // TODO WIRING AND CONFIRMATION 20230929 +PioEncoder enc3(27); // Back Left // TODO WIRING AND CONFIRMATION 20230929 +PioEncoder enc4(14); // Back Right @@ -68,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 @@ -112,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 @@ -384,19 +400,25 @@ 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 + + // TESTING DEBUG : commented out for testing + /* 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 == 5) @@ -675,7 +697,7 @@ void setup() { Serial.print("Initializing encoders.."); set_hex(0x5); enc1.begin(); - enc1.flip(); + //enc1.flip(); enc2.begin(); enc3.begin(); enc4.begin(); @@ -859,7 +881,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"; @@ -868,10 +891,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); } @@ -949,56 +973,61 @@ void loop() { swrv.enable_steering = false; break; } - swrv = updateEncoderData(swrv, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount()); // Update encoder data in the swerve_drive struct + swrv = updateEncoderData(swrv, enc1.getCount(), enc3.getCount(), enc4.getCount(), enc4.getCount()); // Update encoder data in the swerve_drive struct swrv = updateSwerveCommand(swrv); // Calculate power for each drive and steering motor + + //DEBUG TESTING code: + Serial.printf("FL spin target %f \t\t at %f\r\n", swrv.front_left_target_spin, normalizeAngle(swrv.front_left_spin_angle)); + //Serial.printf("FR spin target %f \t\t at %f\r\n", swrv.front_right_target_spin, swrv.front_right_spin_angle); + //Serial.printf("BL spin target %f \t\t at %f\r\n", swrv.back_left_target_spin, 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) - float arm_speed = 0.0f; // TODO as of 20230928: PLACEHOLDER for input for arm speed + // 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 = getButton(CLAW_OPEN_BUTTON) - 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 + // TESTING 20230929 comment out to test steering /* - 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); */ - int loop_motor_change_interval = 8; - if(loop_counter_core_0 % loop_motor_change_interval == 0) { // Only change motors every 20 loops, or 2 seconds - int target_active_motor = ((loop_counter_core_0 / loop_motor_change_interval) % 4) + 5; - for(int i = 5; i <= 8; i++) { - // 20 for drive motors, 120 for steering motors - set_motor(i, (20 + (100 * (i < 4))) * (i == target_active_motor)); - } - - Serial.print("Powering motor"); - Serial.println(target_active_motor); - } + // 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; + // TESTING 20230929 comment out since encoders not yet connected + //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 temporarily disabled for motor testing above on 20230929 - //set_motor(LIFTALL, clawarm.arm_set_motor_int); + set_motor(LIFTALL, clawarm.arm_set_motor_int); // update servos /* @@ -1009,158 +1038,9 @@ 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 + 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++; @@ -1168,88 +1048,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 diff --git a/src/manipulator.cpp b/src/manipulator.cpp index fb0b2dc..4b2be96 100644 --- a/src/manipulator.cpp +++ b/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,7 +101,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 out = input; arm_speed = out.arm_speed_coefficient * min(out.arm_speed_limit, max(-out.arm_speed_limit, arm_speed)); diff --git a/src/manipulator.h b/src/manipulator.h index 05314f9..cd50143 100644 --- a/src/manipulator.h +++ b/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 diff --git a/src/spinlock.cpp b/src/spinlock.cpp new file mode 100644 index 0000000..38c22ab --- /dev/null +++ b/src/spinlock.cpp @@ -0,0 +1,34 @@ +#include +#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; +} diff --git a/src/spinlock.h b/src/spinlock.h new file mode 100644 index 0000000..3456422 --- /dev/null +++ b/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 diff --git a/src/swerve.cpp b/src/swerve.cpp index cdcd260..18a3ae3 100644 --- a/src/swerve.cpp +++ b/src/swerve.cpp @@ -62,10 +62,17 @@ swerve_drive updateSwerveCommand(swerve_drive input) 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); + + + // TESTING DEBUG print 20230929 + Serial.printf("FL delta = %f\t\tBR delta = %f\r\n", front_left_delta, back_right_delta); + Serial.printf("FL steer = %f\t\tBR steer = %f\r\n", out.front_left_spin_power, 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; @@ -87,11 +94,12 @@ swerve_drive updateSwerveCommand(swerve_drive input) float calculateSteeringMotorSpeed(float steering_delta) // Calculate the speed of a steering motor based on its distance from its target angle { + float steering_limit_signed = STEERING_MOTOR_SPEED_LIMIT * (steering_delta < 0.0f ? -1.0f : 1.0f); 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; + return steering_limit_signed; } 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)); + return steering_limit_signed * (1.0f - (abs_steering_delta / STEERING_SLOW_DELTA)); } } @@ -146,15 +154,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 +183,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 +193,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 +206,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; diff --git a/src/swerve.h b/src/swerve.h index fbc28fa..9855b29 100644 --- a/src/swerve.h +++ b/src/swerve.h @@ -85,6 +85,10 @@ 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 +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 diff --git a/src/zserio.cpp b/src/zserio.cpp index 8315073..66769d7 100644 --- a/src/zserio.cpp +++ b/src/zserio.cpp @@ -81,8 +81,7 @@ void comm_parse() { } } } - // TESTING: temporarily disabled for motor testing on 20230929 - /* + if(millis()-ptime > FAILTIME){ //digitalWrite(13,LOW); SerCommDbg.println("No input recieved, sending safe inputs"); @@ -90,5 +89,4 @@ void comm_parse() { memcpy(astate,&safe,sizeof(packet_t)); comm_ok=0; } - */ } From 59be703a36c4e67359d6c64c4fc28237946197fd Mon Sep 17 00:00:00 2001 From: Cole Deck Date: Sat, 30 Sep 2023 10:52:31 -0500 Subject: [PATCH 4/6] update --- src/main.cpp | 49 ++++++++++++++++++++++++++------------------- src/manipulator.cpp | 2 +- src/swerve.cpp | 4 ++-- 3 files changed, 31 insertions(+), 24 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index a7fdaac..5245d6c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -44,8 +44,8 @@ PCF8574 ioex2(0x21, 20, 21); // JANK: soldered to pico or headers PioEncoder enc1(18); // Front Left -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 PioEncoder enc4(14); // Back Right @@ -103,8 +103,8 @@ Sabertooth actuators(130, Serial2); #define TALON_PIN_3 7 #define TALON_PIN_4 9 // pins for arm servos -#define ARM_SERVO_PIN_1 2 -#define ARM_SERVO_PIN_2 3 +#define ARM_SERVO_PIN_1 26 +#define ARM_SERVO_PIN_2 27 #define ARM_SERVO_PIN_3 8 static _107_::Servo talon1, talon2, talon3, talon4, arm1, arm2, arm3; @@ -439,7 +439,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(); @@ -451,7 +452,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(); @@ -463,6 +465,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(); @@ -486,6 +490,15 @@ void set_motor(byte motor, int speed) { 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 == 15) @@ -594,19 +607,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); @@ -699,7 +704,9 @@ void setup() { enc1.begin(); //enc1.flip(); enc2.begin(); + //enc2.flip(); enc3.begin(); + //enc3.flip(); enc4.begin(); Serial.println(" done"); delay(20); @@ -973,14 +980,14 @@ void loop() { swrv.enable_steering = false; break; } - swrv = updateEncoderData(swrv, enc1.getCount(), enc3.getCount(), enc4.getCount(), enc4.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 //DEBUG TESTING code: - Serial.printf("FL spin target %f \t\t at %f\r\n", swrv.front_left_target_spin, normalizeAngle(swrv.front_left_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("FR spin target %f \t\t at %f\r\n", swrv.front_right_target_spin, swrv.front_right_spin_angle); //Serial.printf("BL spin target %f \t\t at %f\r\n", swrv.back_left_target_spin, 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)); + Serial.printf("BL spin target %f \t\t at %f\r\n", swrv.back_left_target_spin, normalizeAngle(swrv.back_left_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 @@ -1010,19 +1017,19 @@ void loop() { // update motors after calculation // TESTING 20230929 comment out to test steering - /* + 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; // TESTING 20230929 comment out since encoders not yet connected - //power_data_transfer_fr = swrv.front_right_spin_power; - //power_data_transfer_bl = swrv.back_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); diff --git a/src/manipulator.cpp b/src/manipulator.cpp index 4b2be96..9e7e7b5 100644 --- a/src/manipulator.cpp +++ b/src/manipulator.cpp @@ -106,7 +106,7 @@ manipulator_arm setArmSpeed(manipulator_arm input, float arm_speed) // Set the a 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 diff --git a/src/swerve.cpp b/src/swerve.cpp index 18a3ae3..1d2b2e8 100644 --- a/src/swerve.cpp +++ b/src/swerve.cpp @@ -70,8 +70,8 @@ swerve_drive updateSwerveCommand(swerve_drive input) // TESTING DEBUG print 20230929 - Serial.printf("FL delta = %f\t\tBR delta = %f\r\n", front_left_delta, back_right_delta); - Serial.printf("FL steer = %f\t\tBR steer = %f\r\n", out.front_left_spin_power, out.back_right_spin_power); + Serial.printf("FR delta = %f\t\tBL delta = %f\r\n", front_right_delta, back_left_delta); + Serial.printf("FR steer = %f\t\tBL steer = %f\r\n", out.front_right_spin_power, out.back_left_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; From 0a49ecfa408a2b91068a83199d594d2c3a581b0d Mon Sep 17 00:00:00 2001 From: evlryah Date: Sat, 30 Sep 2023 12:17:36 -0500 Subject: [PATCH 5/6] Working swerve drive --- src/globals.h | 24 +++++++++----------- src/main.cpp | 61 +++++++++++++++++++++++++++++++++++++++----------- src/swerve.cpp | 48 ++++++++++++++++++++++++--------------- src/swerve.h | 2 +- 4 files changed, 90 insertions(+), 45 deletions(-) diff --git a/src/globals.h b/src/globals.h index b3a27da..2ecbc2f 100644 --- a/src/globals.h +++ b/src/globals.h @@ -57,20 +57,18 @@ 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 5 +// 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 60.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 0.5 // 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.12 // Slow approach speed for steering motors +#define STEERING_TOLERANCE_DISABLE_DRIVE 2.0 // Disable the drive motors if any steering motor is off-target by more than this many degrees -// 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 15.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 5.0 - -// Estimated acceleration delay of steering motors at low speeds (seconds) -#define STEERING_ACCEL_SLOW_DELAY 0.20 // Claw status #define CLAW_UNKNOWN 1 // Position unknown diff --git a/src/main.cpp b/src/main.cpp index 5245d6c..41f2731 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -476,27 +476,49 @@ void set_motor(byte motor, int speed) { } else if (motor == 14) { // all steppers together if (abs(speed) > 0) { + + Serial.printf("DEBUG 1F1A: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + ioex1.digitalWrite(2, LOW); // enable stepperX_pos = speed * 96 + stepperX.currentPosition(); + Serial.printf("DEBUG 1F1B: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); stepperX.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); + //stepperZ.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); + //stepperZ.moveTo(stepperX_pos); + + Serial.printf("DEBUG 1F1C: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + stepperX.runState(); stepperY.runState(); - stepperZ.runState(); + //stepperZ.runState(); + + Serial.printf("DEBUG 1F1D: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + } else { + Serial.printf("DEBUG 1F2A: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + ioex1.digitalWrite(2, HIGH); // disable - stepperX_pos = stepperX.currentPosition(); + + Serial.printf("DEBUG 1F2B: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + + // stepperX_pos = stepperX.currentPosition(); + + Serial.printf("DEBUG 1F2C: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + stepperX.setCurrentPosition(stepperX_pos); stepperY.setCurrentPosition(stepperX_pos); - stepperZ.setCurrentPosition(stepperX_pos); - stepperX.stop(); - stepperY.stop(); - stepperZ.stop(); + //stepperZ.setCurrentPosition(stepperX_pos); + + Serial.printf("DEBUG 1F2D: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + + //stepperX.stop(); + //stepperY.stop(); + //stepperZ.stop(); } @@ -983,11 +1005,13 @@ 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 + Serial.printf("DEBUG 1A: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + //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("FR spin target %f \t\t at %f\r\n", swrv.front_right_target_spin, swrv.front_right_spin_angle); - //Serial.printf("BL spin target %f \t\t at %f\r\n", swrv.back_left_target_spin, swrv.back_left_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 @@ -1013,29 +1037,40 @@ void loop() { int new_tilt_command = TILT_COMMAND_UNSET; clawarm = updateTiltCommand(clawarm, new_tilt_command); + Serial.printf("DEBUG 1B: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + 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 + Serial.printf("DEBUG 1C: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + // update motors after calculation - // TESTING 20230929 comment out to test steering - 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); + Serial.printf("DEBUG 1D: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + // 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); + + Serial.printf("DEBUG 1E: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + power_data_transfer_fl = swrv.front_left_spin_power; - // TESTING 20230929 comment out since encoders not yet connected 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); + Serial.printf("DEBUG 1F: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + // update stepper motors + // TESTING: comment out this code to check performance impact set_motor(LIFTALL, clawarm.arm_set_motor_int); + Serial.printf("DEBUG 1G: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + // update servos /* // TODO: Figure out servo mapping diff --git a/src/swerve.cpp b/src/swerve.cpp index 1d2b2e8..8dc8511 100644 --- a/src/swerve.cpp +++ b/src/swerve.cpp @@ -56,22 +56,24 @@ 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) && 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); // TESTING DEBUG print 20230929 - Serial.printf("FR delta = %f\t\tBL delta = %f\r\n", front_right_delta, back_left_delta); - Serial.printf("FR steer = %f\t\tBL steer = %f\r\n", out.front_right_spin_power, out.back_left_spin_power); + 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; @@ -81,25 +83,35 @@ 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; + float max_abs_steering_delta = max(max(fabs(front_left_delta), fabs(front_right_delta)), max(fabs(back_left_delta), fabs(back_right_delta))); + float steering_disable_drive = (float) (max_abs_steering_delta <= STEERING_TOLERANCE_DISABLE_DRIVE); // 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.front_left_power = out.current_drive_power * out.front_left_coefficient * steering_disable_drive * MOTOR_MAX_POWER; + out.front_right_power = out.current_drive_power * out.front_right_coefficient * steering_disable_drive * MOTOR_MAX_POWER; + out.back_left_power = out.current_drive_power * out.back_left_coefficient * steering_disable_drive * MOTOR_MAX_POWER; + out.back_right_power = out.current_drive_power * out.back_right_coefficient * steering_disable_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 steering_limit_signed = STEERING_MOTOR_SPEED_LIMIT * (steering_delta < 0.0f ? -1.0f : 1.0f); 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_limit_signed; + 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_limit_signed * (1.0f - (abs_steering_delta / STEERING_SLOW_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 steering_limit_signed = STEERING_MOTOR_SPEED_LIMIT * (steering_delta < 0.0f ? -1.0f : 1.0f); // Update the sign to account for the future location estimation above + abs_steering_delta = fabs(steering_delta); // Update abs_steering_delta with the new steering_delta + float steering_speed_fraction = powf(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) { // Detect motor stall during approach and increase speed to allow for approach + // steering_speed_fraction += STEERING_SLOW_APPROACH_SPEED; // Commented out until this is necessary + } + return steering_limit_signed * steering_speed_fraction; // Apply the direction } } @@ -242,7 +254,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 diff --git a/src/swerve.h b/src/swerve.h index 9855b29..4818378 100644 --- a/src/swerve.h +++ b/src/swerve.h @@ -81,7 +81,7 @@ 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 From 225af7feadde55c20dc7ef1be234d4fdf68d8b75 Mon Sep 17 00:00:00 2001 From: evlryah Date: Sat, 30 Sep 2023 14:51:09 -0500 Subject: [PATCH 6/6] NIU Competition code final 20230930 --- src/globals.h | 15 +++++++------- src/main.cpp | 56 +++++++++++++------------------------------------- src/swerve.cpp | 46 ++++++++++++++++++++++++++--------------- 3 files changed, 52 insertions(+), 65 deletions(-) diff --git a/src/globals.h b/src/globals.h index 2ecbc2f..eeb0188 100644 --- a/src/globals.h +++ b/src/globals.h @@ -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,20 +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 // 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 60.0 // Maximum speed allowed for the steering motors (out of 127.0) +#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 0.5 // Steering tolerance in degrees +#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.12 // Slow approach speed for steering motors -#define STEERING_TOLERANCE_DISABLE_DRIVE 2.0 // Disable the drive motors if any steering motor is off-target by more than this many degrees - +#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 diff --git a/src/main.cpp b/src/main.cpp index 41f2731..bd31762 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -408,13 +408,11 @@ void set_motor(byte motor, int speed) { // 15 - 17 : arm servos // speed is -127 to 127 - // TESTING DEBUG : commented out for testing - /* Serial.print("Driving motor "); Serial.print(motor); Serial.print(" with speed "); Serial.println(speed); - */ + if (motor <= 4) { // swerve controls @@ -476,45 +474,27 @@ void set_motor(byte motor, int speed) { } else if (motor == 14) { // all steppers together if (abs(speed) > 0) { - - Serial.printf("DEBUG 1F1A: %llu ms\r\n", millis() - previous_loop_start_time_core_0); - ioex1.digitalWrite(2, LOW); // enable stepperX_pos = speed * 96 + stepperX.currentPosition(); - Serial.printf("DEBUG 1F1B: %llu ms\r\n", millis() - previous_loop_start_time_core_0); - stepperX.setMaxSpeed(abs(speed) / 127.0 * defMaxSpeed); stepperX.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); - - Serial.printf("DEBUG 1F1C: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + //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(); - //stepperZ.runState(); - - Serial.printf("DEBUG 1F1D: %llu ms\r\n", millis() - previous_loop_start_time_core_0); - + //stepperY.runState(); + stepperZ.runState(); } else { - Serial.printf("DEBUG 1F2A: %llu ms\r\n", millis() - previous_loop_start_time_core_0); - ioex1.digitalWrite(2, HIGH); // disable - - Serial.printf("DEBUG 1F2B: %llu ms\r\n", millis() - previous_loop_start_time_core_0); // stepperX_pos = stepperX.currentPosition(); - Serial.printf("DEBUG 1F2C: %llu ms\r\n", millis() - previous_loop_start_time_core_0); - stepperX.setCurrentPosition(stepperX_pos); - stepperY.setCurrentPosition(stepperX_pos); - //stepperZ.setCurrentPosition(stepperX_pos); - - Serial.printf("DEBUG 1F2D: %llu ms\r\n", millis() - previous_loop_start_time_core_0); + //stepperY.setCurrentPosition(stepperX_pos); + stepperZ.setCurrentPosition(stepperX_pos); //stepperX.stop(); //stepperY.stop(); @@ -526,7 +506,7 @@ void set_motor(byte motor, int speed) { else if (motor == 15) arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET)); else if (motor == 16) - arm1.writeMicroseconds(map(speed, -127, 127, MIN_MICROS - OC_OFFSET, MAX_MICROS - OC_OFFSET)); + 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)); } @@ -1005,7 +985,6 @@ 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 - Serial.printf("DEBUG 1A: %llu ms\r\n", millis() - previous_loop_start_time_core_0); //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)); @@ -1019,7 +998,7 @@ void loop() { // Claw servo control int new_claw_command = CLAW_COMMAND_UNSET; - int claw_direction = getButton(CLAW_OPEN_BUTTON) - getButton(CLAW_CLOSE_BUTTON); + int claw_direction = ((int) getButton(CLAW_OPEN_BUTTON)) - ((int) getButton(CLAW_CLOSE_BUTTON)); switch(claw_direction) { case 0: new_claw_command = CLAW_COMMAND_STAY; @@ -1037,11 +1016,9 @@ void loop() { int new_tilt_command = TILT_COMMAND_UNSET; clawarm = updateTiltCommand(clawarm, new_tilt_command); - Serial.printf("DEBUG 1B: %llu ms\r\n", millis() - previous_loop_start_time_core_0); 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 - Serial.printf("DEBUG 1C: %llu ms\r\n", millis() - previous_loop_start_time_core_0); // update motors after calculation set_motor(FLDRIVE, swrv.front_left_power); @@ -1050,28 +1027,23 @@ void loop() { set_motor(BLDRIVE, swrv.back_left_power); - Serial.printf("DEBUG 1D: %llu ms\r\n", millis() - previous_loop_start_time_core_0); - // 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); - Serial.printf("DEBUG 1E: %llu ms\r\n", millis() - previous_loop_start_time_core_0); - 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); - Serial.printf("DEBUG 1F: %llu ms\r\n", millis() - previous_loop_start_time_core_0); - // update stepper motors // TESTING: comment out this code to check performance impact set_motor(LIFTALL, clawarm.arm_set_motor_int); - Serial.printf("DEBUG 1G: %llu ms\r\n", millis() - previous_loop_start_time_core_0); - // 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); diff --git a/src/swerve.cpp b/src/swerve.cpp index 8dc8511..b917e07 100644 --- a/src/swerve.cpp +++ b/src/swerve.cpp @@ -54,7 +54,7 @@ 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 // Calculate the distance and direction each motor needs to steer from where it is now @@ -68,12 +68,18 @@ swerve_drive updateSwerveCommand(swerve_drive input) 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; @@ -84,14 +90,12 @@ 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; - float max_abs_steering_delta = max(max(fabs(front_left_delta), fabs(front_right_delta)), max(fabs(back_left_delta), fabs(back_right_delta))); - float steering_disable_drive = (float) (max_abs_steering_delta <= STEERING_TOLERANCE_DISABLE_DRIVE); - // 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 * steering_disable_drive * MOTOR_MAX_POWER; - out.front_right_power = out.current_drive_power * out.front_right_coefficient * steering_disable_drive * MOTOR_MAX_POWER; - out.back_left_power = out.current_drive_power * out.back_left_coefficient * steering_disable_drive * MOTOR_MAX_POWER; - out.back_right_power = out.current_drive_power * out.back_right_coefficient * steering_disable_drive * 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; } @@ -99,19 +103,29 @@ swerve_drive updateSwerveCommand(swerve_drive input) 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 + 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 - 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 steering_limit_signed = STEERING_MOTOR_SPEED_LIMIT * (steering_delta < 0.0f ? -1.0f : 1.0f); // Update the sign to account for the future location estimation above - abs_steering_delta = fabs(steering_delta); // Update abs_steering_delta with the new steering_delta - float steering_speed_fraction = powf(abs_steering_delta / STEERING_SLOW_DELTA, 2.0f); // Fraction of full speed being used + 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) { // Detect motor stall during approach and increase speed to allow for approach - // steering_speed_fraction += STEERING_SLOW_APPROACH_SPEED; // Commented out until this is necessary + 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); + } } - return steering_limit_signed * steering_speed_fraction; // Apply the direction + 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 } }