From c2b6657641dfb15f9c52ad58698e6b6e4413f5b8 Mon Sep 17 00:00:00 2001 From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com> Date: Sat, 27 Jan 2024 13:47:07 -0800 Subject: [PATCH] simplify setting of states by deleting specific drive set desired state --- .../SwerveDriveJoystickControl.java | 2 +- .../SwerveDrivePS4Control.java | 2 +- .../robot/subsystems/SwerveDrivetrain.java | 25 -------------- .../frc/robot/subsystems/SwerveModule.java | 33 ------------------- 4 files changed, 2 insertions(+), 60 deletions(-) diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java index f29d0b9..816f42f 100644 --- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java +++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java @@ -122,7 +122,7 @@ public void execute() { SmartDashboard.putNumber("PoseX", robotPose.getX()); SmartDashboard.putNumber("PoseDegrees", robotPose.getRotation().getDegrees()); - drivetrain.setDesiredStateDrive(speeds, isFieldRelative); + drivetrain.setDesiredState(speeds, isFieldRelative); } /** diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java index cb040d0..5bf3971 100644 --- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java +++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java @@ -67,7 +67,7 @@ public void execute(){ rightX ); - drivetrain.setDesiredStateDrive(speeds, false); + drivetrain.setDesiredState(speeds, false); } /** diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java index 7f3e85f..8cd30e3 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java @@ -17,7 +17,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.SubsystemBase; /** @@ -221,30 +220,6 @@ public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative) { setDesiredState(speeds); } - /** - * Set speeds of robot when driving. This is for driving, so speeds should be in motor power, not in meters per second - * - * @param speeds Desired speeds of drivetrain (using swerve modules). IMPORTANT: These speeds our in motor power, not in meters per second! That means all values in speeds should be from -1 to 1 - * @param fieldRelative True if the robot is using a field relative coordinate system, false if using a robot relive coordinate system. If field relative, forward will be directly away from driver, no matter the rotation of the robot. - */ - public void setDesiredStateDrive(ChassisSpeeds speeds, boolean fieldRelative) { - speeds = ChassisSpeeds.discretize(speeds, TimedRobot.kDefaultPeriod); - - if (fieldRelative) { - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading()); - } - - SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds); - // SwerveDriveKinematics.desaturateWheelSpeeds(states, ); - - for (int i = 0; i < modules.length; i++) { - modules[i].setDesiredStateDrive(states[i]); - } - - this.desiredSpeeds = speeds; - } - - /** * Get the desired speeds of robot * diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index d680549..f95adcf 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -214,39 +214,6 @@ public void setDesiredState(SwerveModuleState state, boolean shouldOptimize) { this.desiredState = state; } - /** - * Set the state of the swerve module. The state is the speed and angle of the swerve module. - * You can use {@code Rotation2d.fromDegrees()} to create angle. - * This version is meant for driving the robot, where a new state is set every 20ms. - * It tries to make driving smoother by scaling speed by the cosine of the angle - * It also expects states to be in power mode, meaning instead of meters per second the motors are givin the direct speed to spin at - * - * @param state New state of swerve module, contains speed in meters per second and angle as {@link Rotation2d} - * @param shouldOptimize Whether to optimize the way the swerve module gets to the desired state - */ - public void setDesiredStateDrive(SwerveModuleState state) { - if (state != null) { - - // get current angle of robot - Rotation2d encoderAngle = getState().angle; - - // Optimize the reference state to avoid spinning further than 90 degrees - state = optimize(state, encoderAngle); - - // Scale speed by cosine of angle error. - // This scales down movement perpendicular to the desired direction of travel that can occur when modules change directions. - // This results in smoother driving. - final double speed = state.speedMetersPerSecond; - final double speedCosScaled = speed * state.angle.minus(encoderAngle).getCos(); - - final double scaleSplit = SwerveModuleConstants.SWERVE_MODULE_DRIVE_COSIGN_SCALE; - - state.speedMetersPerSecond = (speed * (1 - scaleSplit)) + (speedCosScaled * scaleSplit); - } - - this.desiredState = state; - } - /** * Set the state of the swerve module. Will automatically optimize. * The state is the speed and angle of the swerve module.