adeck/rib-test-code
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Basic 3-mode swerve drive system calculations

Browse Source
This commit is contained in:
evlryah 2023-09-23 04:46:19 -05:00
parent 394746d059
commit cd79a133f1
3 changed files with 116 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/globals.h
View File

@ -1,5 +1,7 @@
#pragma once #pragma once
#include "packet.h" #include "packet.h"
#include <math.h>
#define _USE_MATH_DEFINES // To access M_PI constant for trig functions
extern packet_t pA, pB, safe; extern packet_t pA, pB, safe;
extern packet_t *astate, *incoming; extern packet_t *astate, *incoming;
@ -12,6 +14,10 @@ extern long last_p;
#define ANTICLOCKWISE 1 #define ANTICLOCKWISE 1
*/ */
// Math things
#define DEGREES_PER_RADIAN (180.0 / M_PI)
#define RADIANS_PER_DEGREE (M_PI / 180.0)
// Drive modes // Drive modes
#define DRIVE_BASIC 0 #define DRIVE_BASIC 0
#define DRIVE_TRANSLATION 1 #define DRIVE_TRANSLATION 1
@ -27,15 +33,14 @@ extern long last_p;
#define DRIVE_BASIC_BACKRIGHT 6 #define DRIVE_BASIC_BACKRIGHT 6
typedef struct { typedef struct {
byte drive_mode = DRIVE_BASIC;
// byte spin_direction = CLOCKWISE; // Current state, 0 = clockwise, 1 = anticlockwise // 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 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 drive_mode = DRIVE_BASIC;
//byte target_drive_direction = DRIVE_FORWARDS; // Target state float target_drive_power = 127.0; // 0.0 - 127.0
float current_drive_power = 127.0; // 0.0 - 127.0
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_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 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 front_right_current_spin = 0;
float back_left_current_spin = 0; float back_left_current_spin = 0;
float back_right_current_spin = 0; float back_right_current_spin = 0;
// TARGET wheel orientations relative to robot body // TARGET wheel orientations relative to robot body
float front_left_target_spin = 0; float front_left_target_spin = 0;
float front_right_target_spin = 0; float front_right_target_spin = 0;
float back_left_target_spin = 0; float back_left_target_spin = 0;
float back_right_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 // 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 // Between 0 and 1
float front_left_coefficient = 0; float front_left_coefficient = 0;

101
src/swerve.cpp
View File

@ -1,10 +1,13 @@
#include <Arduino.h> #include <Arduino.h>
#include "globals.h" #include "globals.h"
#include <math.h>
template <typename T> int signum(T val) { template <typename T> int signum(T val) {
return (T(0) < val) - (val < T(0)); 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) float closestAngle(float current, float target)
{ {
while(current > 360) { 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 setDirection(swerve_wheel input, float setpoint)
{ {
swerve_wheel out = input; swerve_wheel out = input;
@ -51,16 +56,41 @@ swerve_wheel setDirection(swerve_wheel input, float setpoint)
} }
return out; return out;
} }
*/
swerve_wheel tempTranslationDrive(swerve_wheel input, float target_speed, float target_angle) // Temporary implementation for translation drive mode swerve_wheel tempTranslationDrive(swerve_wheel input, float target_speed, float target_angle) // Temporary implementation for translation drive mode
{ {
swerve_wheel out = input; 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 = 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 out = setDriveTargetPower(out, target_speed); // Set the power
return out; 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 tempBasicDrive(swerve_wheel input, float target_speed, float target_angle) // Temporary implementation for basic drive mode
{ {
swerve_wheel out = input; 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 byte drive_condition = identifyBasicDriveCondition(target_speed, normalized_target_angle); // Identify the drive condition
// Find inner_difference_angle // 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 inner_difference_angle;
float semicircle_normalized_target_angle = normalized_target_angle % 180.0; float semicircle_normalized_target_angle = normalized_target_angle % 180.0;
if(semicircle_normalized_target_angle < 90) { // inner_difference_angle is less than 90 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 // 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 // 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 = turning_radius + 1.0;
@ -89,18 +119,65 @@ swerve_wheel tempBasicDrive(swerve_wheel input, float target_speed, float target
// Find outer_difference_angle // 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) // 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 // Calculate the target spin angle for the outer wheels in the turn from the outer_difference_angle
float front_left_coefficient = ; float outer_target_angle;
float front_right_coefficient = ; 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_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) 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 = 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 out = setDriveTargetPower(out, target_speed); // Set the power
return out; return out;
} }
@ -136,9 +213,9 @@ byte identifyBasicDriveCondition(float target_speed, float target_angle) // Iden
return drive_condition; 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; angle %= 360;
if(angle < 0) { if(angle < 0.0) {
angle += 360; angle += 360;
} }
return angle; return angle;
@ -162,7 +239,7 @@ swerve_wheel setTargetSpin(swerve_wheel input, float front_left, float front_rig
return out; 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; swerve_wheel out = input;
float delta_spin_offset = new_spin_offset - input.spin_offset; float delta_spin_offset = new_spin_offset - input.spin_offset;

15
src/swerve.h
View File

@ -2,10 +2,23 @@
float closestAngle(float current, float target); 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 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 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 swerve_wheel setDriveTargetPower(swerve_wheel input, float target_drive_power); // Set a new drive power