diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index ce6e312..201e0e9 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -83,11 +83,10 @@ private void configureButtonBindings() { public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); - if (SwerveDrive.instance != null) { - autonChooser.addOption("Preload Mobility", new PreloadMobility()); - autonChooser.addOption("Preload", new Preload()); - autonChooser.addOption("Mobility", new Mobility()); - } + autonChooser.addOption("Preload Mobility", new PreloadMobility()); + autonChooser.addOption("Preload", new Preload()); + autonChooser.addOption("Mobility", new Mobility()); + SmartDashboard.putData("Autonomous", autonChooser); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java b/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java index cfc3f53..f14d6d4 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java @@ -1,6 +1,7 @@ package com.stuypulse.robot.commands.auton; import com.stuypulse.robot.commands.swerve.SwerveDriveBack; +import com.stuypulse.robot.commands.tank.TankDriveBack; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -8,8 +9,10 @@ public class Mobility extends SequentialCommandGroup { public Mobility() { addCommands( - new WaitCommand(8), - new SwerveDriveBack(2).withTimeout(2) + new WaitCommand(2), + + // new SwerveDriveBack(2).withTimeout(2) + new TankDriveBack().withTimeout(2) ); } -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/Preload.java b/src/main/java/com/stuypulse/robot/commands/auton/Preload.java index 7cc6091..bffa037 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/Preload.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/Preload.java @@ -1,5 +1,6 @@ package com.stuypulse.robot.commands.auton; +import com.stuypulse.robot.commands.launcher.LauncherLaunch; import com.stuypulse.robot.commands.shooter.ShootCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -9,7 +10,9 @@ public class Preload extends SequentialCommandGroup { public Preload() { addCommands( new WaitCommand(2), - new ShootCommand(1,1) + new LauncherLaunch(0,1).withTimeout(0.25), + new LauncherLaunch(1, 1) + // new ShootCommand(1,1) ); } } diff --git a/src/main/java/com/stuypulse/robot/commands/tank/TankDriveBack.java b/src/main/java/com/stuypulse/robot/commands/tank/TankDriveBack.java new file mode 100644 index 0000000..a36b40a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/tank/TankDriveBack.java @@ -0,0 +1,29 @@ +package com.stuypulse.robot.commands.tank; + +import com.stuypulse.robot.subsystems.TankDrive; + +import edu.wpi.first.wpilibj2.command.Command; + +public class TankDriveBack extends Command { + private TankDrive tank; + + public TankDriveBack() { + this.tank = TankDrive.getInstance(); + addRequirements(tank); + } + + @Override + public void initialize() { + tank.tankDriveVolts(-12,-12); + } + + @Override + public void execute() { + tank.tankDriveVolts(-12,-12); + } + + @Override + public void end(boolean isInterrupted) { + tank.stop(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/tank/TankDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/tank/TankDriveDrive.java index f316d70..10255cf 100644 --- a/src/main/java/com/stuypulse/robot/commands/tank/TankDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/tank/TankDriveDrive.java @@ -2,6 +2,7 @@ import com.stuypulse.robot.subsystems.TankDrive; +import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -10,37 +11,36 @@ public class TankDriveDrive extends Command { private TankDrive tank; private XboxController driver; - - private final SlewRateLimiter driveSlew; - private final SlewRateLimiter turnSlew; - - double speed; - double turn; public TankDriveDrive(XboxController driver) { this.tank = TankDrive.getInstance(); this.driver = driver; - - driveSlew = new SlewRateLimiter(Drivetrain.DRIVE_SLEW_RATE); - turnSlew = new SlewRateLimiter(Drivetrain.TURN_SLEW_RATE); - addRequirements(tank); } public interface Drivetrain { // Low Pass Filter and deadband for Driver Controls - double SPEED_DEADBAND = 0.02; //Driver Settings/Speed Deadband + double SPEED_DEADBAND = 0.05; //Driver Settings/Speed Deadband double ANGLE_DEADBAND = 0.05; //Driver Settings/Turn Deadband // Slew Rate Limiter Constants - double DRIVE_SLEW_RATE = 8; //Driver Settings/Drive Slew Rate - double TURN_SLEW_RATE = 16; //Driver Settings/Turn Slew Rate + double DRIVE_POWER = 2; + double DRIVE_SLEW_RATE = 5; //Driver Settings/Drive Slew Rate + double DRIVE_RC_CONSTANT = 0.1; //Driver Settings/Drive RC Constant + + double TURN_POWER = 1; + double TURN_SLEW_RATE = 10; //Driver Settings/Turn Slew Rate + double TURN_RC_CONSTANT = 0.05; //Driver Settings/Drive RC Constant } - public double filterSpeed(double newSpeed) { - newSpeed = spow(newSpeed, 2); - newSpeed = driveSlew.calculate(newSpeed); - speed += (newSpeed - speed) * 0.75; + private final SlewRateLimiter speedSlewRate = new SlewRateLimiter(Drivetrain.DRIVE_SLEW_RATE); + private final LinearFilter speedLinearFilter = LinearFilter.singlePoleIIR(Drivetrain.DRIVE_SLEW_RATE, 0.02); + + public double filterSpeed(double speed) { + speed = spow(speed, Drivetrain.DRIVE_POWER); + speed = speedSlewRate.calculate(speed); + speed = speedLinearFilter.calculate(speed); + if (Math.abs(speed) < Drivetrain.SPEED_DEADBAND) { return 0; } else { @@ -48,10 +48,14 @@ public double filterSpeed(double newSpeed) { } } - public double filterTurn(double newTurn) { - newTurn = spow(newTurn, 1); - newTurn = turnSlew.calculate(newTurn); - turn += (newTurn - turn) * 0.75; + private final SlewRateLimiter turnSlew = new SlewRateLimiter(Drivetrain.TURN_SLEW_RATE); + private final LinearFilter turnLinearFilter = LinearFilter.singlePoleIIR(Drivetrain.TURN_SLEW_RATE, 0.02); + + public double filterTurn(double turn) { + turn = spow(turn, Drivetrain.TURN_POWER); + turn = turnSlew.calculate(turn); + turn = turnLinearFilter.calculate(turn); + if (Math.abs(turn) < Drivetrain.ANGLE_DEADBAND) { return 0; } else { diff --git a/src/main/java/com/stuypulse/robot/subsystems/TankDrive.java b/src/main/java/com/stuypulse/robot/subsystems/TankDrive.java index 4ff0c91..4b7efac 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/TankDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/TankDrive.java @@ -150,7 +150,7 @@ public double getVelocity() { /*************** * ROBOT ANGLE * - ***************/ + ***************/ //Gets current Angle of the Robot as a double (contiuous / not +-180) public Rotation2d getRawGyroAngle() { @@ -186,18 +186,22 @@ public void stop() { } public void tankDrive(double left, double right) { - tankDrive.tankDrive(left, right, true); + tankDrive.tankDrive(left, right, false); } public void arcadeDrive(double speed, double rotation) { - tankDrive.arcadeDrive(speed, rotation, true); + tankDrive.arcadeDrive(speed, rotation, false); } public void curvatureDrive(double xSpeed, double zRotation) { tankDrive.curvatureDrive(xSpeed, zRotation * Math.max(Math.abs(xSpeed), 0.33), true); } - + public void tankDriveVolts(double leftVolts, double rightVolts) { + leftMotors[0].setVoltage(rightVolts); + rightMotors[0].setVoltage(rightVolts); + tankDrive.feed(); + } /********************** * ODOMETRY FUNCTIONS * **********************/ @@ -250,6 +254,8 @@ public void periodic() { updateOdometry(); field.setRobotPose(getPose()); + tankDrive.feed(); + // Smart Dashboard Information SmartDashboard.putNumber("TankDrive/Roll (deg)", getRoll().getDegrees());