From 4ee423ec9a055eb0cbf7a469e5c5642b9b7d8a16 Mon Sep 17 00:00:00 2001 From: evlryah Date: Thu, 28 Sep 2023 21:40:18 -0500 Subject: [PATCH] Organize swerve code, add tank drive mode --- src/globals.h | 18 +++++-- src/main.cpp | 24 ++++++++-- src/swerve.cpp | 126 ++++++++++++++++++++++++++++++++----------------- src/swerve.h | 29 +++++++----- 4 files changed, 134 insertions(+), 63 deletions(-) diff --git a/src/globals.h b/src/globals.h index 4d63d41..23fb8f2 100644 --- a/src/globals.h +++ b/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 @@ -27,10 +28,17 @@ extern long last_p; #define MOTOR_MAX_POWER 127.0 // Highest value accepted by motor control functions // Drive modes -#define DRIVE_STOP 0 -#define DRIVE_BASIC 1 -#define DRIVE_TRANSLATION 2 -#define DRIVE_ROTATION 3 +#define DRIVE_STOP 0 +#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 diff --git a/src/main.cpp b/src/main.cpp index d0dfc11..3a05736 100644 --- a/src/main.cpp +++ b/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 - 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++; @@ -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++; } diff --git a/src/swerve.cpp b/src/swerve.cpp index 6d07381..cdcd260 100644 --- a/src/swerve.cpp +++ b/src/swerve.cpp @@ -12,6 +12,8 @@ template 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 - angle = fmod(angle, 360); - if(angle < 0.0) { - angle += 360; +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 out = input; + + 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 - 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; + 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; } -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 /* diff --git a/src/swerve.h b/src/swerve.h index 4a608e3..fbc28fa 100644 --- a/src/swerve.h +++ b/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 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 +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