diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fec2730..4853c5c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,17 +33,18 @@ public final class Constants { public static final class CANIds { // SPARK MAX CAN IDs - public static final int kFrontLeftDrivingCanId = 13; - public static final int kRearLeftDrivingCanId = 7; + public static final int kFrontLeftDrivingCanId = 20; + public static final int kFrontLeftTurningCanId = 13; public static final int kFrontRightDrivingCanId = 40; - public static final int kRearRightDrivingCanId = 55; + public static final int kRearRightTurningCanId = 12; - public static final int kFrontLeftTurningCanId = 20; + public static final int kRearLeftDrivingCanId = 7; public static final int kRearLeftTurningCanId = 23; + public static final int kRearRightDrivingCanId = 55; public static final int kFrontRightTurningCanId = 36; - public static final int kRearRightTurningCanId = 12; + // Intake CAN IDs @@ -51,7 +52,9 @@ public static final class CANIds { public static final int kIntakeTurnPitchCanID = 10; // Shooter IDs - public static final int kShooterTopMotorCanId = 29; + public static final int kShooterAngleMotorCanId = 29; + public static final int kShooterLeftMotorCanId = 59; + public static final int kShooterRightMotorCanId = 62; // } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 90f4f66..1ed1996 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -118,6 +119,14 @@ private void configureButtonBindings() { /*◇─◇──◇─◇ ✨FloopIntake✨ ◇─◇──◇─◇*/ + driverController.rightBumper() + .whileTrue(new RunCommand( + () -> intake.setIntakeSpeed(0.2), + intake )); + driverController.leftBumper() + .whileTrue(new RunCommand( + () -> intake.setIntakeSpeed(-0.2), + intake )); /* secondaryController.x() .onTrue(new RunCommand( () -> intake.set) ) diff --git a/src/main/java/frc/robot/subsystems/FloopIntake.java b/src/main/java/frc/robot/subsystems/FloopIntake.java index 018a37f..52a8615 100644 --- a/src/main/java/frc/robot/subsystems/FloopIntake.java +++ b/src/main/java/frc/robot/subsystems/FloopIntake.java @@ -45,7 +45,7 @@ import frc.robot.Constants.FloopIntakeConstants; import frc.robot.Constants.CANIds; -public class FloopIntake{ +public class FloopIntake extends SubsystemBase implements Loggable{ private final CANSparkMax intake = new CANSparkMax(CANIds.kIntakeCanID, MotorType.kBrushless); private final CANSparkMax intakeTurnPitch = new CANSparkMax(CANIds.kIntakeTurnPitchCanID, MotorType.kBrushless); @@ -100,6 +100,11 @@ public void setPitchSetpoint(double setpoint) { intakePIDController.setGoal(setpoint); } + @Config + public void setIntakeSpeed(double speed){ + intake.set(speed); + } + } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 09f5580..3ff0fa9 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -43,8 +43,13 @@ public class Shooter extends SubsystemBase implements Loggable{ - private final CANSparkFlex shooterMotor = new CANSparkFlex(CANIds.kShooterTopMotorCanId, MotorType.kBrushless); - private final RelativeEncoder shooterEncoder = shooterMotor.getEncoder(); + private final CANSparkFlex shooterAngleMotor = new CANSparkFlex(CANIds.kShooterAngleMotorCanId, MotorType.kBrushless); + private final CANSparkFlex shooterLeft = new CANSparkFlex(CANIds.kShooterLeftMotorCanId, MotorType.kBrushless); + private final CANSparkFlex shooterRight = new CANSparkFlex(CANIds.kShooterRightMotorCanId, MotorType.kBrushless); + + private final RelativeEncoder shooterEncoder = shooterAngleMotor.getEncoder(); + private final RelativeEncoder shooterLeftEncoder = shooterAngleMotor.getEncoder(); + private final RelativeEncoder shooterRightEncoder = shooterAngleMotor.getEncoder(); private final PIDController shooterRPMController = new PIDController( ShooterConstants.shooterTopP, @@ -53,9 +58,18 @@ public class Shooter extends SubsystemBase implements Loggable{ ); public Shooter(){ - shooterMotor.restoreFactoryDefaults(); - shooterMotor.setIdleMode(IdleMode.kCoast); - shooterMotor.burnFlash(); + shooterAngleMotor.restoreFactoryDefaults(); + shooterLeft.restoreFactoryDefaults(); + shooterRight.restoreFactoryDefaults(); + + + shooterAngleMotor.setIdleMode(IdleMode.kCoast); + shooterLeft.setIdleMode(IdleMode.kCoast); + shooterRight.setIdleMode(IdleMode.kCoast); + + shooterAngleMotor.burnFlash(); + shooterLeft.burnFlash(); + shooterRight.burnFlash(); } @@ -74,7 +88,8 @@ public double shooterRPM(){ @Config public void setShooterSpeed(double speed){ - shooterMotor.set(speed); + shooterLeft.set(speed); + shooterRight.set(speed); } @Config @@ -82,8 +97,7 @@ public void setRPMSetpoint(double rpm){ shooterRPMController.setSetpoint(rpm); } - - + /*◇─◇──◇─◇ ✨Commands✨ ◇─◇──◇─◇*/