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

Mostly complete swerve.cpp , add manipulator.cpp

Browse Source
This commit is contained in:
evlryah 2023-09-27 21:14:09 -05:00
parent 05b28b578a
commit 9a29c468ba
6 changed files with 157 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/globals.h
View File

@ -18,6 +18,8 @@ extern long last_p;
#define DEGREES_PER_RADIAN (180.0 / M_PI)
#define RADIANS_PER_DEGREE (M_PI / 180.0)
#define MOTOR_MAX_POWER 127.0 // Highest value accepted by motor control functions
// Drive modes
#define DRIVE_BASIC 0
#define DRIVE_TRANSLATION 1
@ -32,15 +34,19 @@ extern long last_p;
#define DRIVE_BASIC_BACKLEFT 5
#define DRIVE_BASIC_BACKRIGHT 6
// 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
// Length of the buffer to monitor recent steering 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 steering motors' current speeds
// This value must always be at least 2, otherwise the code will break due to there being an array with a zero or negative length or a division by zero
#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
// Number of encoder ticks per degree of rotation for the swerve drive steering motors
#define STEERING_ENCODER_TICKS_PER_DEGREE 1.0 // TODO as of 20230927
// Maximum speed allowed for the steering motors (out of 127.0)
#define STEERING_MOTOR_SPEED_LIMIT 127.0 // TODO as of 20230927, lower this if they're spinning too fast for the robot to handle
// Start decelerating the steering motors linearly when they're within this many degrees of their target angle
#define STEERING_SLOW_DELTA 30.0
#define SerComm Serial1 //Serial port connected to Xbee
#define DIAMOND_LEFT 0

19
src/main.cpp
View File

