From cd79a133f13733ed6c2418e59425af114271b477 Mon Sep 17 00:00:00 2001 From: evlryah Date: Sat, 23 Sep 2023 04:46:19 -0500 Subject: [PATCH] Basic 3-mode swerve drive system calculations --- src/globals.h | 19 +++++++--- src/swerve.cpp | 101 +++++++++++++++++++++++++++++++++++++++++++------ src/swerve.h | 15 +++++++- 3 files changed, 116 insertions(+), 19 deletions(-) diff --git a/src/globals.h b/src/globals.h index 8a6c25a..158de67 100644 --- a/src/globals.h +++ b/src/globals.h @@ -1,5 +1,7 @@ #pragma once #include "packet.h" +#include +#define _USE_MATH_DEFINES // To access M_PI constant for trig functions extern packet_t pA, pB, safe; extern packet_t *astate, *incoming; @@ -12,6 +14,10 @@ extern long last_p; #define ANTICLOCKWISE 1 */ +// Math things +#define DEGREES_PER_RADIAN (180.0 / M_PI) +#define RADIANS_PER_DEGREE (M_PI / 180.0) + // Drive modes #define DRIVE_BASIC 0 #define DRIVE_TRANSLATION 1 @@ -27,15 +33,14 @@ extern long last_p; #define DRIVE_BASIC_BACKRIGHT 6 typedef struct { - byte drive_mode = DRIVE_BASIC; // byte spin_direction = CLOCKWISE; // Current state, 0 = clockwise, 1 = anticlockwise // byte drive_direction = DRIVE_FORWARDS; // Current state, Directions: 0 = forwards, 1 = right, 2 = backwards, 3 = left - - //byte target_spin_direction = CLOCKWISE; // Target state - //byte target_drive_direction = DRIVE_FORWARDS; // Target state + // byte target_spin_direction = CLOCKWISE; // Target state + // byte target_drive_direction = DRIVE_FORWARDS; // Target state - float target_drive_power = 127.0; // 0 - 127 - float current_drive_power = 127.0; // 0 - 127 + byte drive_mode = DRIVE_BASIC; + float target_drive_power = 127.0; // 0.0 - 127.0 + float current_drive_power = 127.0; // 0.0 - 127.0 float spin_location = 0; // 0 - 360, CURRENT tracked body orientation relative to initial (or reset) orientation float spin_offset = 0; // 0 - 360, offset applied to target spin when setting @@ -45,11 +50,13 @@ typedef struct { float front_right_current_spin = 0; float back_left_current_spin = 0; float back_right_current_spin = 0; + // TARGET wheel orientations relative to robot body float front_left_target_spin = 0; float front_right_target_spin = 0; float back_left_target_spin = 0; float back_right_target_spin = 0; + // Motor power coefficients, this is used when motors must turn at different speeds. This is an input value, and is not directly affected by the current robot conditions // Between 0 and 1 float front_left_coefficient = 0; diff --git a/src/swerve.cpp b/src/swerve.cpp index 9542c52..3888afa 100644 --- a/src/swerve.cpp +++ b/src/swerve.cpp @@ -1,10 +1,13 @@ #include #include "globals.h" +#include template int signum(T val) { return (T(0) < val) - (val < T(0)); } -// based on https://compendium.readthedocs.io/en/latest/tasks/drivetrains/swerve.html + +// 3-mode drive control system based on https://compendium.readthedocs.io/en/latest/tasks/drivetrains/swerve.html + float closestAngle(float current, float target) { while(current > 360) { @@ -27,6 +30,8 @@ float closestAngle(float current, float target) } +/* +// TODO as of 20230923 for setDirection() : fix to work with modifications made to swerve_drive struct on 20230922 swerve_wheel setDirection(swerve_wheel input, float setpoint) { swerve_wheel out = input; @@ -51,16 +56,41 @@ swerve_wheel setDirection(swerve_wheel input, float setpoint) } return out; } +*/ swerve_wheel tempTranslationDrive(swerve_wheel input, float target_speed, float target_angle) // Temporary implementation for translation drive mode { swerve_wheel out = input; - out = setTargetSpin(out, target_angle, target_angle, target_angle, target_angle); // Set the target angle for each rotation motor + + float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle + + out = setTargetSpin(out, normalized_target_angle, normalized_target_angle, normalized_target_angle, normalized_target_angle); // 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 return out; } +/* + TODO for tempRotationDrive() as of 20230923: add functionality to track the robot body's rotation relative to the field through an IMU or encoders somehow, + and allow the option to modify the spin offset to compensate for it. +*/ +swerve_wheel tempRotationDrive(swerve_wheel input, float target_speed) // Temporary implementation for rotation drive mode (rotating in place), positive speed is clockwise, negative speed is counterclockwise +{ + swerve_wheel out = input; + + 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 = 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 + + return out; +} + +/* + TODO for tempBasicDrive() as of 20230923: add functionality to track the robot body's rotation relative to the field through an IMU or encoders somehow, + and allow the option to modify the spin offset to compensate for it. +*/ swerve_wheel tempBasicDrive(swerve_wheel input, float target_speed, float target_angle) // Temporary implementation for basic drive mode { swerve_wheel out = input; @@ -69,7 +99,7 @@ swerve_wheel tempBasicDrive(swerve_wheel input, float target_speed, float target byte drive_condition = identifyBasicDriveCondition(target_speed, normalized_target_angle); // Identify the drive condition // Find inner_difference_angle - // Measure from the target angle to the nearest y-axis, this is for the inner wheels in the turn (wheels closer to the center of the turning circle) + // Measure from the target angle to the nearest section of the y-axis (like on a unit circle), this is for the inner wheels in the turn (wheels closer to the center of the turning circle) float inner_difference_angle; float semicircle_normalized_target_angle = normalized_target_angle % 180.0; if(semicircle_normalized_target_angle < 90) { // inner_difference_angle is less than 90 @@ -79,7 +109,7 @@ swerve_wheel tempBasicDrive(swerve_wheel input, float target_speed, float target } // Find the turning radius for the inner wheels, multiply by the distance between the robot's wheels to get the actual distance - float inner_wheel_turning_radius = tan(90.0 - inner_difference_angle) / 2.0; + float inner_wheel_turning_radius = tan(RADIANS_PER_DEGREE * (90.0 - inner_difference_angle)) / 2.0; // Find the turning radius for the outer wheels, multiply by the distance between the robot's wheels to get the actual distance float outer_wheel_turning_radius = turning_radius + 1.0; @@ -89,18 +119,65 @@ swerve_wheel tempBasicDrive(swerve_wheel input, float target_speed, float target // Find outer_difference_angle // Measure from the target angle to the nearest y-axis, this is for the outer wheels in the turn (wheels further from the center of the turning circle) - float outer_difference_angle = 90.0 - arctan(2.0 * outer_wheel_turning_radius); + float outer_difference_angle = 90.0 - (DEGREES_PER_RADIAN * arctan(2.0 * outer_wheel_turning_radius)); - // TODO: figure out which set of wheels is the inner and outer set, and then set the values below - float front_left_coefficient = ; - float front_right_coefficient = ; + // Calculate the target spin angle for the outer wheels in the turn from the outer_difference_angle + float outer_target_angle; + int normalized_target_quadrant = ((int) normalized_target_angle ) / 90; + if (normalized_target_quadrant % 2 == 0) { // Quadrants 0 or 2 + outer_target_angle = outer_difference_angle + (float) (90 * normalized_target_quadrant); + } else { // Quadrants 1 or 3 + outer_target_angle = -outer_difference_angle + (float) (90 * (normalized_target_quadrant + 1)); + } + + // Prepare these variables to be written to in the switch statement below + // Set these to 0.0, so the robot will stop if none of the cases are hit for some reason + float front_left_coefficient = 0.0; + float front_right_coefficient = 0.0; + + // Determine the speed coefficient for each motor (how fast motors are spinning relative to each other) + switch(drive_condition) { + case DRIVE_BASIC_STOP: // Motors are stopping, set target_speed to 0 + front_left_coefficient = 0.0; + front_right_coefficient = 0.0; + target_speed = 0.0; + break; + case DRIVE_BASIC_FORWARD: case DRIVE_BASIC_BACKWARD: // Motors are all spinning at the same speed since the robot is going in a straight line + front_left_coefficient = 1.0; + front_right_coefficient = 1.0; + break; + case DRIVE_BASIC_FRONTLEFT: case DRIVE_BASIC_BACKLEFT: // Left wheels are the inner wheels in the turn, so they will go slower + front_left_coefficient = inner_drive_coefficient; + front_right_coefficient = 1.0; + break; + case DRIVE_BASIC_FRONTRIGHT: case DRIVE_BASIC_BACKRIGHT: // Right wheels are the inner wheels in the turn, so they will go slower + front_left_coefficient = 1.0; + front_right_coefficient = inner_drive_coefficient; + break; + } + // Set the coefficients for the remaining 2 motors float back_left_coefficient = front_left_coefficient; // Front and back coefficients are the same for each side (left and right) float back_right_coefficient = front_right_coefficient; // Front and back coefficients are the same for each side (left and right) + // Set target wheel spin angles + switch(drive_condition) { + case DRIVE_BASIC_STOP: // Do not modify wheel target angles, robot is stopping + break; + case DRIVE_BASIC_FORWARD: case DRIVE_BASIC_BACKWARD: // All wheels facing in the same direction since the robot is going in a straight line + out = setTargetSpin(out, normalized_target_angle, normalized_target_angle, normalized_target_angle, normalized_target_angle); + break; + case DRIVE_BASIC_FRONTLEFT: case DRIVE_BASIC_BACKLEFT: // Vary the angles of the wheels since the robot is turning, left wheels are the inner wheels in the turn + out = setTargetSpin(out, normalized_target_angle, outer_difference_angle, normalized_target_angle, outer_difference_angle); + break; + case DRIVE_BASIC_FRONTRIGHT: case DRIVE_BASIC_BACKRIGHT: // Vary the angles of the wheels since the robot is turning, right wheels are the inner wheels in the turn + out = setTargetSpin(out, outer_difference_angle, normalized_target_angle, outer_difference_angle, normalized_target_angle); + break; + } - out = setTargetSpin(out, target_angle, target_angle, target_angle, target_angle); // Set the target angle for each rotation motor + // Wrap up, set motor speed and speed coefficient values out = setMotorCoefficients(out, front_left_coefficient, front_right_coefficient, back_left_coefficient, back_right_coefficient); // Set motor speed coefficients out = setDriveTargetPower(out, target_speed); // Set the power + return out; } @@ -136,9 +213,9 @@ byte identifyBasicDriveCondition(float target_speed, float target_angle) // Iden return drive_condition; } -float normalizeAngle(float angle) { // Takes an input angle and normalizes it to a point between 0 and 360 degrees +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 %= 360; - if(angle < 0) { + if(angle < 0.0) { angle += 360; } return angle; @@ -162,7 +239,7 @@ swerve_wheel setTargetSpin(swerve_wheel input, float front_left, float front_rig return out; } -swerve_wheel setSpinOffset(swerve_wheel input, float new_spin_offset) { // Set a new spin offset, and maintain the current target spin on each motor relative to the robot body +swerve_wheel setSpinOffset(swerve_wheel 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_wheel out = input; float delta_spin_offset = new_spin_offset - input.spin_offset; diff --git a/src/swerve.h b/src/swerve.h index f8f0c74..3cff618 100644 --- a/src/swerve.h +++ b/src/swerve.h @@ -2,10 +2,23 @@ float closestAngle(float current, float target); +// TODO as of 20230923 for setDirection() : fix to work with modifications made to swerve_drive struct on 20230922 +// swerve_wheel setDirection(swerve_wheel input, float setpoint); + +swerve_wheel tempTranslationDrive(swerve_wheel input, float target_speed, float target_angle); // Temporary implementation for translation drive mode + +swerve_wheel tempRotationDrive(swerve_wheel input, float target_speed) // Temporary implementation for rotation drive mode (rotating in place), positive speed is clockwise, negative speed is counterclockwise + +swerve_wheel tempBasicDrive(swerve_wheel input, float target_speed, float target_angle); // Temporary implementation for basic drive mode + +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_wheel setMotorCoefficients(swerve_wheel input, float front_left, float front_right, float back_left, float back_right); // Set the motor speed coefficients for each motor swerve_wheel setTargetSpin(swerve_wheel input, float front_left, float front_right, float back_left, float back_right); // Set the target spin for each wheel -swerve_wheel setSpinOffset(swerve_wheel input, float new_spin_offset); // Set a new spin offset, and maintain the current target spin on each motor relative to the robot body +swerve_wheel setSpinOffset(swerve_wheel 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_wheel setDriveTargetPower(swerve_wheel input, float target_drive_power); // Set a new drive power