Fix errors, start work on swerve inputs

src/globals.h

@@ -36,56 +36,7 @@ extern long last_p;
 // The buffer will track the last N states of the encoders, and the times at which they were recorded, to determine the robot's current speed
 #define ENCODER_BUFFER_ENTRY_COUNT 5
 
-typedef struct { // swerve_wheel struct, used to track and manage the state of the robot's swerve drive system
-    // 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 drive_mode = DRIVE_BASIC;
-    float target_drive_power = 127.0; // 0.0 - 127.0 , TARGET speed: this is the speed that the robot is trying to get to
-    float current_drive_power = 127.0; // 0.0 - 127.0 : CURRENT / COMMAND speed (power) being given to drive motors (before coefficient is applied)
-    
-    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, this allows modification of the robot's forward orientation relative to its body
-    
-    // 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;
 
-    // COMMAND wheel orientations relative to robot body, this is what the motors are currently being told to do
-    float front_left_ccommand_spin = 0;
-    float front_right_command_spin = 0;
-    float back_left_command_spin = 0;
-    float back_right_command_spin = 0;
-
-    // TARGET wheel orientations relative to robot body, this is the state that the robot is trying to get to
-    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;
-
-    
-    
-    // Encoder tracking, used to track speed of the robot to determine when it is safe to reorient the wheels
-    // The 0th entry in the buffer is the most recent, the highest entry is the oldest
-    // The buffer is modified each time updateSwerveCommand() is called
-    int encoder_buffer_times_ms[ENCODER_BUFFER_ENTRY_COUNT]; // Buffer that tracks the times at which encoder states were measures, uses milliseconds (either unix millis or relative to pico start)
-    int encoder_buffer_front_left[ENCODER_BUFFER_ENTRY_COUNT];
-    int encoder_buffer_front_right[ENCODER_BUFFER_ENTRY_COUNT];
-    int encoder_buffer_back_left[ENCODER_BUFFER_ENTRY_COUNT];
-    int encoder_buffer_back_right[ENCODER_BUFFER_ENTRY_COUNT];
-    
-} swerve_wheel;
 
 #define SerComm Serial1   //Serial port connected to Xbee
 #define DIAMOND_LEFT 0

src/main.cpp

@@ -22,6 +22,10 @@ const char* password = "pink4bubble";
 //SevenSegmentRowDDLayer *sevenSeg; 
 //JoystickDDLayer *appjoystick;
 
+swerve_drive swrv;
+bool drive_type = DRIVE_TRANSLATION;
+int drive_mode = DRIVE_TRANSLATION; 
+
 packet_t pA, pB, safe;
 packet_t *astate, *incoming;
 comm_state cs;
@@ -799,7 +803,16 @@ void setup() {
                                     optionsdisplay->getLayerId())));
   dumbdisplay.playbackLayerSetupCommands("basic");*/
   //dumbdisplay.configAutoPin(DD_AP_VERT); // auto vertical pin layout
+
+
+
+
+
+  swrv.target_drive_power = 0;
+  swrv.current_drive_power = 0; // don't want the robot to move right away here
   setup_complete = true;
+
+  
   rp2040.wdt_begin(500); // start watchdog with 500ms limit. Safety feature; reset during crash to disable motors!
 }
 
