Added Balance Control

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

@@ -86,5 +86,12 @@ public static final class OIConstants {
         public static final double kDeadband = 0.1;
     }
 
+    public static final class BalanceConstants {
+        public static final double kBalancingControlGoalDegrees = 0;
+        public static final double kBalancingControlTresholdDegrees = 2.5;
+        public static final double kBalancingControlDriveP = 0.04; // P (Proportional) constant of a PID loop
+        public static final double kBalancingControlBackwardsPowerMultiplier = 2;
+    }
+
 }
 

src/main/java/frc/robot/commands/BalanceControlCommand.java

@@ -0,0 +1,113 @@
+package frc.robot.commands;
+
+import frc.robot.Robot;
+import frc.robot.Constants.BalanceConstants;
+import frc.robot.subsystems.SwerveDriveSubsystem;
+import frc.robot.Constants.DriveConstants;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.SwerveDriveSubsystem;
+
+public class BalanceControlCommand extends CommandBase {
+
+    private double error;
+    private double currentAngle;
+    private double drivePower;
+    private final SwerveDriveSubsystem swerveSubsystem;
+    private int counter;
+    private boolean m_isFinished;
+
+
+    public BalanceControlCommand(SwerveDriveSubsystem swerveSubsystem){
+      System.out.println("Started Balance");
+      this.swerveSubsystem = swerveSubsystem;
+      this.m_isFinished = false;
+      addRequirements(swerveSubsystem);
+      }
+      // Called when the command is initially scheduled.
+      @Override
+      public void initialize() {}
+
+      // Called every time the scheduler runs while the command is scheduled.
+
+      @Override
+      public void execute() {
+        // Uncomment the line below this to simulate the gyroscope axis with a controller joystick
+        // Double currentAngle = -1 * Robot.controller.getRawAxis(Constants.LEFT_VERTICAL_JOYSTICK_AXIS) * 45;
+
+        this.currentAngle = swerveSubsystem.getRoll();
+
+        error = BalanceConstants.kBalancingControlGoalDegrees - currentAngle;
+        drivePower = Math.min(BalanceConstants.kBalancingControlDriveP * error, 1);
+
+        if (Math.abs(error) < BalanceConstants.kBalancingControlTresholdDegrees) {
+          counter += 1;
+        }
+
+        if (counter > 100) {
+          this.m_isFinished = true;
+        }
+
+        // Our robot needed an extra push to drive up in reverse, probably due to weight imbalances
+        if (drivePower < 0) {
+          drivePower *= BalanceConstants.kBalancingControlBackwardsPowerMultiplier;
+        }
+
+        // Limit the max power
+        if (Math.abs(drivePower) > 0.5) {
+          drivePower = Math.copySign(0.5, drivePower);
+        }
+
+        // Construct desired chassis speeds
+        ChassisSpeeds chassisSpeeds;
+
+        chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+          drivePower, 0, 0, swerveSubsystem.getRotation2d());
+
+
+        SwerveModuleState[] moduleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds);
+
+        // Output each module states to wheels
+        this.swerveSubsystem.setModuleStates(moduleStates);
+
+
+        SmartDashboard.putNumber("Current Angle: ", currentAngle);
+        SmartDashboard.putNumber("Error ", error);
+        SmartDashboard.putNumber("Drive Power: ", drivePower);
+      }
+
+      // Called once the command ends or is interrupted.
+      @Override
+      public void end(boolean interrupted) {
+        System.out.println("COMMAND FINISHED");
+        ChassisSpeeds chassisSpeedsStopForward;
+        ChassisSpeeds chassisSpeedsStopSide;
+        chassisSpeedsStopForward = ChassisSpeeds.fromFieldRelativeSpeeds(0, 0.1, 0, swerveSubsystem.getRotation2d());
+        chassisSpeedsStopSide = ChassisSpeeds.fromFieldRelativeSpeeds(0.1, 0, 0, swerveSubsystem.getRotation2d());
+
+        SwerveModuleState[] moduleStatesForward = DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeedsStopForward);
+        SwerveModuleState[] moduleStatesSide = DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeedsStopSide);
+
+        SwerveModuleState[] combinedStates = new SwerveModuleState[] {
+          moduleStatesForward[0],
+          moduleStatesSide[1],
+          moduleStatesSide[2],
+          moduleStatesForward[3]
+        };
+
+        swerveSubsystem.setModuleStates(combinedStates);
+
+        //swerveSubsystem.stopModules();
+
+
+      }
+
+      // Returns true when the command should end.
+      @Override
+      public boolean isFinished() {
+        // End the command when we are within the specified threshold of being 'flat' (gyroscope pitch of 0 degrees)
+        return this.m_isFinished;
+      }
+    }

src/main/java/frc/robot/commands/SwitchFunction.java

@@ -0,0 +1,32 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import frc.robot.subsystems.SwerveDriveSubsystem;
+
+public class SwitchFunction extends CommandBase {
+
+    private final SwerveDriveSubsystem swerveSubsystem;
+
+    public SwitchFunction(SwerveDriveSubsystem swerveSubsystem) {
+        this.swerveSubsystem = swerveSubsystem;
+        addRequirements(swerveSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        swerveSubsystem.zeroHeading();
+    }
+
+    @Override
+    public void execute() {}
+
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

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

@@ -59,6 +59,14 @@ public double getHeading(){
         return Math.IEEEremainder(gyro.getAngle(), 360);
     }
 
+    public double getPitch() {
+        return gyro.getPitch();
+    }
+
+    public double getRoll() {
+        return gyro.getRoll();
+    }
+
     public Rotation2d getRotation2d(){
         return Rotation2d.fromDegrees(getHeading());
     }