78 lines
4.7 KiB
C
78 lines
4.7 KiB
C
#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_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_drive tempTranslationDrive(swerve_drive input, float target_speed, float target_angle); // Temporary implementation for translation drive mode
|
|
|
|
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_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_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
|