@@ -833,9 +846,10 @@ void loop() {
   
   comm_parse();
   if (getButton(SHOULDER_TOP_RIGHT))
-    turbo = true;
+    drive_type = DRIVE_TRANSLATION;
-  else 
+  else if(getButton(SHOULDER_TOP_LEFT))
-    turbo = false;
+    drive_type = DRIVE_BASIC;
+
   //const DDFeedback*  fb;
   /*if (!dumbdisplay.connected() || !WiFi.isConnected()) {
     target_left_power = 0;
@@ -849,8 +863,28 @@ void loop() {
   int zeroed_ly =      ((int)(astate->stickY) - 127);
   int zeroed_rx =    -1 * ((int)(astate->stickRX) - 127);
   int zeroed_ry =      ((int)(astate->stickRY) - 127);
+  
+  if (sqrt(zeroed_lx^2 + zeroed_ly^2) > 10) { // if left stick is touched
+    drive_mode = drive_type;
+  }
+  if (sqrt(zeroed_rx^2 + zeroed_ry^2) > 10) { // if right stick is touched - override translation
+    drive_mode = DRIVE_ROTATION;
+  }
 
 
+
+  // Modulus swerve drive
+
+  if (drive_mode == DRIVE_BASIC) {
+    swrv = tempBasicDrive(swrv, sqrt(zeroed_lx^2 + zeroed_ly^2), atan2(zeroed_ly, zeroed_lx));
+  }
+  if (drive_mode == DRIVE_TRANSLATION) {
+    swrv = tempTranslationDrive(swrv, sqrt(zeroed_lx^2 + zeroed_ly^2), atan2(zeroed_ly, zeroed_lx));
+  }
+  if (drive_mode == DRIVE_ROTATION) {
+    swrv = tempRotationDrive(swrv, zeroed_rx);
+  }
+
   // Goliath / 2 side arcade tank drive code below
   /*int zeroed_power =    -1 * ((int)(astate->stickX) - 127);
   int zeroed_turn =      ((int)(astate->stickY) - 127);

src/swerve.cpp

@@ -1,5 +1,6 @@
 #include <Arduino.h>
 #include "globals.h"
+#include "swerve.h"
 #include <math.h>
 
 template <typename T> int signum(T val) {
@@ -55,9 +56,9 @@ swerve_drive updateSwerveCommand(swerve_drive input) {
 
 /*
 // 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_drive setDirection(swerve_drive input, float setpoint)
 {
-    swerve_wheel out = input;
+    swerve_drive out = input;
     float currentAngle = input.current_spin_location;
     // find closest angle to setpoint
     float setpointAngle = closestAngle(currentAngle, setpoint);
@@ -81,9 +82,9 @@ 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_drive tempTranslationDrive(swerve_drive input, float target_speed, float target_angle) // Temporary implementation for translation drive mode
 {
-    swerve_wheel out = input;
+    swerve_drive out = input;
 
     float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle
 
@@ -97,11 +98,11 @@ swerve_wheel tempTranslationDrive(swerve_wheel input, float target_speed, float
     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_drive tempRotationDrive(swerve_drive 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;
+    swerve_drive out = input;
 
-    float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle
+    //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
@@ -114,9 +115,9 @@ swerve_wheel tempRotationDrive(swerve_wheel input, float target_speed) // Tempor
     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_drive tempBasicDrive(swerve_drive input, float target_speed, float target_angle) // Temporary implementation for basic drive mode
 {
-    swerve_wheel out = input;
+    swerve_drive 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
@@ -124,7 +125,7 @@ swerve_wheel tempBasicDrive(swerve_wheel input, float target_speed, float target
     // Find inner_difference_angle
     // 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;
+    float semicircle_normalized_target_angle = fmod(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
@@ -135,14 +136,14 @@ swerve_wheel tempBasicDrive(swerve_wheel input, float target_speed, float target
     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;
+    float outer_wheel_turning_radius = inner_wheel_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 - (DEGREES_PER_RADIAN * arctan(2.0 * outer_wheel_turning_radius));
+    float outer_difference_angle = 90.0 - (DEGREES_PER_RADIAN * atan(2.0 * outer_wheel_turning_radius));
 
     // Calculate the target spin angle for the outer wheels in the turn from the outer_difference_angle
     float outer_target_angle;
@@ -218,7 +219,7 @@ byte identifyBasicDriveCondition(float target_speed, float target_angle) // Iden
     }
     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) {
+    switch(target_quadrant) {
         case 0:
             drive_condition = DRIVE_BASIC_FRONTRIGHT;
             break;
@@ -232,20 +233,22 @@ byte identifyBasicDriveCondition(float target_speed, float target_angle) // Iden
             drive_condition = DRIVE_BASIC_FRONTLEFT;
             break;
         default: // Unreachable, assuming that the target angle is less than 360
+            drive_condition = DRIVE_BASIC_STOP;
+            break;
     }
     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 %= 360;
+    angle = fmod(angle, 360);
     if(angle < 0.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_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_wheel out = input;
+    swerve_drive out = input;
     out.front_left_coefficient = front_left;
     out.front_right_coefficient = front_right;
     out.back_left_coefficient = back_left;
@@ -253,29 +256,29 @@ swerve_wheel setMotorCoefficients(swerve_wheel input, float front_left, float fr
     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_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_wheel out = input;
+    swerve_drive out = input;
-    out.front_left_target_spin = front_left + in.spin_offset;
+    out.front_left_target_spin = front_left + input.spin_offset;
-    out.front_right_target_spin = front_right + in.spin_offset;
+    out.front_right_target_spin = front_right + input.spin_offset;
-    out.back_left_target_spin = back_left + in.spin_offset;
+    out.back_left_target_spin = back_left + input.spin_offset;
-    out.back_right_target_spin = back_right + in.spin_offset;
+    out.back_right_target_spin = back_right + input.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 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_wheel out = input;
+    swerve_drive 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_left_target_spin = input.front_left_target_spin - delta_spin_offset;
-    out.front_right_target_spin = in.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 = in.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 = in.front_left_target_spin - delta_spin_offset;
+    out.back_right_target_spin = input.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_drive setDriveTargetPower(swerve_drive input, float target_drive_power) { // Set a new drive power
-    swerve_wheel out = input;
+    swerve_drive out = input;
     out.target_drive_power = target_drive_power;
     return out;
 }

src/swerve.h

@@ -1,26 +1,77 @@
 #include "globals.h"
 
+typedef struct { // swerve_drive struct, used to track and manage the state of the robot's swerve drive system
+    // 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 drive_mode = DRIVE_BASIC;
+    float target_drive_power = 127.0; // 0.0 - 127.0 , TARGET speed: this is the speed that the robot is trying to get to
+    float current_drive_power = 127.0; // 0.0 - 127.0 : CURRENT / COMMAND speed (power) being given to drive motors (before coefficient is applied)
+    
+    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, this allows modification of the robot's forward orientation relative to its body
+    
+    // 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;
+
+    // COMMAND wheel orientations relative to robot body, this is what the motors are currently being told to do
+    float front_left_command_spin = 0;
+    float front_right_command_spin = 0;
+    float back_left_command_spin = 0;
+    float back_right_command_spin = 0;
+
+    // TARGET wheel orientations relative to robot body, this is the state that the robot is trying to get to
+    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;
+
+    
+    
+    // Encoder tracking, used to track speed of the robot to determine when it is safe to reorient the wheels
+    // The 0th entry in the buffer is the most recent, the highest entry is the oldest
+    // The buffer is modified each time updateSwerveCommand() is called
+    int encoder_buffer_times_ms[ENCODER_BUFFER_ENTRY_COUNT]; // Buffer that tracks the times at which encoder states were measures, uses milliseconds (either unix millis or relative to pico start)
+    int encoder_buffer_front_left[ENCODER_BUFFER_ENTRY_COUNT];
+    int encoder_buffer_front_right[ENCODER_BUFFER_ENTRY_COUNT];
+    int encoder_buffer_back_left[ENCODER_BUFFER_ENTRY_COUNT];
+    int encoder_buffer_back_right[ENCODER_BUFFER_ENTRY_COUNT];
+    
+} swerve_drive;
+
 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_drive setDirection(swerve_drive input, float setpoint);
 
 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
 
-swerve_wheel tempTranslationDrive(swerve_wheel input, float target_speed, float target_angle); // Temporary implementation for translation drive mode
+swerve_drive tempTranslationDrive(swerve_drive 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_drive tempRotationDrive(swerve_drive 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
+swerve_drive tempBasicDrive(swerve_drive 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_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_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_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_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_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_wheel setDriveTargetPower(swerve_wheel 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