Mostly complete updateSwerveCommand(), add swerve drive setup code
This commit is contained in:
evlryah
parent
e12337eb16
commit
6b778e2321
@ -34,9 +34,13 @@ extern long last_p;
|
||||
|
||||
// Length of the buffer to monitor recent encoder positions to calculate speed
|
||||
// The buffer will track the last N states of the encoders, and the times at which they were recorded, to determine the robot's current speed
|
||||
// This value must always be at least 1, otherwise the code will break due to there being an array with a zero or negative length
|
||||
#define ENCODER_BUFFER_ENTRY_COUNT 5
|
||||
|
||||
|
||||
// Don't change the spin of the wheels if the speed is above MAX_SPEED_WHEEL_ROTATE encoder ticks per second
|
||||
// If the speed is above this point, limit the power supplied to the spin motors to MAX_FAST_SPIN_POWER
|
||||
#define MAX_SPEED_WHEEL_SPIN 0.0f // TODO as of 20230927
|
||||
#define MAX_FAST_SPIN_POWER 0 // TODO as of 20230927
|
||||
|
||||
#define SerComm Serial1 //Serial port connected to Xbee
|
||||
#define DIAMOND_LEFT 0
|
||||
|
||||
23
src/main.cpp
23
src/main.cpp
@ -820,8 +820,13 @@ void setup() {
|
||||
//dumbdisplay.configAutoPin(DD_AP_VERT); // auto vertical pin layout
|
||||
|
||||
|
||||
|
||||
|
||||
// Initialize the swerve_drive struct
|
||||
// TODO as of 20230927: get encoder values for the correct motors here
|
||||
int front_left_encoder;
|
||||
int front_right_encoder;
|
||||
int back_left_encoder;
|
||||
int back_right_encoder;
|
||||
swrv = initializeSwerveDrive(front_left_encoder, front_right_encoder, back_left_encoder, back_right_encoder);
|
||||
|
||||
swrv.target_drive_power = 0;
|
||||
swrv.current_drive_power = 0; // don't want the robot to move right away here
|
||||
@ -891,13 +896,13 @@ void loop() {
|
||||
|
||||
|
||||
if (drive_mode == DRIVE_BASIC) {
|
||||
swrv = tempBasicDrive(swrv, sqrt(zeroed_lx^2 + zeroed_ly^2), atan2(zeroed_ly, zeroed_lx));
|
||||
swrv = basicDrive(swrv, sqrt(zeroed_lx^2 + zeroed_ly^2), atan2(zeroed_ly, zeroed_lx));
|
||||
}
|
||||
if (drive_mode == DRIVE_TRANSLATION) {
|
||||
swrv = tempTranslationDrive(swrv, sqrt(zeroed_lx^2 + zeroed_ly^2), atan2(zeroed_ly, zeroed_lx));
|
||||
swrv = translationDrive(swrv, sqrt(zeroed_lx^2 + zeroed_ly^2), atan2(zeroed_ly, zeroed_lx));
|
||||
}
|
||||
if (drive_mode == DRIVE_ROTATION) {
|
||||
swrv = tempRotationDrive(swrv, zeroed_rx);
|
||||
swrv = rotationDrive(swrv, zeroed_rx);
|
||||
}
|
||||
swrv = updateSwerveCommand(swrv);
|
||||
|
||||
@ -907,10 +912,10 @@ void loop() {
|
||||
set_motor(FRSTEER, swrv.front_right_command_spin);
|
||||
set_motor(BLSTEER, swrv.back_left_command_spin);
|
||||
|
||||
set_motor(FLDRIVE, swrv.current_drive_power);
|
||||
set_motor(BRDRIVE, swrv.current_drive_power);
|
||||
set_motor(FRDRIVE, swrv.current_drive_power);
|
||||
set_motor(BLDRIVE, swrv.current_drive_power);
|
||||
set_motor(FLDRIVE, swrv.front_left_power);
|
||||
set_motor(BRDRIVE, swrv.back_right_power);
|
||||
set_motor(FRDRIVE, swrv.front_right_power);
|
||||
set_motor(BLDRIVE, swrv.back_left_power);
|
||||
|
||||
|
||||
// Goliath / 2 side arcade tank drive code below
|
||||
|
||||
123
src/swerve.cpp
123
src/swerve.cpp
@ -2,6 +2,7 @@
|
||||
#include "globals.h"
|
||||
#include "swerve.h"
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
template <typename T> int signum(T val) {
|
||||
return (T(0) < val) - (val < T(0));
|
||||
@ -30,27 +31,95 @@ float closestAngle(float current, float target)
|
||||
return dir;
|
||||
}
|
||||
|
||||
// TODO as of 20230926
|
||||
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
|
||||
|
||||
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) {
|
||||
// TODO partially complete as of 20230927, need encoder access and bounds for stepper motors
|
||||
swerve_drive updateSwerveCommand(swerve_drive input)
|
||||
{
|
||||
swerve_drive out = input;
|
||||
|
||||
// Monitor encoder values for each wheel and place them in the encoder_buffer arrays in the swerve_drive struct
|
||||
// TODO, get encoder values for each drive motor and place them in the arrays
|
||||
// TODO, get time in milliseconds (either unix millis or relative millis to pico startup)
|
||||
|
||||
// Calculate the robot's average speed over the last few encoder samples
|
||||
// TODO
|
||||
// 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 = 1; i < ENCODER_BUFFER_ENTRY_COUNT; 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];
|
||||
}
|
||||
|
||||
// Determine how much it is safe to change the orientation of the wheels, given the robot's current speed
|
||||
// TODO
|
||||
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;
|
||||
|
||||
// Update every "current" variable in the swerve_drive struct with new data
|
||||
// TODO
|
||||
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;
|
||||
}
|
||||
|
||||
// TODO, get encoder values for each drive motor and place them in the arrays
|
||||
|
||||
// Set the new commanded orientation of the rotation motors and the new commanded drive motor speed
|
||||
// TODO
|
||||
|
||||
// Calculate the robot's average speed over the last few encoder samples, calculate the speed since each individual measurement and average them across all 4 wheels
|
||||
float robot_current_measured_speed; // Robot's average speed over recent encoder samples in encoder ticks per second
|
||||
float front_left_measured_speed = 0.0f;
|
||||
float front_right_measured_speed = 0.0f;
|
||||
float back_left_measured_speed = 0.0f;
|
||||
float back_right_measured_speed = 0.0f;
|
||||
for(int i = 1; i < ENCODER_BUFFER_ENTRY_COUNT; i++)
|
||||
{
|
||||
front_left_measured_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]) );
|
||||
front_right_measured_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]) );
|
||||
back_left_measured_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]) );
|
||||
back_right_measured_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]) );
|
||||
}
|
||||
// Average the speed of all 4 wheels over all samples
|
||||
// Divide by 1000 to convert milliseconds into seconds
|
||||
robot_current_measured_speed = fabs( (front_left_measured_speed + front_right_measured_speed + back_left_measured_speed + back_right_measured_speed) / (float) (4 * (ENCODER_BUFFER_ENTRY_COUNT - 1)) );
|
||||
|
||||
// Determine how quickly it is safe to change the orientation of the wheels, given the robot's current speed
|
||||
if(robot_current_measured_speed <= MAX_SPEED_WHEEL_SPIN && )
|
||||
{ // Robot is going below maximum speed to significantly spin wheels
|
||||
// TODO as of 20230927 : set stepper motor power and target
|
||||
|
||||
} else { // Robot is going above the maximum speed to significantly spin wheels at full power, and the wheels are being commanded to spin significantly, so limit the power supplied to the spin stepper motors
|
||||
// TODO as of 20230927 : set stepper motor power and target, limit power due to high body speed, decelerate to allow robot to modify steering safely (set motor power to 0)
|
||||
front_left_power = 0.0f;
|
||||
front_right_power = 0.0f;
|
||||
back_left_power = 0.0f;
|
||||
back_right_power = 0.0f;
|
||||
}
|
||||
|
||||
// Set the new commanded orientation of the spin stepper motors
|
||||
// TODO as of 20230927
|
||||
// If the target AND current motor power is 0, then don't modify the wheel spin
|
||||
if(out.target_drive_power != 0.0f || out.current_drive_power != 0.0f) {
|
||||
// Set stepper motor position and power
|
||||
}
|
||||
// TODO for evlryah: how is the power/speed of the stepper motors controlled, and is it possible to observe the current in-progress location of a stepper motor?
|
||||
|
||||
// Set the new commanded drive motor speed, apply coefficients
|
||||
out.front_left_power = out.current_drive_power * out.front_left_coefficient;
|
||||
out.front_right_power = out.current_drive_power * out.front_right_coefficient;
|
||||
out.back_left_power = out.current_drive_power * out.back_left_coefficient;
|
||||
out.back_right_power = out.current_drive_power * out.back_right_coefficient;
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
@ -82,7 +151,7 @@ swerve_drive setDirection(swerve_drive input, float setpoint)
|
||||
}
|
||||
*/
|
||||
|
||||
swerve_drive tempTranslationDrive(swerve_drive input, float target_speed, float target_angle) // Temporary implementation for translation drive mode
|
||||
swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle) // Implementation for translation drive mode
|
||||
{
|
||||
swerve_drive out = input;
|
||||
|
||||
@ -94,11 +163,7 @@ swerve_drive tempTranslationDrive(swerve_drive input, float target_speed, float
|
||||
return out;
|
||||
}
|
||||
|
||||
/*
|
||||
TODO for tempRotationDrive() as of 20230923: add functionality to track the robot body's rotation relative to the field through an IMU or encoders somehow,
|
||||
and allow the option to modify the spin offset to compensate for it.
|
||||
*/
|
||||
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 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;
|
||||
|
||||
@ -111,11 +176,7 @@ swerve_drive tempRotationDrive(swerve_drive input, float target_speed) // Tempor
|
||||
return out;
|
||||
}
|
||||
|
||||
/*
|
||||
TODO for tempBasicDrive() as of 20230923: add functionality to track the robot body's rotation relative to the field through an IMU or encoders somehow,
|
||||
and allow the option to modify the spin offset to compensate for it.
|
||||
*/
|
||||
swerve_drive tempBasicDrive(swerve_drive input, float target_speed, float target_angle) // Temporary implementation for basic drive mode
|
||||
swerve_drive basicDrive(swerve_drive input, float target_speed, float target_angle) // Implementation for basic drive mode
|
||||
{
|
||||
swerve_drive out = input;
|
||||
|
||||
@ -306,15 +367,13 @@ Variables to monitor and control:
|
||||
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):
|
||||
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:
|
||||
@ -326,9 +385,7 @@ Basic Mode Controls:
|
||||
Vector Amplitude: Drive speed
|
||||
Vector Angle: Drive angle
|
||||
|
||||
Rotation Mode Controls:
|
||||
Joystick A:
|
||||
Left/Right: Rotation speed
|
||||
All Mode Controls:
|
||||
|
||||
Rotation drive mode wheel orientation
|
||||
Turning in place
|
||||
|
||||
55
src/swerve.h
55
src/swerve.h
@ -1,4 +1,5 @@
|
||||
#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
|
||||
@ -6,44 +7,50 @@ 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
|
||||
|
||||
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)
|
||||
int drive_mode = DRIVE_BASIC;
|
||||
|
||||
float target_drive_power = 127.0; // 0.0 - 127.0 : TARGET power that the robot is trying to get to
|
||||
float current_drive_power = 127.0; // 0.0 - 127.0 : CURRENT power being given to drive motors (before coefficient is applied)
|
||||
|
||||
float front_left_power = 127.0; // 0.0 - 127.0 : CURRENT power for this specific drive motor
|
||||
float front_right_power = 127.0; // 0.0 - 127.0 : CURRENT power for this specific drive motor
|
||||
float back_left_power = 127.0; // 0.0 - 127.0 : CURRENT power for this specific drive motor
|
||||
float back_right_power = 127.0; // 0.0 - 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, 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;
|
||||
float front_left_current_spin = 0.0f;
|
||||
float front_right_current_spin = 0.0f;
|
||||
float back_left_current_spin = 0.0f;
|
||||
float back_right_current_spin = 0.0f;
|
||||
|
||||
// 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;
|
||||
float front_left_command_spin = 0.0f;
|
||||
float front_right_command_spin = 0.0f;
|
||||
float back_left_command_spin = 0.0f;
|
||||
float back_right_command_spin = 0.0f;
|
||||
|
||||
// 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;
|
||||
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;
|
||||
|
||||
// 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;
|
||||
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 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)
|
||||
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];
|
||||
@ -56,13 +63,15 @@ 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 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
|
||||
|
||||
swerve_drive tempTranslationDrive(swerve_drive input, float target_speed, float target_angle); // Temporary implementation for translation drive mode
|
||||
swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle); // 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 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 tempBasicDrive(swerve_drive input, float target_speed, float target_angle); // Temporary implementation for basic drive mode
|
||||
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
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user