From f95d57c65badaaa12ae1379f36633f2453d5306e Mon Sep 17 00:00:00 2001 From: Brennen Collins Date: Mon, 6 Mar 2023 16:04:00 -0500 Subject: [PATCH] Added Balance Control --- src/main/java/frc/robot/Constants.java | 7 ++ .../robot/commands/BalanceControlCommand.java | 113 ++++++++++++++++++ .../frc/robot/commands/SwitchFunction.java | 32 +++++ .../subsystems/SwerveDriveSubsystem.java | 8 ++ 4 files changed, 160 insertions(+) create mode 100644 src/main/java/frc/robot/commands/BalanceControlCommand.java create mode 100644 src/main/java/frc/robot/commands/SwitchFunction.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8c93efd..2a7c399 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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; + } + } diff --git a/src/main/java/frc/robot/commands/BalanceControlCommand.java b/src/main/java/frc/robot/commands/BalanceControlCommand.java new file mode 100644 index 0000000..7f83362 --- /dev/null +++ b/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; + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/SwitchFunction.java b/src/main/java/frc/robot/commands/SwitchFunction.java new file mode 100644 index 0000000..d961396 --- /dev/null +++ b/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; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 7cca697..ed8dcb7 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/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()); }