Merge branch 'main' of https://github.com/redshiftrobotics/crescendo-…

…2024

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);
     }
 
     /**

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);
     }
 
     /**

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
      *

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.