diff --git a/src/main/java/frc/robot/commands/auto/SimpleAutoPlanLeft.java b/src/main/java/frc/robot/commands/auto/SimpleAutoPlanLeft.java index 1f9ac19..ec284ab 100644 --- a/src/main/java/frc/robot/commands/auto/SimpleAutoPlanLeft.java +++ b/src/main/java/frc/robot/commands/auto/SimpleAutoPlanLeft.java @@ -25,7 +25,7 @@ /** An example command that uses an example subsystem. */ public class SimpleAutoPlanLeft extends SequentialCommandGroup { @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" }) - public static double SHOOT_SPEAKER_SPEED = 0.5; + public static double SHOOT_SPEAKER_SPEED = 0.7; public static final double SPEAKER_HEIGHT = 2 * METERS; public static Rotation2d SHOOT_ANGLE = Rotation2d.fromDegrees(60); public static double MOVE_SPEED = 1; @@ -41,11 +41,11 @@ public SimpleAutoPlanLeft(TranslationalDrivebase translational, RotationalDriveb addCommands( new RotateShooterCommand(SHOOT_ANGLE, shooter), - // new WaitCommand(1.0), - new SimpleShootCommand(SHOOT_SPEAKER_SPEED, shooter, indexer), - new TranslationCommand(new Translation2d(0, 3), MOVE_SPEED, translational), - new RotationCommand(Rotation2d.fromDegrees(sign * 45), rotational, localizer), - new TranslationCommand(new Translation2d(0, 5), MOVE_SPEED, translational) + new WaitCommand(1.0), + new SimpleShootCommand(SHOOT_SPEAKER_SPEED, shooter, indexer) + // new TranslationCommand(new Translation2d(0, 3), MOVE_SPEED, translational), + // new RotationCommand(Rotation2d.fromDegrees(sign * 45), rotational, localizer), + // new TranslationCommand(new Translation2d(0, 5), MOVE_SPEED, translational) // new TranslationCommand(new Translation2d(sign * 0, 0.5), 1, translational), // new RotationCommand(Rotation2d.fromDegrees(45).times(-sign), rotational, localizer), diff --git a/src/main/java/frc/robot/commands/shooter/ShooterControlCommand.java b/src/main/java/frc/robot/commands/shooter/ShooterControlCommand.java index 818d2f1..98721da 100644 --- a/src/main/java/frc/robot/commands/shooter/ShooterControlCommand.java +++ b/src/main/java/frc/robot/commands/shooter/ShooterControlCommand.java @@ -35,7 +35,7 @@ public ShooterControlCommand(ShooterSubsystem shooter, IndexerSubsystem indexer, @Override public void initialize() { SmartDashboard.putNumber("targetAngle", ShooterSubsystem.MIN_ANGLE); - shooter.setRotation(55 * DEG); + // shooter.setRotation(55 * DEG); angleLog = new DoubleLogEntry(log, "/my/angle"); shotLog = new BooleanLogEntry(log, "/my/shot"); @@ -45,7 +45,7 @@ public void initialize() { public void execute() { if (Math.abs(xbox.getRightY()) > 0.1) { shooterAngle += -xbox.getRightY() * 80 * DEG * 0.02;// = SmartDashboard.getNumber("targetAngle", ShooterSubsystem.MIN_ANGLE); - shooterAngle = Math.min(shooterAngle, ShooterSubsystem.MAX_ANGLE); + shooterAngle = Math.min(shooterAngle, ShooterSubsystem.MAX_ANGLE - 0.1); shooterAngle = Math.max(shooterAngle, ShooterSubsystem.MIN_ANGLE); // shooter.setRotation(55 * DEG); @@ -56,7 +56,7 @@ public void execute() { if (xbox.getRightTriggerAxis() > 0.5) { - double shooterMag = 0.8;//xbox.getRightTriggerAxis() * 0.5 * shooterDir; + double shooterMag = 0.7;//xbox.getRightTriggerAxis() * 0.5 * shooterDir; shooter.shoot(shooterMag); shotLog.append(true); angleLog.append(shooterAngle);