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

Organize swerve code, add tank drive mode

Browse Source
This commit is contained in:
evlryah 2023-09-28 20:58:48 -05:00
parent 70bfc17aca
commit 2b254aaab4
4 changed files with 96 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/globals.h
View File

@ -27,10 +27,17 @@ extern long last_p;
#define MOTOR_MAX_POWER 127.0 // Highest value accepted by motor control functions
// Drive modes
#define DRIVE_STOP 0
#define DRIVE_BASIC 1
#define DRIVE_TRANSLATION 2
#define DRIVE_ROTATION 3
#define DRIVE_STOP 0
#define DRIVE_BASIC 1
#define DRIVE_TRANSLATION 2
#define DRIVE_ROTATION 3
#define DRIVE_TANK 4
// Default drive mode, currently default to stopping upon initialization
#define DEFAULT_SWERVE_DRIVE_MODE DRIVE_STOP
// Whether to enable swerve drive steering by default (set this to false if the robot is locked to tank drive)
#define DEFAULT_SWERVE_ENABLE_STEERING true
// Controller maximum inputs for joystick
#define CONTROLLER_JOYSTICK_MAX 128.0

8
src/main.cpp
View File

@ -948,15 +948,23 @@ void loop() {
switch(loop_drive_mode) {
case DRIVE_STOP:
swrv = stopSwerve(swrv);
swrv.enable_steering = false;
break;
case DRIVE_BASIC:
swrv = basicDrive(swrv, left_joystick_magnitude, left_joystick_angle);
swrv.enable_steering = true;
break;
case DRIVE_TRANSLATION:
swrv = translationDrive(swrv, left_joystick_magnitude, left_joystick_angle);
swrv.enable_steering = true;
break;
case DRIVE_ROTATION:
swrv = rotationDrive(swrv, zeroed_rx_float);
swrv.enable_steering = true;
break;
case DRIVE_TANK:
swrv = tankDrive(swrv, zeroed_ly_float, zeroed_lx_float);
swrv.enable_steering = false;
break;
}
swrv = updateEncoderData(swrv, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount()); // Update encoder data in the swerve_drive struct

103
src/swerve.cpp
View File

@ -12,6 +12,8 @@ template <typename T> int signum(T val) {
return (T(0) < val) - (val < T(0));
}
// Utility functions
float closestAngle(float current, float target) // Calculate closest distance between current and target, set the sign to the fastest direction to go from current to target
{
// get direction
@ -49,13 +51,12 @@ swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encod
}
// This function calculates the robot's current speed and attempts to modify the current state of the drive towards the target drive state
// TODO partially complete as of 20230927
swerve_drive updateSwerveCommand(swerve_drive input)
{
swerve_drive out = input;
// Set the new speed of the steering motors
if(out.target_drive_power != 0.0f || out.current_drive_power != 0.0f) { // Only set the steering power if the robot is trying to move
if((out.target_drive_power != 0.0f || out.current_drive_power != 0.0f) && out.enable_steering) { // Only set the steering power if the robot is trying to move, and if steering is enabled
// Calculate the distance and direction each motor needs to steer from where it is now
float front_left_delta = closestAngle(out.front_left_spin_angle, out.front_left_target_spin);
float front_right_delta = closestAngle(out.front_right_spin_angle, out.front_right_target_spin);
@ -65,7 +66,7 @@ swerve_drive updateSwerveCommand(swerve_drive input)
out.front_right_spin_power = calculateSteeringMotorSpeed(front_right_delta);
out.back_left_spin_power = calculateSteeringMotorSpeed(back_left_delta);
out.back_right_spin_power = calculateSteeringMotorSpeed(back_right_delta);
} else { // Stop the steering motors if the robot is stopped and not trying to move
} else { // Stop the steering motors if the robot is stopped and not trying to move, or if steering is disabled
out.front_left_spin_power = 0.0f;
out.front_right_spin_power = 0.0f;
out.back_left_spin_power = 0.0f;
@ -145,6 +146,52 @@ swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int fron
return out;
}
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 = fmod(angle, 360);
if(angle < 0.0) {
angle += 360;
}
return angle;
}
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_drive 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_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_drive out = input;
out.front_left_target_spin = front_left + input.spin_offset;
out.front_right_target_spin = front_right + input.spin_offset;
out.back_left_target_spin = back_left + input.spin_offset;
out.back_right_target_spin = back_right + input.spin_offset;
return out;
}
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_drive out = input;
float delta_spin_offset = new_spin_offset - input.spin_offset;
out.front_left_target_spin = input.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 = input.front_left_target_spin - delta_spin_offset;
out.back_right_target_spin = input.front_left_target_spin - delta_spin_offset;
return out;
}
swerve_drive setDriveTargetPower(swerve_drive input, float target_drive_power) { // Set a new drive power
swerve_drive out = input;
out.target_drive_power = target_drive_power;
return out;
}
// Drive mode functions
swerve_drive stopSwerve(swerve_drive input) // Stop all motors
{
swerve_drive out = input;
@ -303,51 +350,19 @@ byte identifyBasicDriveCondition(float target_speed, float target_angle) // Iden
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 = fmod(angle, 360);
if(angle < 0.0) {
angle += 360;
}
return angle;
}
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_drive tankDrive(swerve_drive input, float target_speed, float turn_angle) // Implementation for tank drive mode, positive angle is left, negative angle is right
{
swerve_drive 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;
//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;
}
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_drive out = input;
out.front_left_target_spin = front_left + input.spin_offset;
out.front_right_target_spin = front_right + input.spin_offset;
out.back_left_target_spin = back_left + input.spin_offset;
out.back_right_target_spin = back_right + input.spin_offset;
return out;
}
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_drive out = input;
float delta_spin_offset = new_spin_offset - input.spin_offset;
out.front_left_target_spin = input.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 = input.front_left_target_spin - delta_spin_offset;
out.back_right_target_spin = input.front_left_target_spin - delta_spin_offset;
return out;
}
swerve_drive setDriveTargetPower(swerve_drive input, float target_drive_power) { // Set a new drive power
swerve_drive out = input;
out.target_drive_power = target_drive_power;
return out;
}
// Modulus Plus: Drive control planning: v1 on 20230922
/*

29
src/swerve.h
View File

@ -7,7 +7,8 @@ typedef struct { // swerve_drive struct, used to track and manage the state of t
// byte target_spin_direction = CLOCKWISE; // Target state
// byte target_drive_direction = DRIVE_FORWARDS; // Target state
int drive_mode = DRIVE_BASIC;
int drive_mode = DEFAULT_SWERVE_DRIVE_MODE;
bool enable_steering = DEFAULT_SWERVE_ENABLE_STEERING;
float target_drive_power = 0.0f; // -127.0 to 127.0 : TARGET power that the robot is trying to get to
float current_drive_power = 0.0f; // -127.0 to 127.0 : CURRENT power being given to drive motors (before coefficient is applied)
@ -58,7 +59,6 @@ typedef struct { // swerve_drive struct, used to track and manage the state of t
float back_right_coefficient = 0.0f;
// Encoder tracking, used to track speed of the steering motors
// The 0th entry in the buffer is the most recent, the highest index entry is the oldest
// The buffer is modified each time updateEncoderData() is called
@ -70,6 +70,8 @@ typedef struct { // swerve_drive struct, used to track and manage the state of t
} swerve_drive;
// Utility functions
float closestAngle(float current, float target); // Calculate closest distance between current and target, set the sign to the fastest direction to go from current to target
// TODO as of 20230923 for setDirection() : fix to work with modifications made to swerve_drive struct on 20230922
@ -83,6 +85,19 @@ float calculateSteeringMotorSpeed(float steering_delta); // Calculate the speed
swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder); // Process new encoder data, calculate the speed and angle of the steering motors
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_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_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_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_drive setDriveTargetPower(swerve_drive input, float target_drive_power); // Set a new drive power
// Drive mode functions
swerve_drive stopSwerve(swerve_drive input); // Stop all motors
swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle); // Implementation for translation drive mode
@ -93,12 +108,4 @@ swerve_drive basicDrive(swerve_drive input, float target_speed, float target_ang
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_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_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_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_drive setDriveTargetPower(swerve_drive input, float target_drive_power); // Set a new drive power
swerve_drive tankDrive(swerve_drive input, float target_speed, float turn_angle); // Implementation for tank drive mode, positive angle is left, negative angle is right