105 lines
6.5 KiB
C
105 lines
6.5 KiB
C
#include "globals.h"
|
|
#include <stdint.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
|
|
|
|
int drive_mode = DRIVE_BASIC;
|
|
|
|
float target_drive_power = 127.0; // -127.0 to 127.0 : TARGET power that the robot is trying to get to
|
|
float current_drive_power = 127.0; // -127.0 to 127.0 : CURRENT power being given to drive motors (before coefficient is applied)
|
|
|
|
float front_left_power = 127.0; // -127.0 to 127.0 : CURRENT power for this specific drive motor
|
|
float front_right_power = 127.0; // -127.0 to 127.0 : CURRENT power for this specific drive motor
|
|
float back_left_power = 127.0; // -127.0 to 127.0 : CURRENT power for this specific drive motor
|
|
float back_right_power = 127.0; // -127.0 to 127.0 : CURRENT power for this specific drive motor
|
|
|
|
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 in degrees [0.0, 360.0), read from encoders & convert
|
|
float front_left_spin_angle = 0.0f;
|
|
float front_right_spin_angle = 0.0f;
|
|
float back_left_spin_angle = 0.0f;
|
|
float back_right_spin_angle = 0.0f;
|
|
|
|
// COMMAND steering motor power, -127.0 to 127.0
|
|
float front_left_spin_power = 0.0f;
|
|
float front_right_spin_power = 0.0f;
|
|
float back_left_spin_power = 0.0f;
|
|
float back_right_spin_power = 0.0f;
|
|
|
|
// TARGET wheel orientations relative to robot body in degrees [0.0, 360.0), this is the state that the robot is trying to get to
|
|
float front_left_target_spin = 0.0f;
|
|
float front_right_target_spin = 0.0f;
|
|
float back_left_target_spin = 0.0f;
|
|
float back_right_target_spin = 0.0f;
|
|
|
|
// MEASURED steering motor speed in degrees per second, positive values are clockwise, calculated each time updateEncoderData() is called
|
|
float front_left_measured_spin_speed = 0.0f;
|
|
float front_right_measured_spin_speed = 0.0f;
|
|
float back_left_measured_spin_speed = 0.0f;
|
|
float back_right_measured_spin_speed = 0.0f;
|
|
|
|
// Initial encoder values of the steering motors, current orientation of the steering motors is calculated from this (assuming that they are all facing forward upon initialization)
|
|
int front_left_initial_spin_encoder = 0;
|
|
int front_right_initial_spin_encoder = 0;
|
|
int back_left_initial_spin_encoder = 0;
|
|
int back_right_initial_spin_encoder = 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.0f;
|
|
float front_right_coefficient = 0.0f;
|
|
float back_left_coefficient = 0.0f;
|
|
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
|
|
uint64_t encoder_buffer_times_ms[ENCODER_BUFFER_ENTRY_COUNT]; // Buffer tracks the times at which encoder states were measures, uses milliseconds (relative to pico start), using 64 bit value for safety
|
|
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); // 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
|
|
// swerve_drive setDirection(swerve_drive input, float setpoint);
|
|
|
|
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 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
|
|
|
|
float calculateSteeringMotorSpeed(float steering_delta); // Calculate the speed of a steering motor based on its distance from its target angle
|
|
|
|
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 stopSwerve(swerve_drive input); // Stop all motors
|
|
|
|
swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle); // Implementation for translation drive mode
|
|
|
|
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 basicDrive(swerve_drive input, float target_speed, float target_angle); // 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
|