Swerve

src/main/java/frc/lib/util/FeedforwardGains.java

@@ -93,4 +93,4 @@ public FeedforwardGains build() {
       return new FeedforwardGains(this);
     }
   }
-}
+}

src/main/java/frc/lib/util/PIDGains.java

@@ -94,4 +94,4 @@ public PIDGains build() {
       return new PIDGains(this);
     }
   }
-}
+}

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

@@ -10,19 +10,22 @@
 import frc.lib.util.FeedforwardGains;
 import frc.lib.util.PIDGains;
 
-public class Constants {
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.units.measure.Time;
 
-  public static class RobotConstants {
-    public static final Time ROBOT_CLOCK_SPEED = Time.ofBaseUnits(20, Units.Milliseconds);
-  }
+import static edu.wpi.first.units.Units.*;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import frc.lib.util.FeedforwardGains;
+import frc.lib.util.PIDGains;
 
-  public static class SwerveConstants {
-    public static final LinearVelocity MAX_SPEED =
-        LinearVelocity.ofBaseUnits(4.25, Units.MetersPerSecond);
-    public static final AngularVelocity MAX_ANGULAR_VELOCITY =
-        AngularVelocity.ofBaseUnits(0.75, Units.RotationsPerSecond);
-    public static final Time SWERVE_UPDATE_PERIOD = Time.ofBaseUnits(20, Units.Millisecond);
-  }
+
+public class Constants {
 
   public static class ElevatorConstants {
     public static final Distance START_SETPOINT = Distance.ofBaseUnits(0, Units.Meters);
@@ -63,4 +66,52 @@ public static class EndEffectorConstants {
     public static final int MOTOR_ID = 98;
     public static final int LASERCAN_ID = 99;
   }
+  public static class RobotConstants {
+
+    public static final Distance WIDTH = Inches.of(26.0);
+    public static final Distance LENGTH = Inches.of(26.0);
+
+    public static  final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(
+      new Translation2d(RobotConstants.LENGTH.divide(2), RobotConstants.WIDTH.divide(2)),
+      new Translation2d(RobotConstants.LENGTH.divide(2), RobotConstants.WIDTH.divide(-2)),
+      new Translation2d(RobotConstants.LENGTH.divide(-2), RobotConstants.WIDTH.divide(2)),
+      new Translation2d(RobotConstants.LENGTH.divide(-2), RobotConstants.WIDTH.divide(-2))
+      );    
+
+      public static final Time ROBOT_CLOCK_SPEED = Time.ofBaseUnits(20, Units.Milliseconds);
+
+  }
+
+  public static class SwerveConstants {
+    public static final PIDGains TURN_PID = new PIDGains.Builder().kp(0.2).build();
+    public static final PIDGains DRIVE_PID = new PIDGains.Builder().kp(0.1).build();
+    public static final FeedforwardGains TURN_FEEDFORWARD = new FeedforwardGains .Builder().kv(0.2).build();
+    public static final FeedforwardGains DRIVE_FEEDFORWARD = new FeedforwardGains .Builder().kv(0.26).build();
+    public static final Current TURN_CURRENT_LIMIT = Current.ofBaseUnits(20, Units.Amps);
+    public static final Current DRIVE_CURRENT_LIMIT = Current.ofBaseUnits(211, Units.Amps);
+    public static final double DRIVE_CONVERSION_FACTOR = 1;
+    public static final double TURN_CONVERSION_FACTOR = 1;
+
+    public static final double DEADBAND = 0.05;
+
+    public static final LinearVelocity MAX_SPEED = MetersPerSecond.of(4.25);
+
+    public static final AngularVelocity MAX_ROTATION = RadiansPerSecond.of(2 * Math.PI);
+
+    public static final double LEFT_STICK_SCAILING = 2;
+
+    public static final double RIGHT_STICK_SCAILING = 3;
+
+    public static final int FRONT_LEFT_DRIVE_ID = 1;
+    public static final int FRONT_LEFT_TURN_ID = 2;
+    public static final int FRONT_RIGHT_DRIVE_ID = 3;
+    public static final int FRONT_RIGHT_TURN_ID = 4;
+    public static final int BACK_LEFT_DRIVE_ID = 5;
+    public static final int BACK_LEFT_TURN_ID = 6;
+    public static final int BACK_RIGHT_DRIVE_ID = 7;
+    public static final int BACK_RIGHT_TURN_ID = 8;
+
+    public static final int GYRO_ID = 18;
+
+  }
 }

src/main/java/frc/robot/Robot.java

@@ -39,6 +39,7 @@ public void disabledExit() {}
 
   @Override
   public void autonomousInit() {
+
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     if (m_autonomousCommand != null) {
@@ -57,6 +58,7 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+    m_robotContainer.teleopInit();
   }
 
   @Override

src/main/java/frc/robot/RobotContainer.java

@@ -8,6 +8,10 @@
 import edu.wpi.first.epilogue.NotLogged;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -17,6 +21,10 @@
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.EndEffector;
 import frc.robot.subsystems.Swerve;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.subsystems.Swerve;
+
 
 @Logged
 public class RobotContainer {
@@ -27,21 +35,22 @@ public class RobotContainer {
   private final EndEffector endEffector;
 
   @NotLogged private final CommandXboxController xboxController;
+  private final XboxController controller;
+  private boolean colour;
 
   public RobotContainer() {
     swerve = new Swerve();
     elevator = new Elevator();
     endEffector = new EndEffector();
+    controller = new XboxController(0);
 
     xboxController = new CommandXboxController(0);
 
     configureBindings();
+    defualtCommands();
   }
 
   private void configureBindings() {
-    swerve.setDefaultCommand(
-        swerve.driveFieldRelativeCommand(
-            xboxController::getLeftY, xboxController::getLeftX, xboxController::getRightX));
 
     Trigger testEffector = xboxController.axisGreaterThan(0, 0.05);
     testEffector.onTrue(endEffector.setVelocityCommand(EndEffectorConstants.INTAKE_SPEED));
@@ -54,9 +63,27 @@ private void configureBindings() {
     Trigger outtakeEffector = xboxController.b();
     outtakeEffector.onTrue(endEffector.intakeCommand());
     outtakeEffector.onFalse(endEffector.setVelocityCommand(AngularVelocity.ofBaseUnits(0, Units.RPM)));
+
+    Trigger zero = new Trigger(() -> controller.getAButton());
+
+    zero.onTrue(swerve.zeroRobotCommad(colour));
   }
 
-  public void teleopInit() {}
+  private void defualtCommands(){
+    if (colour){
+    swerve.setDefaultCommand(swerve.driveFieldRelativeBlueCommand(controller::getLeftY, controller::getLeftX, controller::getRightX));
+    }
+
+    else{
+      swerve.setDefaultCommand(swerve.driveFieldRelativeRedCommand(controller::getLeftY, controller::getLeftX, controller::getRightX));
+    }
+
+
+  }
+
+  public void teleopInit() {
+    colour = (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue);
+  }
 
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");

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

@@ -1,63 +1,158 @@
 package frc.robot.subsystems;
 
+import static edu.wpi.first.units.Units.Degree;
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Microseconds;
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
+
+import java.util.function.DoubleSupplier;
+
+import org.opencv.core.Mat;
+
+import com.ctre.phoenix6.hardware.Pigeon2;
+
 import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.Kinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import frc.robot.Constants.RobotConstants;
 import frc.robot.Constants.SwerveConstants;
-import java.util.function.DoubleSupplier;
 
 @Logged
-public class Swerve extends SubsystemBase {
+public class Swerve extends SubsystemBase{
 
-  private Pose2d estimatedPose;
+  private Pose2d estimatedPosition;
+  private Rotation2d simHeading;
+  private Rotation2d gyroAngle;
+  private SwerveModuleState[] swerveModuleStates;
+  private SwerveModuleState frontLeft;
+  private SwerveModuleState frontRight;
+  private SwerveModuleState backLeft;
+  private SwerveModuleState backRight;
+  private final Pigeon2 gyro;
 
   public Swerve() {
 
-    // NOTE: An empty constructor will set the robot at
-    // (0,0) on the field,
-    // facing to the right.
+    gyro = new Pigeon2(SwerveConstants.GYRO_ID);
+
+    simHeading = new Rotation2d(0.0);
+
+    estimatedPosition = new Pose2d();
+
+    swerveModuleStates = new SwerveModuleState[4];
+
+    frontLeft = new SwerveModuleState();
+    frontRight = new SwerveModuleState();
+    backLeft = new SwerveModuleState();
+    backRight = new SwerveModuleState();
 
-    estimatedPose = new Pose2d();
   }
 
-  /**
-   * Drives the robot for the current duty cycle.
-   *
-   * @param x field-relative X linear velocity in meters/s
-   * @param y field-relative Y linear velocity in meters/s
-   * @param omega field-relative angular velocity in radians/s
-   */
-  private void driveFieldRelative(LinearVelocity x, LinearVelocity y, AngularVelocity omega) {
-    estimatedPose =
-        estimatedPose.plus(
-            new Transform2d(
-                x.times(SwerveConstants.SWERVE_UPDATE_PERIOD),
-                y.times(SwerveConstants.SWERVE_UPDATE_PERIOD),
-                new Rotation2d(omega.times(SwerveConstants.SWERVE_UPDATE_PERIOD))));
+  @Override
+  public void simulationPeriodic(){
+    gyroAngle = simHeading;
+    System.out.println("anlge of robot in sim is " + gyroAngle);
+  }
+
+  @Override
+  public void periodic() {
+    gyroAngle = gyro.getRotation2d();
+  }
+
+  private void DriveFiledRelativeBlueScailier(double x, double y, double omega){
+
+
+    double liniarMagintued = Math.pow(Math.hypot(x, y), SwerveConstants.LEFT_STICK_SCAILING);
+
+    Rotation2d directions = new Rotation2d(y, x);
+
+    y = liniarMagintued * -directions.getCos() * SwerveConstants.MAX_SPEED.in(MetersPerSecond);
+    x = liniarMagintued * directions.getSin() * SwerveConstants.MAX_SPEED.in(MetersPerSecond);
+
+    omega =  Math.pow(omega, SwerveConstants.RIGHT_STICK_SCAILING) * SwerveConstants.MAX_ROTATION.in(RadiansPerSecond);
+
+    driveFieldRelative(MetersPerSecond.of(MathUtil.applyDeadband(-x, Constants.SwerveConstants.DEADBAND)), MetersPerSecond.of(MathUtil.applyDeadband(y, Constants.SwerveConstants.DEADBAND)), RadiansPerSecond.of(MathUtil.applyDeadband(-omega, Constants.SwerveConstants.DEADBAND)));
+  }
+
+  private void DriveFiledRelativeRedScailier(double x, double y, double omega){
+
+
+    double liniarMagintued = Math.pow(Math.hypot(x, y), SwerveConstants.LEFT_STICK_SCAILING);
+
+    Rotation2d directions = new Rotation2d(y, x);
+
+    y = liniarMagintued * -directions.getCos() * SwerveConstants.MAX_SPEED.in(MetersPerSecond);
+    x = liniarMagintued * directions.getSin() * SwerveConstants.MAX_SPEED.in(MetersPerSecond);
+
+    System.out.println("max speed in radians is " + SwerveConstants.MAX_ROTATION.in(RadiansPerSecond));
+    System.out.println("rotaton speed is " + Math.pow(omega, SwerveConstants.RIGHT_STICK_SCAILING) * SwerveConstants.MAX_ROTATION.in(RadiansPerSecond));
+    omega =  Math.pow(omega, SwerveConstants.RIGHT_STICK_SCAILING) * SwerveConstants.MAX_ROTATION.in(RadiansPerSecond);
+
+    driveFieldRelative(MetersPerSecond.of(MathUtil.applyDeadband(x, Constants.SwerveConstants.DEADBAND)), MetersPerSecond.of(MathUtil.applyDeadband(-y, Constants.SwerveConstants.DEADBAND)), RadiansPerSecond.of(MathUtil.applyDeadband(-omega, Constants.SwerveConstants.DEADBAND)));
   }
 
+
+  private void driveFieldRelative(LinearVelocity x, LinearVelocity y, AngularVelocity omega){
+    simHeading = simHeading.plus(new Rotation2d(omega.times(Seconds.of(.02))));
+    estimatedPosition = new Pose2d(estimatedPosition.getX() + x.times(RobotConstants.ROBOT_CLOCK_SPEED).in(Meters), estimatedPosition.getY() + (y.times(RobotConstants.ROBOT_CLOCK_SPEED).in(Meters)), new Rotation2d(estimatedPosition.getRotation().getRadians() + omega.times(Seconds.of(.02)).in(Radians)));
+    ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(x.times(Seconds.of(.02)).in(Meters), y.times(Seconds.of(.02)).in(Meters), omega.times(Seconds.of(.02)).in(Radians), gyroAngle);
+    swerveModuleStates = RobotConstants.KINEMATICS.toSwerveModuleStates(speeds);
+    //front left
+    frontLeft = swerveModuleStates[0];
+    //front Right
+    frontRight = swerveModuleStates[1];
+    //back Left
+    backLeft = swerveModuleStates[2];
+    //back Right
+    backRight = swerveModuleStates[3];
+  }
+
+  public void zeroing(boolean colour){
+    System.out.println(colour);
+    simHeading = colour == true ?  new Rotation2d(0.0) :  new Rotation2d(Math.PI);
+  }
+
+
+   public Command driveFieldRelativeRedCommand(DoubleSupplier x, DoubleSupplier y, DoubleSupplier omega){
+    return Commands.sequence(
+      Commands.run(
+() -> DriveFiledRelativeBlueScailier(x.getAsDouble(), y.getAsDouble(), omega.getAsDouble()), this)
+);
+   }
+
+   public Command driveFieldRelativeBlueCommand(DoubleSupplier x, DoubleSupplier y, DoubleSupplier omega){
+    return Commands.sequence(
+      Commands.run(
+() -> DriveFiledRelativeRedScailier(x.getAsDouble(), y.getAsDouble(), omega.getAsDouble()), this)
+);
+   }
+
+   public Command zeroRobotCommad(boolean colour){
+    return Commands.runOnce(() -> zeroing(colour), this);
+   }
+
   /**
-   * Returns a Command that passes the input parameters into {@link
-   * #driveFieldRelative(LinearVelocity, LinearVelocity, AngularVelocity)}.
-   *
-   * @param x field-relative X with range of -1.0 to 1.0
-   * @param y field-relative Y with range of -1.0 to 1.0
-   * @param omega field-relative omega with range of -1.0 to 1.0
+   * Step 3b: Public command factory driveFieldRelativeCommand that takes field-relative inputs
+   * and returns a command that passes the input parameters into the private driveFieldRelative
+   * @param X field-relative X with range of -1.0 to 1.0
+   * @param Y field-relative Y with range of -1.0 to 1.0
+   * @param Omega field-relative omega with range of -1.0 to 1.0
    */
-  public Command driveFieldRelativeCommand(
-      DoubleSupplier x, DoubleSupplier y, DoubleSupplier omega) {
-    return Commands.run(
-        () ->
-            driveFieldRelative(
-                SwerveConstants.MAX_SPEED.times(-x.getAsDouble()),
-                SwerveConstants.MAX_SPEED.times(-y.getAsDouble()),
-                SwerveConstants.MAX_ANGULAR_VELOCITY.times(-omega.getAsDouble())),
-        this);
-  }
+
 }