diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4853c5c..ebe5ea0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -48,13 +48,13 @@ public static final class CANIds { // Intake CAN IDs - public static final int kIntakeCanID = 8; - public static final int kIntakeTurnPitchCanID = 10; + public static final int kIntakeCanID = 12; + public static final int kIntakeTurnPitchCanID = 61; // Shooter IDs - public static final int kShooterAngleMotorCanId = 29; - public static final int kShooterLeftMotorCanId = 59; - public static final int kShooterRightMotorCanId = 62; + public static final int kShooterAngleMotorCanId = 10; + public static final int kShooterLeftMotorCanId = 62; + public static final int kShooterRightMotorCanId = 59; // } @@ -164,9 +164,9 @@ public static final class TransferConstants { public static final class ShooterConstants { - public static final double shooterTopP = 0; - public static final double shooterTopI = 0; - public static final double shooterTopD = 0; + public static final double shooterAngleP = 0; + public static final double shooterAngleI = 0; + public static final double shooterAngleD = 0.001; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1ed1996..0b2a71d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -41,7 +41,6 @@ public class RobotContainer { // Secondary controller CommandXboxController secondaryController = new CommandXboxController(OIConstants.kSecondaryControllerPort); - SendableChooser autoSelector = new SendableChooser<>(); /** @@ -50,21 +49,23 @@ public class RobotContainer { public RobotContainer() { // Configure the button bindings configureButtonBindings(); - + // Configure Oblog logger Logger.configureLoggingAndConfig(this, true); // Auton selector config - // autoSelector.setDefaultOption("Test", new Test4And1Auto()); + // autoSelector.setDefaultOption("Test", new Test4And1Auto()); autoSelector.addOption("No auto", null); // Configure default commands - /*◇─◇──◇─◇ - Drivetrain - ◇─◇──◇─◇*/ - + /* + * ◇─◇──◇─◇ + * Drivetrain + * ◇─◇──◇─◇ + */ + robotDrive.setDefaultCommand( // The left stick controls translation of the robot. // Turning is controlled by the X axis of the right stick. @@ -76,14 +77,14 @@ public RobotContainer() { true, true), robotDrive)); - /*◇─◇──◇─◇ - Elevator - ◇─◇──◇─◇*/ + /* + * ◇─◇──◇─◇ + * Elevator + * ◇─◇──◇─◇ + */ - } - /** * Use this method to define your button->command mappings. Buttons can be * created by @@ -95,43 +96,67 @@ public RobotContainer() { */ private void configureButtonBindings() { - /*◇─◇──◇─◇ - Drivetrain - ◇─◇──◇─◇*/ + /* + * ◇─◇──◇─◇ + * Drivetrain + * ◇─◇──◇─◇ + */ driverController.x() .whileTrue(new RunCommand( () -> robotDrive.setX(), robotDrive)); - driverController.start().onTrue(new InstantCommand(()->robotDrive.zeroHeading())); + driverController.start().onTrue(new InstantCommand(() -> robotDrive.zeroHeading())); + /* + * ◇─◇──◇─◇ + * ✨Shooter✨ + * ◇─◇──◇─◇ + */ - /*◇─◇──◇─◇ - ✨Shooter✨ - ◇─◇──◇─◇*/ - - driverController.y() + driverController.y() + .onTrue(new RunCommand( + () -> shooter.moveShooterAngle(0.5), + shooter)); + + driverController.rightTrigger() .whileTrue(new RunCommand( () -> shooter.setShooterSpeed(0.5), + shooter)) + .onFalse(new RunCommand( + () -> shooter.setShooterSpeed(0), shooter)); - - /*◇─◇──◇─◇ - ✨FloopIntake✨ - ◇─◇──◇─◇*/ - driverController.rightBumper() + ; + driverController.rightBumper() .whileTrue(new RunCommand( - () -> intake.setIntakeSpeed(0.2), - intake )); - driverController.leftBumper() + () -> shooter.setShooterSpeed(1), + shooter)) + .onFalse(new RunCommand( + () -> shooter.setShooterSpeed(0), + shooter)); + ; + + driverController.b() .whileTrue(new RunCommand( - () -> intake.setIntakeSpeed(-0.2), - intake )); -/* - secondaryController.x() - .onTrue(new RunCommand( () -> intake.set) ) + () -> shooter.moveShooterAngle(-0.5), + shooter)); - + /* + * ◇─◇──◇─◇ + * ✨FloopIntake✨ + * ◇─◇──◇─◇ + */ +/* + driverController.rightBumper() + .onTrue(new RunCommand( + () -> intake.setIntakeSpeed(0.2), + intake)); + driverController.leftBumper() + .onTrue(new RunCommand( + () -> intake.setIntakeSpeed(-0.2), + intake)); */ + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 3ff0fa9..efe87c7 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -43,7 +43,7 @@ public class Shooter extends SubsystemBase implements Loggable{ - private final CANSparkFlex shooterAngleMotor = new CANSparkFlex(CANIds.kShooterAngleMotorCanId, MotorType.kBrushless); + private final CANSparkMax shooterAngleMotor = new CANSparkMax(CANIds.kShooterAngleMotorCanId, MotorType.kBrushless); private final CANSparkFlex shooterLeft = new CANSparkFlex(CANIds.kShooterLeftMotorCanId, MotorType.kBrushless); private final CANSparkFlex shooterRight = new CANSparkFlex(CANIds.kShooterRightMotorCanId, MotorType.kBrushless); @@ -51,10 +51,10 @@ public class Shooter extends SubsystemBase implements Loggable{ private final RelativeEncoder shooterLeftEncoder = shooterAngleMotor.getEncoder(); private final RelativeEncoder shooterRightEncoder = shooterAngleMotor.getEncoder(); - private final PIDController shooterRPMController = new PIDController( - ShooterConstants.shooterTopP, - ShooterConstants.shooterTopI, - ShooterConstants.shooterTopD + private final PIDController shooterAngleController = new PIDController( + ShooterConstants.shooterAngleP, + ShooterConstants.shooterAngleI, + ShooterConstants.shooterAngleD ); public Shooter(){ @@ -67,6 +67,8 @@ public Shooter(){ shooterLeft.setIdleMode(IdleMode.kCoast); shooterRight.setIdleMode(IdleMode.kCoast); + shooterLeft.setInverted(true); + shooterAngleMotor.burnFlash(); shooterLeft.burnFlash(); shooterRight.burnFlash(); @@ -91,12 +93,10 @@ public void setShooterSpeed(double speed){ shooterLeft.set(speed); shooterRight.set(speed); } - @Config - public void setRPMSetpoint(double rpm){ - shooterRPMController.setSetpoint(rpm); + public void moveShooterAngle(double speed){ + shooterAngleMotor.set(speed); } - /*◇─◇──◇─◇ ✨Commands✨