From 45070bc6dd857cd9827b04446b013cb9f80ba2fc Mon Sep 17 00:00:00 2001 From: Ray Cai Flanagan Date: Fri, 22 Mar 2024 11:53:19 -0400 Subject: [PATCH] Code --- src/main/java/frc/robot/RobotContainer.java | 34 ++++++--- .../java/frc/robot/robot/commands/Auton.java | 75 +++++++++++++------ .../robot/commands/{Arm.java => MoveArm.java} | 6 +- .../robot/subsystems/arm/ArmSubsystem.java | 4 + .../swerve/rev/RevSwerveConfig.java | 4 +- 5 files changed, 85 insertions(+), 38 deletions(-) rename src/main/java/frc/robot/robot/commands/{Arm.java => MoveArm.java} (82%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 624a1a3..0906f56 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; // Commands @@ -18,7 +19,7 @@ import edu.wpi.first.wpilibj.Timer; public class RobotContainer { - private static final double ARM_SPEED = 0.25; + private static final double ARM_SPEED = 0.5; // Controllers private final CommandXboxController driver = new CommandXboxController(0); @@ -40,7 +41,7 @@ public class RobotContainer { public RobotContainer() { // Move the arm up and down - Arm armCommand = new Arm( + MoveArm armCommand = new MoveArm( armSubsystem, () -> ( operator.leftBumper().getAsBoolean() @@ -53,23 +54,34 @@ public RobotContainer() { armSubsystem.setDefaultCommand(armCommand); - // Move the arm to the amp - Arm amp = new Arm(armSubsystem, () -> 5, () -> 0.11899); - operator.b().whileTrue(amp); + // Amp + operator.y().whileTrue( + new SequentialCommandGroup( + new MoveArm(armSubsystem, () -> 5, () -> 0.10), + new Shoot(shootSubsystem, () -> 1), + new Intake(intakeSubsystem, () -> 1) + ) + ); - // Move the arm to the speaker - Arm speaker = new Arm(armSubsystem, () -> 5, () -> 0.33); - operator.x().whileTrue(speaker); + // Speaker + operator.x().whileTrue( + new MoveArm(armSubsystem, () -> 5, () -> 0.33) + // new SequentialCommandGroup( + // new Shoot(shootSubsystem, () -> 1), + // new Intake(intakeSubsystem, () -> 1) + // ) + ); // Intake - Intake intakeCommand = new Intake(intakeSubsystem, () -> operator.getRawAxis(leftJoystick)); + Intake intakeCommand = new Intake(intakeSubsystem, () -> operator.getRawAxis(leftJoystick)*.8); intakeSubsystem.setDefaultCommand(intakeCommand); // Shoot Shoot shootCommand = new Shoot( - shootSubsystem, - () -> operator.getRawAxis(rightJoystick)); + shootSubsystem, + () -> operator.getRawAxis(rightJoystick) + ); shootSubsystem.setDefaultCommand(shootCommand); diff --git a/src/main/java/frc/robot/robot/commands/Auton.java b/src/main/java/frc/robot/robot/commands/Auton.java index 2488137..ce410fe 100644 --- a/src/main/java/frc/robot/robot/commands/Auton.java +++ b/src/main/java/frc/robot/robot/commands/Auton.java @@ -58,19 +58,7 @@ private double getRotation(DoubleSupplier rotationSup) { return MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); } - private void shootSpeaker(Timer timer) { - if (timer.get() < 2) { - armSubsystem.moveArm(1, 0.32); - } - else if(timer.get() < 3) { - shootSubsystem.shoot(1); - } - else if(timer.get() < 4) { - intakeSubsystem.intake(1); - } - } - - // Left side of speaker + // Short side of speaker private void auton1() { if (timer.get() < 2) { armSubsystem.moveArm(1, 0.32); @@ -81,22 +69,44 @@ else if(timer.get() < 3) { else if(timer.get() < 4) { intakeSubsystem.intake(1); } - else if(timer.get() < 5) { - intakeSubsystem.stopIntake(); + else if(timer.get() < 5.2) { shootSubsystem.stopShoot(); armSubsystem.stopArm(); swerveSubsystem.drive( getTranslation( - () -> -0.5, - () -> 0.1), - getRotation(() -> 0), + () -> -.6, //-0.6 backwards + () -> .6), // right + getRotation(() -> -9), //0 true, true ); } else if(timer.get() < 7) { + swerveSubsystem.drive( + getTranslation( + () -> 0.25, + () -> 0.2), + getRotation(() -> 2 + ), + true, + true + ); + intakeSubsystem.stopIntake(); + } + else if(timer.get() < 7.5) { + intakeSubsystem.intake(-0.1); + } + else if(timer.get() < 8) { swerveSubsystem.stopDrive(); + shootSubsystem.shoot(1); + } + else if(timer.get() < 8.5) { + intakeSubsystem.intake(1); + } + else if(timer.get() < 10) { + intakeSubsystem.stopIntake(); + shootSubsystem.stopShoot(); } } @@ -151,7 +161,7 @@ else if(timer.get() < 8) { } } - // Right side of speaker + // Long side of speaker private void auton3() { if (timer.get() < 2) { armSubsystem.moveArm(1, 0.32); @@ -163,21 +173,42 @@ else if(timer.get() < 4) { intakeSubsystem.intake(1); } else if(timer.get() < 5) { - intakeSubsystem.stopIntake(); shootSubsystem.stopShoot(); armSubsystem.stopArm(); swerveSubsystem.drive( getTranslation( - () -> -0.5, - () -> -0.5), + () -> -0.6, + () -> -0.6), getRotation(() -> 0), true, true ); } else if(timer.get() < 7) { + swerveSubsystem.drive( + getTranslation( + () -> 0.25, + () -> 0.2), + getRotation(() -> 0), + true, + true + ); + intakeSubsystem.stopIntake(); + } + else if(timer.get() < 7.5) { + intakeSubsystem.intake(-0.1); + } + else if(timer.get() < 8) { swerveSubsystem.stopDrive(); + shootSubsystem.shoot(1); + } + else if(timer.get() < 8.5) { + intakeSubsystem.intake(1); + } + else if(timer.get() < 10) { + intakeSubsystem.stopIntake(); + shootSubsystem.stopShoot(); } } diff --git a/src/main/java/frc/robot/robot/commands/Arm.java b/src/main/java/frc/robot/robot/commands/MoveArm.java similarity index 82% rename from src/main/java/frc/robot/robot/commands/Arm.java rename to src/main/java/frc/robot/robot/commands/MoveArm.java index e311fcd..f0dbf0d 100644 --- a/src/main/java/frc/robot/robot/commands/Arm.java +++ b/src/main/java/frc/robot/robot/commands/MoveArm.java @@ -7,20 +7,20 @@ import frc.robot.robot.subsystems.arm.ArmSubsystem; -public class Arm extends Command { +public class MoveArm extends Command { private DoubleSupplier speedSup; private DoubleSupplier angleSup; private ArmSubsystem armSubsystem; - public Arm(ArmSubsystem subsystem, DoubleSupplier speedSup) { + public MoveArm(ArmSubsystem subsystem, DoubleSupplier speedSup) { this.armSubsystem = subsystem; addRequirements(armSubsystem); this.speedSup = speedSup; } - public Arm(ArmSubsystem subsystem, DoubleSupplier speedSup, DoubleSupplier angleSup) { + public MoveArm(ArmSubsystem subsystem, DoubleSupplier speedSup, DoubleSupplier angleSup) { this.armSubsystem = subsystem; addRequirements(armSubsystem); diff --git a/src/main/java/frc/robot/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/robot/subsystems/arm/ArmSubsystem.java index ce9e53d..635b6b0 100644 --- a/src/main/java/frc/robot/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/robot/subsystems/arm/ArmSubsystem.java @@ -23,11 +23,15 @@ public ArmSubsystem() { } public void moveArm(double speed) { + if(getArmEncoder() < 0.09 && speed < 0) return; + leftArmMotor.set(ControlMode.PercentOutput, speed); rightArmMotor.set(ControlMode.PercentOutput, -speed); } public void moveArm(double speed, double targetAngle) { + if(targetAngle < 0.09 && speed < 0) return; + double encoderVal = armEncoder.getOutput(); leftArmMotor.set(ControlMode.PercentOutput, (targetAngle - encoderVal) * speed); diff --git a/src/main/java/frc/robot/robot/subsystems/swerve/rev/RevSwerveConfig.java b/src/main/java/frc/robot/robot/subsystems/swerve/rev/RevSwerveConfig.java index 78ecdfc..16551cf 100644 --- a/src/main/java/frc/robot/robot/subsystems/swerve/rev/RevSwerveConfig.java +++ b/src/main/java/frc/robot/robot/subsystems/swerve/rev/RevSwerveConfig.java @@ -99,9 +99,9 @@ public class RevSwerveConfig { /* Swerve Profiling Values */ /** Meters per Second */ - public static final double maxSpeed = 2; + public static final double maxSpeed = 20; /** Radians per Second */ - public static final double maxAngularVelocity = 9; + public static final double maxAngularVelocity = 20; public RevSwerveConfig() { canCoderConfig = new CANcoderConfiguration();