517 lines
26 KiB
C++
517 lines
26 KiB
C++
#include <Arduino.h>
|
|
#include "globals.h"
|
|
#include "swerve.h"
|
|
#include <math.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
|
|
// 3-mode drive control system based on https://compendium.readthedocs.io/en/latest/tasks/drivetrains/swerve.html
|
|
|
|
|
|
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
|
|
float dir = normalizeAngle(target) - normalizeAngle(current);
|
|
|
|
// convert from -360 to 360 to -180 to 180
|
|
if (abs(dir) > 180.0)
|
|
{
|
|
dir = -(signum(dir) * 360.0) + dir;
|
|
}
|
|
return dir;
|
|
}
|
|
|
|
swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder) // Initialize a swerve_drive struct, use the current time and encoder positions to fill the arrays
|
|
{
|
|
swerve_drive out;
|
|
|
|
for(int i = 1; i < ENCODER_BUFFER_ENTRY_COUNT; i++) // Iterate over the elements in the arrays
|
|
{
|
|
out.encoder_buffer_times_ms[i] = 0; // Initialize the contents of this array to 0, since no ongoing encoder samples have been taken yet
|
|
out.encoder_buffer_front_left[i] = front_left_encoder; // Initialze the contents of this array to this motor's current encoder position, since no ongoing encoder samples have been taken yet
|
|
out.encoder_buffer_front_right[i] = front_right_encoder; // Initialze the contents of this array to this motor's current encoder position, since no ongoing encoder samples have been taken yet
|
|
out.encoder_buffer_back_left[i] = back_left_encoder; // Initialze the contents of this array to this motor's current encoder position, since no ongoing encoder samples have been taken yet
|
|
out.encoder_buffer_back_right[i] = back_right_encoder; // Initialze the contents of this array to this motor's current encoder position, since no ongoing encoder samples have been taken yet
|
|
}
|
|
out.encoder_buffer_times_ms[0] = millis(); // Record an initial encoder state sample
|
|
|
|
// Store the initial positions of the steering motor encoders
|
|
out.front_left_initial_spin_encoder = front_left_encoder;
|
|
out.front_right_initial_spin_encoder = front_right_encoder;
|
|
out.back_left_initial_spin_encoder = back_left_encoder;
|
|
out.back_right_initial_spin_encoder = back_right_encoder;
|
|
|
|
return out;
|
|
}
|
|
|
|
// This function calculates the robot's current speed and attempts to modify the current state of the drive towards the target drive state
|
|
swerve_drive updateSwerveCommand(swerve_drive input)
|
|
{
|
|
swerve_drive out = input;
|
|
float new_drive_coefficient = out.target_drive_power;
|
|
// Set the new speed of the steering motors
|
|
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);
|
|
float back_left_delta = closestAngle(out.back_left_spin_angle, out.back_left_target_spin);
|
|
float back_right_delta = closestAngle(out.back_right_spin_angle, out.back_right_target_spin);
|
|
// Use the delta and speed of each steering motor to calculate the necessary speed
|
|
out.front_left_spin_power = calculateSteeringMotorSpeed(front_left_delta, out.front_left_measured_spin_speed);
|
|
out.front_right_spin_power = calculateSteeringMotorSpeed(front_right_delta, out.front_right_measured_spin_speed);
|
|
out.back_left_spin_power = calculateSteeringMotorSpeed(back_left_delta, out.back_left_measured_spin_speed);
|
|
out.back_right_spin_power = calculateSteeringMotorSpeed(back_right_delta, out.back_right_measured_spin_speed);
|
|
|
|
float max_abs_steering_delta = max(max(fabs(front_left_delta), fabs(front_right_delta)), max(fabs(back_left_delta), fabs(back_right_delta)));
|
|
if (max_abs_steering_delta > STEERING_TOLERANCE_DISABLE_DRIVE) {
|
|
new_drive_coefficient = 0;
|
|
}
|
|
Serial.printf("max_abs_steering_delta = %f\t\tndc = %f\r\n", max_abs_steering_delta, new_drive_coefficient);
|
|
|
|
// TESTING DEBUG print 20230929
|
|
Serial.printf("FL delta = %f\t\tFL steer = %f\r\n", front_left_delta, out.front_left_spin_power);
|
|
Serial.printf("FR delta = %f\t\tFR steer = %f\r\n", front_right_delta, out.front_right_spin_power);
|
|
Serial.printf("BL delta = %f\t\tBL steer = %f\r\n", back_left_delta, out.back_left_spin_power);
|
|
Serial.printf("BR delta = %f\t\tBR steer = %f\r\n", back_right_delta, out.back_right_spin_power);
|
|
|
|
|
|
} 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;
|
|
out.back_right_spin_power = 0.0f;
|
|
}
|
|
|
|
// Set the current drive power to the target drive power, TODO: this is TEMPORARY, add in something to slow the current (set) speed until the wheels are in the correct direction
|
|
//out.current_drive_power = out.target_drive_power;
|
|
// Set the new drive motor power, apply coefficients, set between -127.0 and 127.0
|
|
out.current_drive_power = new_drive_coefficient;
|
|
out.front_left_power = new_drive_coefficient * out.front_left_coefficient * DRIVE_MOTOR_MAX_POWER;
|
|
out.front_right_power = new_drive_coefficient * out.front_right_coefficient * DRIVE_MOTOR_MAX_POWER;
|
|
out.back_left_power = new_drive_coefficient * out.back_left_coefficient * DRIVE_MOTOR_MAX_POWER;
|
|
out.back_right_power = new_drive_coefficient * out.back_right_coefficient * DRIVE_MOTOR_MAX_POWER;
|
|
|
|
return out;
|
|
}
|
|
|
|
float calculateSteeringMotorSpeed(float steering_delta, float current_angular_speed) // Calculate the speed of a steering motor based on its distance from its target angle and its current angular speed
|
|
{
|
|
float abs_steering_delta = fabs(steering_delta);
|
|
if(abs_steering_delta > STEERING_SLOW_DELTA && abs_steering_delta > STEERING_TOLERANCE) { // In full speed range, still far enough away from the target angle
|
|
return STEERING_MOTOR_SPEED_LIMIT * (steering_delta < 0.0f ? -1.0f : 1.0f);
|
|
} else { // Slow down the speed of the steering motor since it's close to its target angle
|
|
|
|
float calc_steering_delta = steering_delta + (STEERING_ACCEL_SLOW_DELAY * current_angular_speed); // Modify the steering delta to the estimated delta in STEERING_ACCEL_SLOW_DELAY seconds to account for motor acceleration
|
|
float calc_steering_limit_signed = STEERING_MOTOR_SPEED_LIMIT * (calc_steering_delta < 0.0f ? -1.0f : 1.0f); // Update the sign to account for the future location estimation above
|
|
float calc_abs_steering_delta = fabs(calc_steering_delta); // Update abs_steering_delta with the new steering_delta
|
|
float steering_speed_fraction = powf(calc_abs_steering_delta / STEERING_SLOW_DELTA, 2.0f); // Fraction of full speed being used
|
|
//return steering_limit_signed * (1.0f - (abs_steering_delta / STEERING_SLOW_DELTA));
|
|
if(current_angular_speed < STEERING_STALL_DETECT_ANGULAR_SPEED || steering_speed_fraction < STEERING_SLOW_APPROACH_SPEED) { // Detect motor stall during approach and increase speed to allow for approach
|
|
steering_speed_fraction = STEERING_SLOW_APPROACH_SPEED;
|
|
|
|
if(calc_abs_steering_delta < STEERING_HOVER_RANGE) { // Decrease speed further if the steering is extremely close to the target
|
|
steering_speed_fraction *= (calc_abs_steering_delta / STEERING_HOVER_RANGE);
|
|
} else if(abs_steering_delta < STEERING_HOVER_RANGE) {
|
|
steering_speed_fraction *= (abs_steering_delta / STEERING_HOVER_RANGE);
|
|
}
|
|
}
|
|
if(calc_abs_steering_delta < STEERING_TOLERANCE) { // Stop the steering motors if they are within the tolerance range
|
|
return 0.0f;
|
|
}
|
|
|
|
return calc_steering_limit_signed * steering_speed_fraction; // Apply the direction
|
|
}
|
|
}
|
|
|
|
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
|
|
{
|
|
swerve_drive out = in;
|
|
// Move values over 1 slot in the encoder data buffer to make room for new data, discard data at the end of the buffer
|
|
for(int i = 0; i < ENCODER_BUFFER_ENTRY_COUNT - 1; i++)
|
|
{
|
|
out.encoder_buffer_times_ms[i + 1] = out.encoder_buffer_times_ms[i];
|
|
out.encoder_buffer_front_left[i + 1] = out.encoder_buffer_front_left[i];
|
|
out.encoder_buffer_front_right[i + 1] = out.encoder_buffer_front_right[i];
|
|
out.encoder_buffer_back_left[i + 1] = out.encoder_buffer_back_left[i];
|
|
out.encoder_buffer_back_right[i + 1] = out.encoder_buffer_back_right[i];
|
|
}
|
|
|
|
uint64_t current_time_ms = millis(); // Milliseconds since pico startup, using 64 bit value for safety
|
|
out.encoder_buffer_times_ms[0] = current_time_ms;
|
|
out.encoder_buffer_front_left[0] = front_left_encoder;
|
|
out.encoder_buffer_front_right[0] = front_right_encoder;
|
|
out.encoder_buffer_back_left[0] = back_left_encoder;
|
|
out.encoder_buffer_back_right[0] = back_right_encoder;
|
|
|
|
if(current_time_ms == 0) { // Set current_time_ms to a nonzero value if it is zero to prevent a division by zero later in the function
|
|
current_time_ms = 1;
|
|
}
|
|
|
|
// Calculate the speed in degrees per second of each of the steering motors
|
|
out.front_left_measured_spin_speed = 0.0f;
|
|
out.front_right_measured_spin_speed = 0.0f;
|
|
out.back_left_measured_spin_speed = 0.0f;
|
|
out.back_right_measured_spin_speed = 0.0f;
|
|
for(int i = 1; i < ENCODER_BUFFER_ENTRY_COUNT; i++)
|
|
{
|
|
out.front_left_measured_spin_speed += ( (float) abs(out.encoder_buffer_front_left[0] - out.encoder_buffer_front_left[i]) / (float) (current_time_ms - out.encoder_buffer_times_ms[i]) );
|
|
out.front_right_measured_spin_speed += ( (float) abs(out.encoder_buffer_front_right[0] - out.encoder_buffer_front_right[i]) / (float) (current_time_ms - out.encoder_buffer_times_ms[i]) );
|
|
out.back_left_measured_spin_speed += ( (float) abs(out.encoder_buffer_back_left[0] - out.encoder_buffer_back_left[i]) / (float) (current_time_ms - out.encoder_buffer_times_ms[i]) );
|
|
out.back_right_measured_spin_speed += ( (float) abs(out.encoder_buffer_back_right[0] - out.encoder_buffer_back_right[i]) / (float) (current_time_ms - out.encoder_buffer_times_ms[i]) );
|
|
}
|
|
|
|
out.front_left_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);
|
|
out.front_right_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);
|
|
out.back_left_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);
|
|
out.back_right_measured_spin_speed /= (float) (ENCODER_BUFFER_ENTRY_COUNT - 1);
|
|
|
|
// Calculate the current orientation of each drive wheel in degrees
|
|
out.front_left_spin_angle = ((float) (front_left_encoder - out.front_left_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
|
|
out.front_right_spin_angle = ((float) (front_right_encoder - out.front_right_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
|
|
out.back_left_spin_angle = ((float) (back_left_encoder - out.back_left_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
|
|
out.back_right_spin_angle = ((float) (back_right_encoder - out.back_right_initial_spin_encoder)) / STEERING_ENCODER_TICKS_PER_DEGREE;
|
|
|
|
return out;
|
|
}
|
|
|
|
int normalizeEncoderData(int encoder_data) // Normalize encoder data to a range of [0, STEERING_ENCODER_TICKS_PER_ROTATION)
|
|
{
|
|
encoder_data %= (int) STEERING_ENCODER_TICKS_PER_ROTATION;
|
|
encoder_data += (STEERING_ENCODER_TICKS_PER_ROTATION * (encoder_data < 0));
|
|
return encoder_data;
|
|
}
|
|
|
|
float relativeAngleFromEncoder(int encoder_data_relative ) // Calculate the relative angle from the difference between 2 encoder data points
|
|
{
|
|
return ((float) normalizeEncoderData(encoder_data_relative)) / ((float) STEERING_ENCODER_TICKS_PER_DEGREE);
|
|
}
|
|
|
|
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);
|
|
angle += (360 * (angle < 0.0));
|
|
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;
|
|
|
|
out = setMotorCoefficients(out, 1.0, 1.0, 1.0, 1.0); // Set the motor speed coefficients to 1 for all motors
|
|
out = setDriveTargetPower(out, 0.0); // Set the power
|
|
return out;
|
|
}
|
|
|
|
swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle) // Implementation for translation drive mode
|
|
{
|
|
swerve_drive out = input;
|
|
|
|
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 = setDriveTargetPower(out, target_speed); // Set the power
|
|
return out;
|
|
}
|
|
|
|
swerve_drive rotationDrive(swerve_drive input, float target_speed) // Implementation for rotation drive mode (rotating in place), positive speed is clockwise, negative speed is counterclockwise
|
|
{
|
|
swerve_drive out = input;
|
|
|
|
//float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle
|
|
|
|
out = setTargetSpin(out, 45.0, 135.0, 315.0, 225.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 basicDrive(swerve_drive input, float target_speed, float target_angle) // Implementation for basic drive mode
|
|
{
|
|
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
|
|
|
|
// 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 = 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
|
|
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(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 = 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 * 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;
|
|
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_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;
|
|
}
|
|
|
|
// 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 = 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_quadrant) {
|
|
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
|
|
drive_condition = DRIVE_BASIC_STOP;
|
|
break;
|
|
}
|
|
return drive_condition;
|
|
}
|
|
|
|
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;
|
|
|
|
float normalized_turn_angle = normalizeAngle(turn_angle); // Normalize the turn angle
|
|
|
|
// Calculate the speed of each motor
|
|
float angle_distance_from_y_axis = 90.0f - fabs(fmod(normalized_turn_angle, 180.0f) - 90.0f); // Distance in degrees that the joystick is from the y-axis
|
|
float slow_side_coefficient = angle_distance_from_y_axis / 90.0f; // Coefficient applied to whichever motor is not going full speed in the tank drive
|
|
int turn_angle_quadrant = ((int) normalized_turn_angle) / 4;
|
|
float left_side_power, right_side_power;
|
|
|
|
// Determine the motor speeds based on the quadrant that the joystick is in
|
|
switch(turn_angle_quadrant) {
|
|
case 0: // Going forward and turning right, Northeast quadrant
|
|
left_side_power = 1.0f;
|
|
right_side_power = slow_side_coefficient;
|
|
break;
|
|
case 1: // Going backward and turning left, Southeast quadrant
|
|
left_side_power = -1.0f;
|
|
right_side_power = -slow_side_coefficient;
|
|
break;
|
|
case 2: // Going backward and turning right, Southwest quadrant
|
|
left_side_power = -slow_side_coefficient;
|
|
right_side_power = -1.0f;
|
|
break;
|
|
case 3: // Going forward and turning left, Northwest quadrant
|
|
left_side_power = slow_side_coefficient;
|
|
right_side_power = 1.0f;
|
|
break;
|
|
}
|
|
|
|
out = setMotorCoefficients(out, left_side_power, right_side_power, left_side_power, right_side_power); // Set the motor speed coefficients
|
|
out = setDriveTargetPower(out, target_speed); // Set the 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:
|
|
Left and Right shoulder buttons:
|
|
Switch between drive modes (Left for basic, right for translation)
|
|
D-Pad:
|
|
Reset drive orientation relative to body
|
|
Set direction to the corresponding d-pad button direction
|
|
Joystick B:
|
|
Left/Right: Rotation speed, bypass existing drive mode and start rotating
|
|
|
|
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
|
|
|
|
All Mode Controls:
|
|
|
|
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)
|
|
\\ \\
|
|
|
|
// //
|
|
|
|
*/
|