From 394746d0590edc9c78ea22b3880a60ec54eaa68b Mon Sep 17 00:00:00 2001 From: Cole Deck Date: Fri, 22 Sep 2023 22:34:00 -0500 Subject: [PATCH] 9/22/23: Begin writing swerve drive system Co-authored-by: evlryah Co-authored-by: Jianqi --- src/globals.h | 50 ++++++++++ src/main.cpp | 1 + src/swerve.cpp | 253 ++++++++++++++++++++++++++++++++++++++++++++++++- src/swerve.h | 11 +++ 4 files changed, 312 insertions(+), 3 deletions(-) create mode 100644 src/swerve.h diff --git a/src/globals.h b/src/globals.h index 2069a01..8a6c25a 100644 --- a/src/globals.h +++ b/src/globals.h @@ -7,7 +7,57 @@ extern comm_state cs; extern char comm_ok; extern long last_p; +/* +#define CLOCKWISE 0 +#define ANTICLOCKWISE 1 +*/ +// Drive modes +#define DRIVE_BASIC 0 +#define DRIVE_TRANSLATION 1 +#define DRIVE_ROTATION 2 + +// Basic mode conditions, specifies which direction and turning direction the robot is using +#define DRIVE_BASIC_STOP 0 +#define DRIVE_BASIC_FORWARD 1 +#define DRIVE_BASIC_FRONTLEFT 2 +#define DRIVE_BASIC_FRONTRIGHT 3 +#define DRIVE_BASIC_BACKWARD 4 +#define DRIVE_BASIC_BACKLEFT 5 +#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 + + float target_drive_power = 127.0; // 0 - 127 + float current_drive_power = 127.0; // 0 - 127 + + 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 + + // CURRENT wheel orientations relative to robot body, read from encoders & convert + float front_left_current_spin = 0; + 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; + float front_right_coefficient = 0; + float back_left_coefficient = 0; + float back_right_coefficient = 0; + +} swerve_wheel; #define SerComm Serial1 //Serial port connected to Xbee #define DIAMOND_LEFT 0 diff --git a/src/main.cpp b/src/main.cpp index f158c63..dced0df 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,6 +13,7 @@ #include "zserio.h" #include "SerialUART.h" #include +#include "swerve.h" const char* ssid = "TEST"; const char* password = "pink4bubble"; diff --git a/src/swerve.cpp b/src/swerve.cpp index 8ed6a6c..9542c52 100644 --- a/src/swerve.cpp +++ b/src/swerve.cpp @@ -1,12 +1,22 @@ +#include +#include "globals.h" template int signum(T val) { return (T(0) < val) - (val < T(0)); } - -double closestAngle(double current, double target) +// based on https://compendium.readthedocs.io/en/latest/tasks/drivetrains/swerve.html +float closestAngle(float current, float target) { + while(current > 360) { + current -= 360.0; + } + current = 360 - current; + while(target > 360) { + target -= 360.0; + } + target = 360 - target; // get direction - double dir = target % 360.0 - current % 360.0; + float dir = target - current; // convert from -360 to 360 to -180 to 180 if (abs(dir) > 180.0) @@ -15,3 +25,240 @@ double closestAngle(double current, double target) } return dir; } + + +swerve_wheel setDirection(swerve_wheel input, float setpoint) +{ + swerve_wheel out = input; + float currentAngle = input.current_spin_location; + // find closest angle to setpoint + float setpointAngle = closestAngle(currentAngle, setpoint); + // find closest angle to setpoint + 180 + float setpointAngleFlipped = closestAngle(currentAngle, setpoint + 180.0); + // if the closest angle to setpoint is shorter + if (abs(setpointAngle) <= abs(setpointAngleFlipped)) + { + // unflip the motor direction use the setpoint + out.target_spin_direction = CLOCKWISE; + out.target_spin_location = (currentAngle + setpointAngle); + } + // if the closest angle to setpoint + 180 is shorter + else + { + // flip the motor direction and use the setpoint + 180 + out.target_spin_direction = ANTICLOCKWISE; + out.target_spin_location = (currentAngle + setpointAngleFlipped); + } + 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 + 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; +} + +swerve_wheel tempBasicDrive(swerve_wheel input, float target_speed, float target_angle) // Temporary implementation for basic drive mode +{ + swerve_wheel out = input; + + float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle + 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) + 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 + inner_difference_angle = semicircle_normalized_target_angle; + } else { // inner_difference_angle is greater than 90 + inner_difference_angle = 180 - semicircle_normalized_target_angle; + } + + // 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; + + // 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; + + // Find the inner drive motor coefficient (to slow down the inner motors to compensate for a smaller turning radius), this is the ratio bewteen the inner and outer turning radii + float inner_drive_coefficient = outer_wheel_turning_radius / inner_wheel_turning_radius; + + // 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); + + // 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 = ; + 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) + + + out = setTargetSpin(out, target_angle, target_angle, target_angle, target_angle); // Set the target angle for each rotation motor + 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; +} + +byte identifyBasicDriveCondition(float target_speed, float target_angle) // Identify the condition in which the basic drive mode will be operating +{ + if(target_speed <= 0.0 || target_speed >= 127.0) { // Speed is stopped, negative, or too high + return DRIVE_BASIC_STOP; + } + target_angle = normalizeAngle(target_angle); // Normalize the target angle + if(target_angle == 0) { + return DRIVE_BASIC_FORWARD; + } + if(target_angle == 180) { + return DRIVE_BASIC_BACKWARD; + } + int target_quadrant = ((int) target_angle) / 90; // Which quadrant is the target angle in (quadrant 0 is northeast, increases clockwise) + byte drive_condition = DRIVE_BASIC_STOP; // Keep the drive stopped if the normalized target angle somehow exceeded 360. + switch(target_octant) { + case 0: + drive_condition = DRIVE_BASIC_FRONTRIGHT; + break; + case 1: + drive_condition = DRIVE_BASIC_BACKRIGHT; + break; + case 2: + drive_condition = DRIVE_BASIC_BACKLEFT; + break; + case 3: + drive_condition = DRIVE_BASIC_FRONTLEFT; + break; + default: // Unreachable, assuming that the target angle is less than 360 + } + return drive_condition; +} + +float normalizeAngle(float angle) { // Takes an input angle and normalizes it to a point between 0 and 360 degrees + angle %= 360; + if(angle < 0) { + angle += 360; + } + return angle; +} + +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 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_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 out = input; + out.front_left_target_spin = front_left + in.spin_offset; + out.front_right_target_spin = front_right + in.spin_offset; + out.back_left_target_spin = back_left + in.spin_offset; + out.back_right_target_spin = back_right + in.spin_offset; + 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 out = input; + + float delta_spin_offset = new_spin_offset - input.spin_offset; + + out.front_left_target_spin = in.front_left_target_spin - delta_spin_offset; + out.front_right_target_spin = in.front_left_target_spin - delta_spin_offset; + out.back_left_target_spin = in.front_left_target_spin - delta_spin_offset; + out.back_right_target_spin = in.front_left_target_spin - delta_spin_offset; + return out; +} + +swerve_wheel setDriveTargetPower(swerve_wheel input, float target_drive_power) { // Set a new drive power + swerve_wheel out = input; + out.target_drive_power = target_drive_power; + return out; +} + + + +// Modulus Plus: Drive control planning: v1 on 20230922 +/* + +315 000 045 + +270 ^ 090 + +225 180 135 + +If speed AND any wheel's target-current angle are BOTH above a certain threshold, decelerate before spinning the wheel + +Variables to monitor and control: + Drive modes: + Translation (Linear motion in any direction while preserving body orientation) + Basic (Forwards or backwards, may turn) + Rotation (rotation in place) + Rotation angle (in Translation or Basic drive mode) + Drive speed + Drive forward orientation relative to robot body + +Common Controls: + Button 1: + Switch between drive modes + Button 2: + Toggle preservation of drive forward orientation relative to field + When off, the drive forward direction is locked relative to the body + When on, the drive forward direction is locked relative to the field + D-Pad (While button 2 is pressed): + Reset drive orientation relative to body + Set direction to the corresponding d-pad button direction + +Translation Mode Controls: + Joystick A: + Vector Amplitude: Drive speed + Vector Angle: Drive angle + +Basic Mode Controls: + Joystick A: + Vector Amplitude: Drive speed + Vector Angle: Drive angle + +Rotation Mode Controls: + Joystick A: + Left/Right: Rotation speed + +Rotation drive mode wheel orientation +Turning in place +// \\ + +\\ // + + +Translation or Basic drive mode wheel orientation, with a rotation angle of 0 +Driving in a straight line +|| || + + +|| || + +Translation drive mode wheel orientation, with a rotation angle of 90 or 270 +Driving to the left or right +_ _ +- - + +_ _ +- - + +Translation drive mode wheel orientation, with a rotation angle of 45 +Driving forward and to the right +// // + +// // + +Basic drive mode wheel orientation with a rotation angle of 315 +Driving forwards while turning (to the left in this example) +\\ \\ + +// // + +*/ diff --git a/src/swerve.h b/src/swerve.h new file mode 100644 index 0000000..f8f0c74 --- /dev/null +++ b/src/swerve.h @@ -0,0 +1,11 @@ +#include "globals.h" + +float closestAngle(float current, float target); + +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 setDriveTargetPower(swerve_wheel input, float target_drive_power); // Set a new drive power