diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 24e4ab0..525b6a7 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -41,10 +41,10 @@ public final class Constants { public static final double TURBO_SPEED = 1.0; public static final class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = -0.362548828125 - 0.25; //TODO: per robot swerve module offsets. - public static final double FRONT_RIGHT_ROT_OFFSET = -0.138916015625 - 0.25; //TODO: per robot swerve module offsets. - public static final double BACK_LEFT_ROT_OFFSET = 0.024169921875 - 0.25; //TODO: per robot swerve module offsets. - public static final double BACK_RIGHT_ROT_OFFSET = -0.1435546875 - 0.25; //TODO: per robot swerve module offsets. + public static final double FRONT_LEFT_ROT_OFFSET = -0.363525390625 - 0.25; //TODO: per robot swerve module offsets. + public static final double FRONT_RIGHT_ROT_OFFSET = 0.365966796875 + 0.25; //TODO: per robot swerve module offsets. + public static final double BACK_LEFT_ROT_OFFSET = 0.02392578125 - 0.25; //TODO: per robot swerve module offsets. + public static final double BACK_RIGHT_ROT_OFFSET = 0.35498046875 + 0.25; //TODO: per robot swerve module offsets. } public static final class IDs { @@ -146,7 +146,17 @@ public final class Constants { public static final double LEFT_AXIS_DEADBAND = 0.1; } + public static final class IntakeConstants { + public static final class IDs{ + public static final int PIVOTMOTOR_ID = 17; + public static final int INTAKEMOTOR_ID = 18; + } + public static final double IN_POSITION =0.0; + public static final double OUT_POSITION =0.0; + public static final double INTAKE_SPEED= 0.0; + public static final double OUTTAKE_SPEED= 0.0; + } public static final class ShooterConstants { public static final int LEFT_SHOOTER_ID = 15; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6f7feda..262f48c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -21,12 +21,12 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; - +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // Autos import frc4388.utility.controller.VirtualController; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; - +import frc4388.robot.subsystems.Intake; // Subsystems import frc4388.robot.subsystems.Shooter; @@ -51,7 +51,7 @@ public class RobotContainer { /* Subsystems */ public final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter); // private final LED m_robotLED = new LED(); - + public final Intake m_robotIntake= new Intake(m_robotMap.pivotArm, m_robotMap.intakeWheel); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, @@ -59,7 +59,8 @@ public class RobotContainer { m_robotMap.gyro); - /* Controllers */ + + /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID); @@ -155,6 +156,21 @@ public class RobotContainer { // ? /* Operator Buttons */ + /*Intake */ + DualJoystickButton(getDeadbandedDriverController(), getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.PIDOut()), + new InstantCommand(() -> m_robotIntake.spinIntakeMotor()) + )) + .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotor())); + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.PIDIn())); + + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(()->m_robotIntake.handoff())) + .onFalse(new InstantCommand(()-> m_robotIntake.stopIntakeMotor())); + /*Shooter*/ new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 7eb76d8..a3d9e45 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; +import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.Constants.ShooterConstants; // import edu.wpi.first.wpilibj.motorcontrol.Spark; // import frc4388.robot.Constants.LEDConstants; @@ -56,10 +57,14 @@ public class RobotMap { public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); + public final TalonFX pivotArm = new TalonFX(IntakeConstants.IDs.PIVOTMOTOR_ID); + public final TalonFX intakeWheel = new TalonFX(IntakeConstants.IDs.INTAKEMOTOR_ID); + /* Shooter Subsystem */ public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID); public final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID); + void configureDriveMotorControllers() { // initialize SwerveModules this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java new file mode 100644 index 0000000..9e7b78a --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -0,0 +1,57 @@ +package frc4388.robot.subsystems; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.IntakeConstants; + +public class Intake extends SubsystemBase{ + private TalonFX pivotMotor; + private TalonFX intakeMotor; + + public Intake(TalonFX pivotTalonFX, TalonFX intakeTalonFX){ + pivotMotor = pivotTalonFX; + intakeMotor = intakeTalonFX; + + pivotMotor.setNeutralMode(NeutralModeValue.Brake); + intakeMotor.setNeutralMode(NeutralModeValue.Brake); + + var PIDConfigs = new Slot0Configs(); + PIDConfigs.kP = 0; + PIDConfigs.kI = 0; + PIDConfigs.kD = 0; + pivotMotor.getConfigurator().apply(PIDConfigs); + + } + + public void PIDPosition(double position) { + var request = new PositionVoltage(position); + pivotMotor.setControl(request); + } + + public void PIDIn() { + PIDPosition(IntakeConstants.IN_POSITION); + } + + public void PIDOut() { + PIDPosition(IntakeConstants.OUT_POSITION); + } + + public void handoff(){ + intakeMotor.set(IntakeConstants.OUTTAKE_SPEED); + } + + public void spinIntakeMotor(){ + intakeMotor.set(IntakeConstants.INTAKE_SPEED); + } + public void stopIntakeMotor(){ + intakeMotor.set(0); + } + public void stopArmMotor(){ + pivotMotor.set(0); + } + +}