@ -860,6 +860,11 @@ void print_status() {
}
void print_encoder_positions() { // Print encoder positions for steering motors
char buffer[200];
sprintf(buffer, "ENC1 = %i\nENC2 = %i\nENC3 = $i\nENC4 = %i\n", enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount());
SerComm.print(buffer);
}
void loop() {
rp2040.wdt_reset();
@ -904,13 +909,17 @@ void loop() {
if (drive_mode == DRIVE_ROTATION) {
swrv = rotationDrive(swrv, zeroed_rx);
}
swrv = updateSwerveCommand(swrv);
swrv = updateEncoderData(swrv, enc2.getCount(), enc1.getCount(), enc4.getCount(), enc3.getCount()); // Update encoder data in the swerve_drive struct
swrv = updateSwerveCommand(swrv); // Calculate power for each drive and steering motor
print_encoder_positions(); // DEBUG ONLY, print steering motor encoder positions
// update motors after calculation
set_motor(FLSTEER, swrv.front_left_command_spin);
set_motor(BRSTEER, swrv.back_right_command_spin);
set_motor(FRSTEER, swrv.front_right_command_spin);
set_motor(BLSTEER, swrv.back_left_command_spin);
set_motor(FLSTEER, swrv.front_left_spin_power);
set_motor(BRSTEER, swrv.back_right_spin_power);
set_motor(FRSTEER, swrv.front_right_spin_power);
set_motor(BLSTEER, swrv.back_left_spin_power);
set_motor(FLDRIVE, swrv.front_left_power);
set_motor(BRDRIVE, swrv.back_right_power);

3
src/manipulator.cpp Normal file
View File

@ -0,0 +1,3 @@
#include <Arduino.h>
#include "globals.h"
#include "manipulator.h"

6
src/manipulator.h Normal file
View File

@ -0,0 +1,6 @@
#include "globals.h"
typedef struct {
} manipulator_arm;

185
src/swerve.cpp
View File

@ -3,25 +3,19 @@
#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));
}
// 3-mode drive control system based on https://compendium.readthedocs.io/en/latest/tasks/drivetrains/swerve.html
float closestAngle(float current, float target)
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
{
while(current > 360) {
current -= 360.0;
}
current = 360 - current;
while(target > 360) {
target -= 360.0;
}
target = 360 - target;
// get direction
float dir = target - current;
float dir = normalizeAngle(target) - normalizeAngle(current);
// convert from -360 to 360 to -180 to 180
if (abs(dir) > 180.0)
@ -45,112 +39,101 @@ swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encod
}
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
// TODO partially complete as of 20230927, need encoder access and bounds for stepper motors
// TODO partially complete as of 20230927
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
// 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];
}
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;
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;
// 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
// 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);
out.front_left_spin_power = calculateSteeringMotorSpeed(front_left_delta);
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
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;
}
// TODO, get encoder values for each drive motor and place them in the arrays
// 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 /* || TODO: difference between current spin and target spin is above a certain number of degrees */)
{ // 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)
out.front_left_power = 0.0f;
out.front_right_power = 0.0f;
out.back_left_power = 0.0f;
out.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;
// Set the new drive motor power, apply coefficients, set between -127.0 and 127.0
out.front_left_power = out.current_drive_power * out.front_left_coefficient * MOTOR_MAX_POWER;
out.front_right_power = out.current_drive_power * out.front_right_coefficient * MOTOR_MAX_POWER;
out.back_left_power = out.current_drive_power * out.back_left_coefficient * MOTOR_MAX_POWER;
out.back_right_power = out.current_drive_power * out.back_right_coefficient * MOTOR_MAX_POWER;
return out;
}
/*
// 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)
float calculateSteeringMotorSpeed(float steering_delta) // Calculate the speed of a steering motor based on its distance from its target angle
{
swerve_drive out = input;
float currentAngle = input.current_spin_location;
// find closest angle to setpoint
float setpointAngle = closestAngle(currentAngle, setpoint);
// find closest angle to setpoint + 180
float setpointAngleFlipped = closestAngle(currentAngle, setpoint + 180.0);
// if the closest angle to setpoint is shorter
if (abs(setpointAngle) <= abs(setpointAngleFlipped))
{
// unflip the motor direction use the setpoint
out.target_spin_direction = CLOCKWISE;
out.target_spin_location = (currentAngle + setpointAngle);
float abs_steering_delta = fabs(steering_delta);
if(abs_steering_delta > STEERING_SLOW_DELTA) { // In full speed range, still far enough away from the target angle
return STEERING_MOTOR_SPEED_LIMIT;
} else { // Slow down the speed of the steering motor since it's close to its target angle
return STEERING_MOTOR_SPEED_LIMIT * (1.0f - (abs_steering_delta / STEERING_SLOW_DELTA));
}
// if the closest angle to setpoint + 180 is shorter
else
{
// flip the motor direction and use the setpoint + 180
out.target_spin_direction = ANTICLOCKWISE;
out.target_spin_location = (currentAngle + setpointAngleFlipped);
}
return out;
}
*/
swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int front_right_encoder, int back_left_encoder, int back_right_encoder) // Load new encoder data into the swerve_drive struct, calculate the speed 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);
return out;
}
swerve_drive translationDrive(swerve_drive input, float target_speed, float target_angle) // Implementation for translation drive mode
{

58
src/swerve.h
View File

@ -9,35 +9,47 @@ typedef struct { // swerve_drive struct, used to track and manage the state of t
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 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; // 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 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, read from encoders & convert
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;
// 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 wheel orientations relative to robot body, this is what the motors are currently being told to do
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;
// 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, this is the state that the robot is trying to get to
// 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;
@ -47,9 +59,9 @@ typedef struct { // swerve_drive struct, used to track and manage the state of t
// 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
// 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];
@ -58,7 +70,7 @@ typedef struct { // swerve_drive struct, used to track and manage the state of t
} swerve_drive;
float closestAngle(float current, float target);
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);
@ -67,6 +79,10 @@ swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encod
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); // Load new encoder data into the swerve_drive struct, calculate the speed of the steering 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