9/22/23: Begin writing swerve drive system

Co-authored-by: evlryah <efrost@hawk.iit.edu>
Co-authored-by: Jianqi <jjin19@hawk.iit.edu>

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

src/main.cpp

@@ -13,6 +13,7 @@
 #include "zserio.h"
 #include "SerialUART.h"
 #include <Sabertooth.h>
+#include "swerve.h"
 
 const char* ssid = "TEST";
 const char* password = "pink4bubble";

src/swerve.cpp

@@ -1,12 +1,22 @@
+#include <Arduino.h>
+#include "globals.h"
 
 template <typename T> int signum(T val) {
     return (T(0) < val) - (val < T(0));
 }
-
+// based on https://compendium.readthedocs.io/en/latest/tasks/drivetrains/swerve.html
-double closestAngle(double current, double target)
+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)
+\\   \\
+
+//   //
+
+*/

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