diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0af3739..3e1c75f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -44,6 +44,8 @@ public class RobotContainer { m_robotMap.gyro); private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter); + + private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); //private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); @@ -110,12 +112,16 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + + /* Operator Buttons */ new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); - - /* Operator Buttons */ - + + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors())); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 72292b0..1259982 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -11,10 +11,13 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_Pigeon2; +import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj.motorcontrol.Talon; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.ShooterConstants; @@ -63,12 +66,12 @@ public class RobotMap { public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); /* Shooter Subsystem */ - public final WPI_TalonFX leftShooter = new WPI_TalonFX(ShooterConstants.LEFT_SHOOTER_ID); - public final WPI_TalonFX rightShooter = new WPI_TalonFX(ShooterConstants.RIGHT_SHOOTER_ID); + public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID); + public final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID); /* Intake Subsystem */ - // public final CANSparkMax intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_MOTOR_ID, MotorType.kBrushless); - // public final CANSparkMax pivotMotor = new CANSparkMax(IntakeConstants.PIVOT_MOTOR_ID, MotorType.kBrushless); + public final CANSparkMax intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless); + public final CANSparkMax pivotMotor = new CANSparkMax(IntakeConstants.PIVOT_MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless); void configureLEDMotorControllers() { } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index dd1d96f..b571827 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -21,17 +21,26 @@ public class Intake extends SubsystemBase { this.pivot = pivot; } + //hanoff public void spinIntakeMotor() { intakeMotor.set(IntakeConstants.INTAKE_SPEED); } - public void rotateArmIn() { //handoff + //Rotate robot in for handoff + public void rotateArmIn() { //TODO + pivot.set(IntakeConstants.PIVOT_SPEED); } - public void rotateArmOut() { //intake + //Rotates robot out for intake + public void rotateArmOut() { //TODO - spinIntakeMotor(); + pivot.set(-IntakeConstants.INTAKE_SPEED); + + } + + public void stopIntakeMotors() { + intakeMotor.set(0); } public void rotateArm() {