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

9/22/23: Begin writing swerve drive system

Browse Source 
Co-authored-by: evlryah <efrost@hawk.iit.edu>
Co-authored-by: Jianqi <jjin19@hawk.iit.edu>
This commit is contained in:
Cole Deck 2023-09-22 22:34:00 -05:00
parent b4e999475e
commit 394746d059
4 changed files with 312 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
src/globals.h
View File

@ -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

1
src/main.cpp
View File

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

253
src/swerve.cpp
View File

@ -1,12 +1,22 @@
#include <Arduino.h>
#include "globals.h"
template <typename T> 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)
\\ \\
// //
*/

11
src/swerve.h Normal file
View File

@ -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