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

…2024

buildGradleFix.ps1

@@ -1,2 +1,6 @@
+# Fixes build.gradle warning
+
+# To run, open your terminal and select powershell from the options dropdown. Then type ./buildGradleFix.ps1 to run this script.
+# If you got your laptop from the school you will have to copy and paste these commands into powershell because they disabled powershell scripts by defualt
 Invoke-WebRequest -Uri https://repo.maven.apache.org/maven2/org/junit/jupiter/junit-jupiter/5.10.1/junit-jupiter-5.10.1.module -OutFile C:\Users\Public\wpilib\2024\maven\org\junit\jupiter\junit-jupiter\5.10.1\junit-jupiter-5.10.1.module
 Invoke-WebRequest -Uri https://repo.maven.apache.org/maven2/org/junit/junit-bom/5.10.1/junit-bom-5.10.1.module -OutFile C:\Users\Public\wpilib\2024\maven\org\junit\junit-bom\5.10.1\junit-bom-5.10.1.module

src/main/TODO

@@ -0,0 +1,4 @@
+TODO File
+
+- Create easy way to switch setting files, preferences, other
+- Create swerve offset finding code

src/main/java/frc/robot/Constants.java

@@ -1,5 +1,12 @@
 package frc.robot;
 
+import java.io.File;
+import java.io.IOException;
+
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.fasterxml.jackson.databind.node.ObjectNode;
+
 import edu.wpi.first.math.util.Units;
 
 /**
@@ -9,6 +16,20 @@
  */
 public final class Constants {
 
+    // private static final String botName = "WoodBot";
+    // private static final String fileFormat = "configs/%s.json";
+
+    // private static final JsonNode rootNode;
+    // static {
+    //     JsonNode node = null;
+    //     try {
+    //         node = new ObjectMapper().readTree(new File(String.format(fileFormat, botName)));
+    //     } catch (IOException e) {
+    //         System.out.println("Invalid config file.");
+    //     }
+    //     rootNode = node;
+    // }
+
     public static class DriverConstants {
         public static final int DRIVER_JOYSTICK_PORT = 0;
 
@@ -29,16 +50,20 @@ public static class OperatorConstants {
     }
 
     public static class SwerveModuleConstants {
-        public static final double MAX_SPEED = 1;
-
+
         // Values from https://www.swervedrivespecialties.com/products/mk4-swerve-module. We have L1 Modules.
         public static final double DRIVE_MOTOR_GEAR_RATIO = 57 / 7;
         public static final double STEERING_MOTOR_GEAR_RATIO = 12.8;
-
+        
         public static final double STEERING_ENCODER_SENSOR_COEFFICIENT = 0.000244140625; // if you put 1/4096 it just becomes zero
-
+        
         public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(7);
         public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER_METERS * Math.PI;
+
+        // Other settings
+        public static final double MAX_SPEED_LIMIT = 1;
+        public static final double SWERVE_MODULE_DRIVE_COSIGN_COEFFICIENT = 1;
+
 
         // Steering PID values
         public static final double STEERING_PID_P = 1;

src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java

@@ -1,5 +1,6 @@
 package frc.robot.commands.SwerveRemoteOperation;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -56,9 +57,9 @@ public void initialize() {
     public void execute() {
 
         // Get joystick inputs
-        final double speedX = applyJoystickDeadzone(-joystick.getX(), DriverConstants.JOYSTICK_DEAD_ZONE);
-		final double speedY = applyJoystickDeadzone(-joystick.getY(), DriverConstants.JOYSTICK_DEAD_ZONE);
-		final double speedOmega = applyJoystickDeadzone(joystick.getTwist(), DriverConstants.JOYSTICK_DEAD_ZONE);
+        final double speedX = MathUtil.applyDeadband(-joystick.getX(), DriverConstants.JOYSTICK_DEAD_ZONE);
+		final double speedY = MathUtil.applyDeadband(-joystick.getY(), DriverConstants.JOYSTICK_DEAD_ZONE);
+		final double speedR = MathUtil.applyDeadband(joystick.getTwist(), DriverConstants.JOYSTICK_DEAD_ZONE);
 
         // // Code for rotating with buttons if driver prefers 
         // double speedOmega = 0;
@@ -87,7 +88,7 @@ public void execute() {
         final ChassisSpeeds speeds = new ChassisSpeeds(
             speedX * DriverConstants.maxSpeedOptionsTranslation[speedLevel] * speedCoefficient,
             speedY * DriverConstants.maxSpeedOptionsTranslation[speedLevel] * speedCoefficient,
-            speedOmega * DriverConstants.maxSpeedOptionsRotation[speedLevel] * speedCoefficient
+            speedR * DriverConstants.maxSpeedOptionsRotation[speedLevel] * speedCoefficient
         );
 
         // Display relevant data on shuffleboard.
@@ -104,22 +105,21 @@ public void execute() {
         SmartDashboard.putNumber("Target Speed Y MPH", speeds.vyMetersPerSecond * metersPerSecondToMPH);
         SmartDashboard.putNumber("Target RPM", speeds.omegaRadiansPerSecond * radiansPerSecondToRPM);
 
-        final ChassisSpeeds realSpeeds = drivetrain.getState();
+        final ChassisSpeeds currentRobotSpeeds = drivetrain.getState();
 
-        SmartDashboard.putNumber("Real Speed X MPH", realSpeeds.vxMetersPerSecond * metersPerSecondToMPH);
-        SmartDashboard.putNumber("Real Speed Y MPH", realSpeeds.vyMetersPerSecond * metersPerSecondToMPH);
-        SmartDashboard.putNumber("Real RPM", realSpeeds.omegaRadiansPerSecond * radiansPerSecondToRPM);
+        SmartDashboard.putNumber("Real Speed X MPH", currentRobotSpeeds.vxMetersPerSecond * metersPerSecondToMPH);
+        SmartDashboard.putNumber("Real Speed Y MPH", currentRobotSpeeds.vyMetersPerSecond * metersPerSecondToMPH);
+        SmartDashboard.putNumber("Real RPM", currentRobotSpeeds.omegaRadiansPerSecond * radiansPerSecondToRPM);
 
         SmartDashboard.putNumber("Joystick X", joystick.getX());
 		SmartDashboard.putNumber("Joystick Y", joystick.getY());
 		SmartDashboard.putNumber("Joystick R", joystick.getTwist());
 
         SmartDashboard.putBoolean("X Active", speedX != 0);
 		SmartDashboard.putBoolean("Y Active", speedY != 0);
-		SmartDashboard.putBoolean("R Active", speedOmega != 0);
+		SmartDashboard.putBoolean("R Active", speedR != 0); 
 
-
-        drivetrain.setDesiredState(speeds, isFieldRelative);
+        drivetrain.setDesiredStateDrive(speeds, isFieldRelative);
     }
 
     /**
@@ -139,24 +139,4 @@ public boolean isFinished() {
     public void end(boolean interrupted) {
         drivetrain.stop();
     }
-
-
-
-    // --- Util ---
-
-    /**
-     * Utility method. Apply a deadzone to the joystick output to account for stick drift and small bumps.
-     * 
-     * @param joystickValue Value in [-1, 1] from joystick axis
-     * @return {@code 0} if {@code |joystickValue| <= deadzone}, else the {@code joystickValue} scaled to the new control area
-     */
-    public static double applyJoystickDeadzone(double joystickValue, double deadzone) {
-        if (Math.abs(joystickValue) <= deadzone) {
-            // If the joystick |value| is in the deadzone than zero it out
-            return 0;
-        }
-
-        // scale value from the range [0, 1] to (deadzone, 1]
-        return joystickValue * (1 + deadzone) - Math.signum(joystickValue) * deadzone;
-    }
 }

src/main/java/frc/robot/configs/WoodBot.json

@@ -0,0 +1,3 @@
+{
+
+}

src/main/java/frc/robot/subsystems/SwerveDrivetrain.java

@@ -17,6 +17,7 @@
 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;
 
 /**
@@ -147,7 +148,7 @@ public void stop() {
         modulesMap(SwerveModule::stop);
     }
 
-    /** Put all swerve modules to default state */
+    /** Put all swerve modules to default state, facing forward and staying still */
     public void toDefaultStates() {
         modulesMap(SwerveModule::toDefaultState);
     }
@@ -213,6 +214,22 @@ public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative) {
         setDesiredState(speeds);
     }
 
+    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);
+
+        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

@@ -38,6 +38,7 @@ public class SwerveModule extends SubsystemBase {
 
     /** Status Signal that gives steering encoders current position in rotations */
     private final StatusSignal<Double> steeringPosition;
+    private final double steeringOffset;
 
     /** Default state, forward and still */
     private final static SwerveModuleState defaultState = new SwerveModuleState();
@@ -75,7 +76,7 @@ public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steer
         drivePIDController.setD(SwerveModuleConstants.DRIVE_PID_D);
         drivePIDController.setFF(SwerveModuleConstants.DRIVE_PID_FF);
         drivePIDController.setIZone(SwerveModuleConstants.DRIVE_PID_IZone);
-        drivePIDController.setOutputRange(-SwerveModuleConstants.MAX_SPEED, SwerveModuleConstants.MAX_SPEED);
+        drivePIDController.setOutputRange(-SwerveModuleConstants.MAX_SPEED_LIMIT, SwerveModuleConstants.MAX_SPEED_LIMIT);
 
         // --- Steering Motor ---
         steeringMotor = new CANSparkMax(steeringMotorDeviceId, MotorType.kBrushless);
@@ -98,7 +99,9 @@ public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steer
 
         // --- Save other values ---
         this.distanceFromCenter = distanceFromCenter;
+
         steeringPosition = steeringEncoder.getAbsolutePosition();
+        steeringOffset = steeringEncoderZero;
 
         setName(toString());
     }
@@ -112,11 +115,11 @@ public void periodic() {
         if (desiredState != null) { 
             // Get real and desired angles of steering motor
             // In pid the real value is often known as the measure or process variable, while the desired value is the setpoint or reference
-            final double measuredDegrees = getState().angle.getDegrees();
-            final double desiredDegrees = desiredState.angle.getDegrees();
+            final double measuredRotations = getState().angle.getRotations();
+            final double desiredRotations = desiredState.angle.getRotations();
 
             // Use the steering motor pid controller to calculate speed to turn steering motor to get to desired angle
-            final double steeringMotorSpeed = steeringPIDController.calculate(measuredDegrees, desiredDegrees);
+            final double steeringMotorSpeed = steeringPIDController.calculate(measuredRotations, desiredRotations);
             // set steering motor to calculated value
             steeringMotor.set(steeringMotorSpeed);
 
@@ -171,8 +174,25 @@ public SwerveModuleState getState() {
      * @param shouldOptimize Whether to optimize the way the swerve module gets to the desired state
      */
     public void setDesiredState(SwerveModuleState state, boolean shouldOptimize) {
-        if (shouldOptimize) {
-            state = optimize(state, getState().angle);
+        if (shouldOptimize && state != null) {
+            // Optimize the reference state to avoid spinning further than 90 degrees
+            state = SwerveModuleState.optimize(state,  getState().angle);
+        }
+
+        this.desiredState = state;
+    }
+
+    public void setDesiredStateDrive(SwerveModuleState state) {
+        if (state != null) {
+            Rotation2d encoderRotation = getState().angle;
+
+            // Optimize the reference state to avoid spinning further than 90 degrees
+            state = SwerveModuleState.optimize(state, encoderRotation);
+
+            // 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.
+            state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos() * SwerveModuleConstants.SWERVE_MODULE_DRIVE_COSIGN_COEFFICIENT;
         }
 
         this.desiredState = state;
@@ -251,85 +271,17 @@ private double getDriveDistanceRotations() {
      * @return Current position in rotations of the steering motor, accounting for offset
      */
     private double getSteeringAngleRotations() {
-        return steeringPosition.refresh().getValueAsDouble();
-    }
-
-    // --- Util ---
-
-    /**
-     * Optimize a swerve module state so that instead of suddenly rotating the wheel (with steering motor)
-     * to go a certain direction we can instead just turn a half as much and switch
-     * the speed of wheel to go in reverse.
-     * 
-     * @param desiredState The state you want the swerve module to be in
-     * @param currentAngle The current angle of the swerve module in degrees
-     * @return             An optimized version of desiredState
-     */
-    private static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) {
-        // find the target angle in the same 0-360 scope as the desired state
-        double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees());
-
-        // keep the same target speed
-        double targetSpeed = desiredState.speedMetersPerSecond;
-
-        // found how much we have to move to get to target angle
-        double delta = targetAngle - currentAngle.getDegrees();
-
-        // If we have to flip around more than 90 degrees than instead just reverse our direction
-        // and only turn enough so that we have the motor facing in the same direction, just the other way 
-        if (Math.abs(delta) > 90) {
-            targetSpeed = -targetSpeed;
-            targetAngle += delta > 0 ? -180 : 180;
-        }
-
-        return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle));
-    }
-
-    /**
-     * Utility method. Move an angle into the range of the reference. Finds the relative 0 and 360
-     * position for a scope reference and moves the new angle into that.
-     * Example: {@code placeInAppropriate0To360Scope(90, 370) = 10.0}
-     * {@code placeInAppropriate0To360Scope(720, 10) = 730.0}
-     * 
-     * @param scopeReference The reference to find which 0-360 scope we are in.
-     *                       For example {@code 10} is in {@code 0-360} scope while {@code 370} is in {@code 360-720} scope.
-     * @param newAngle       The angle we want to move into found scope.
-     *                       For example if the scope was {@code 0-360} and our angle was {@code 370} it would become {@code 10}
-     * @return               {@code newAngle} in the same scope as {@code scopeReference}
-     */
-    private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) {
-        double lowerBound;
-        double upperBound;
-        double lowerOffset = scopeReference % 360;
-        if (lowerOffset >= 0) {
-            lowerBound = scopeReference - lowerOffset;
-            upperBound = scopeReference + (360 - lowerOffset);
-        } else {
-            upperBound = scopeReference - lowerOffset;
-            lowerBound = scopeReference - (360 + lowerOffset);
-        }
-        while (newAngle < lowerBound) {
-            newAngle += 360;
-        }
-        while (newAngle > upperBound) {
-            newAngle -= 360;
-        }
-        if (newAngle - scopeReference > 180) {
-            newAngle -= 360;
-        } else if (newAngle - scopeReference < -180) {
-            newAngle += 360;
-        }
-        return newAngle;
+        return steeringPosition.refresh().getValueAsDouble() + steeringOffset;
     }
 
     @Override
     public String toString() {
 
-        final String[] xPositions = {"Right", "Middle", "Left"};
         final String[] yPositions = {"Back", "Middle", "Front"}; 
+        final String[] xPositions = {"Right", "Middle", "Left"};
 
-        final int xSignum = (int) Math.signum(distanceFromCenter.getX());
         final int ySignum = (int) Math.signum(distanceFromCenter.getY());
+        final int xSignum = (int) Math.signum(distanceFromCenter.getX());
 
         return xPositions[xSignum + 1] + yPositions[ySignum + 1] + "SwerveModule";
     }