diff --git a/src/globals.h b/src/globals.h index 4d63d41..39a3e70 100644 --- a/src/globals.h +++ b/src/globals.h @@ -27,10 +27,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 diff --git a/src/main.cpp b/src/main.cpp index d0dfc11..cb72e7b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -948,15 +948,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, zeroed_ly_float, zeroed_lx_float); + swrv.enable_steering = false; break; } swrv = updateEncoderData(swrv, enc1.getCount(), enc2.getCount(), enc3.getCount(), enc4.getCount()); // Update encoder data in the swerve_drive struct diff --git a/src/swerve.cpp b/src/swerve.cpp index 6d07381..11b4621 100644 --- a/src/swerve.cpp +++ b/src/swerve.cpp @@ -12,6 +12,8 @@ template 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,19 @@ 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; - } - 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 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; - out.front_left_coefficient = front_left; - out.front_right_coefficient = front_right; - out.back_left_coefficient = back_left; - out.back_right_coefficient = back_right; + + //float normalized_target_angle = normalizeAngle(target_angle); // Normalize the target angle + + out = setTargetSpin(out, 45.0, 135.0, 225.0, 315.0); // Set the target angle for each rotation motor + out = setMotorCoefficients(out, 1.0, 1.0, 1.0, 1.0); // Set the motor speed coefficients to 1 for all motors + 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 /* diff --git a/src/swerve.h b/src/swerve.h index 4a608e3..fbc28fa 100644 --- a/src/swerve.h +++ b/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