Organize swerve code, add tank drive mode
This commit is contained in:
evlryah
parent
70bfc17aca
commit
4ee423ec9a
@ -15,7 +15,8 @@ extern long last_p;
|
||||
*/
|
||||
|
||||
// Loop timing
|
||||
#define LOOP_DELAY_MS 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
|
||||
#define LOOP_DELAY_MS_CORE_0 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
|
||||
#define LOOP_DELAY_MS_CORE_1 50 // Minimum milliseconds between the start of each loop, accounting for processing time during each loop
|
||||
#define LOOP_DELAY_SECONDS ((float)LOOP_DELAY_MS / 1000.0f) // Previous number expressed in seconds
|
||||
|
||||
// Math things
|
||||
@ -27,10 +28,17 @@ extern long last_p;
|
||||
#define MOTOR_MAX_POWER 127.0 // Highest value accepted by motor control functions
|
||||
|
||||
// Drive modes
|
||||
#define DRIVE_STOP 0
|
||||
#define DRIVE_BASIC 1
|
||||
#define DRIVE_TRANSLATION 2
|
||||
#define DRIVE_ROTATION 3
|
||||
#define DRIVE_STOP 0
|
||||
#define DRIVE_BASIC 1
|
||||
#define DRIVE_TRANSLATION 2
|
||||
#define DRIVE_ROTATION 3
|
||||
#define DRIVE_TANK 4
|
||||
|
||||
// Default drive mode, currently default to stopping upon initialization
|
||||
#define DEFAULT_SWERVE_DRIVE_MODE DRIVE_STOP
|
||||
|
||||
// Whether to enable swerve drive steering by default (set this to false if the robot is locked to tank drive)
|
||||
#define DEFAULT_SWERVE_ENABLE_STEERING true
|
||||
|
||||
// Controller maximum inputs for joystick
|
||||
#define CONTROLLER_JOYSTICK_MAX 128.0
|
||||
|
||||
24
src/main.cpp
24
src/main.cpp
@ -72,6 +72,7 @@ int stepperY_pos = 0;
|
||||
int stepperZ_pos = 0;
|
||||
|
||||
uint64_t previous_loop_start_time_core_0 = 0, previous_loop_start_time_core_1 = 0; // Track the previous loop start time, used to calculate how long to sleep for until the start of the next loop
|
||||
uint64_t previous_loop_processing_duration_core_0 = 0, previous_loop_processing_duration_core_1 = 0; // Processing time for the previous loop on each core
|
||||
uint64_t loop_counter_core_0 = 0, loop_counter_core_1 = 0; // Counter for loop() and loop1() respectively, incremented at the end of each loop
|
||||
|
||||
#define defMaxSpeed 8000
|
||||
@ -948,15 +949,23 @@ void loop() {
|
||||
switch(loop_drive_mode) {
|
||||
case DRIVE_STOP:
|
||||
swrv = stopSwerve(swrv);
|
||||
swrv.enable_steering = false;
|
||||
break;
|
||||
case DRIVE_BASIC:
|
||||
swrv = basicDrive(swrv, left_joystick_magnitude, left_joystick_angle);
|
||||
swrv.enable_steering = true;
|
||||
break;
|
||||
case DRIVE_TRANSLATION:
|
||||
swrv = translationDrive(swrv, left_joystick_magnitude, left_joystick_angle);
|
||||
swrv.enable_steering = true;
|
||||
break;
|
||||
case DRIVE_ROTATION:
|
||||
swrv = rotationDrive(swrv, zeroed_rx_float);
|
||||
swrv.enable_steering = true;
|
||||
break;
|
||||
case DRIVE_TANK:
|
||||
swrv = tankDrive(swrv, left_joystick_angle, left_joystick_magnitude);
|
||||
swrv.enable_steering = false;
|
||||
break;
|
||||
}
|
||||
swrv = updateEncoderData(swrv, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount()); // Update encoder data in the swerve_drive struct
|
||||
@ -1153,8 +1162,9 @@ void loop() {
|
||||
}*/
|
||||
//delay(200);
|
||||
|
||||
int64_t delay_time_ms = LOOP_DELAY_MS - (int64_t) (millis() - previous_loop_start_time_core_0); // Dynamically calculate delay time
|
||||
if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS
|
||||
previous_loop_processing_duration_core_0 = millis() - previous_loop_start_time_core_0;
|
||||
int64_t delay_time_ms = LOOP_DELAY_MS_CORE_0 - (int64_t) previous_loop_processing_duration_core_0; // Dynamically calculate delay time
|
||||
if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS_CORE_0
|
||||
delay(delay_time_ms);
|
||||
}
|
||||
loop_counter_core_0++;
|
||||
@ -1174,7 +1184,7 @@ void loop1() {
|
||||
}
|
||||
else {
|
||||
delay(25);
|
||||
loop_counter_core_1++;
|
||||
//loop_counter_core_1++;
|
||||
}
|
||||
|
||||
//SerComm.println("update");
|
||||
@ -1244,5 +1254,11 @@ void loop1() {
|
||||
digitalWrite(2, HIGH);
|
||||
digitalWrite(3, HIGH);*/
|
||||
|
||||
delay(25);
|
||||
|
||||
previous_loop_processing_duration_core_1 = millis() - previous_loop_start_time_core_1;
|
||||
int64_t delay_time_ms = LOOP_DELAY_MS_CORE_1 - (int64_t) previous_loop_processing_duration_core_1; // Dynamically calculate delay time
|
||||
if(delay_time_ms > 0 && delay_time_ms < 100) { // Only delay if the processing time has not exceeded LOOP_DELAY_MS_CORE_0
|
||||
delay(delay_time_ms);
|
||||
}
|
||||
loop_counter_core_1++;
|
||||
}
|
||||
|
||||
126
src/swerve.cpp
126
src/swerve.cpp
@ -12,6 +12,8 @@ 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
|
||||
@ -49,13 +51,12 @@ swerve_drive initializeSwerveDrive(int front_left_encoder, int front_right_encod
|
||||
}
|
||||
|
||||
// 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
|
||||
swerve_drive updateSwerveCommand(swerve_drive input)
|
||||
{
|
||||
swerve_drive out = input;
|
||||
|
||||
// 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
|
||||
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);
|
||||
@ -65,7 +66,7 @@ swerve_drive updateSwerveCommand(swerve_drive input)
|
||||
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
|
||||
} 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;
|
||||
@ -145,6 +146,52 @@ swerve_drive updateEncoderData(swerve_drive in, int front_left_encoder, int fron
|
||||
return out;
|
||||
}
|
||||
|
||||
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);
|
||||
if(angle < 0.0) {
|
||||
angle += 360;
|
||||
}
|
||||
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;
|
||||
@ -303,51 +350,44 @@ byte identifyBasicDriveCondition(float target_speed, float target_angle) // Iden
|
||||
return drive_condition;
|
||||
}
|
||||
|
||||
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);
|
||||
if(angle < 0.0) {
|
||||
angle += 360;
|
||||
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;
|
||||
}
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Modulus Plus: Drive control planning: v1 on 20230922
|
||||
/*
|
||||
|
||||
29
src/swerve.h
29
src/swerve.h
@ -7,7 +7,8 @@ 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
|
||||
|
||||
int drive_mode = DRIVE_BASIC;
|
||||
int drive_mode = DEFAULT_SWERVE_DRIVE_MODE;
|
||||
bool enable_steering = DEFAULT_SWERVE_ENABLE_STEERING;
|
||||
|
||||
float target_drive_power = 0.0f; // -127.0 to 127.0 : TARGET power that the robot is trying to get to
|
||||
float current_drive_power = 0.0f; // -127.0 to 127.0 : CURRENT power being given to drive motors (before coefficient is applied)
|
||||
@ -58,7 +59,6 @@ typedef struct { // swerve_drive struct, used to track and manage the state of t
|
||||
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
|
||||
@ -70,6 +70,8 @@ typedef struct { // swerve_drive struct, used to track and manage the state of t
|
||||
|
||||
} swerve_drive;
|
||||
|
||||
// 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
|
||||
|
||||
// TODO as of 20230923 for setDirection() : fix to work with modifications made to swerve_drive struct on 20230922
|
||||
@ -83,6 +85,19 @@ float calculateSteeringMotorSpeed(float steering_delta); // Calculate the speed
|
||||
|
||||
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
|
||||
|
||||
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
|
||||
|
||||
|
||||
// Drive mode functions
|
||||
|
||||
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
|
||||
@ -93,12 +108,4 @@ swerve_drive basicDrive(swerve_drive input, float target_speed, float target_ang
|
||||
|
||||
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
|
||||
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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user