From 23a7418d6e06dfe576b6533b6edc05ee2aa9f8dd Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 3 Dec 2024 17:40:30 -0700 Subject: [PATCH] add changes --- src/main/java/frc4388/robot/Constants.java | 20 +++++-- .../java/frc4388/robot/RobotContainer.java | 25 ++++++-- src/main/java/frc4388/robot/RobotMap.java | 4 ++ .../java/frc4388/robot/subsystems/Intake.java | 57 +++++++++++++++++++ 4 files changed, 97 insertions(+), 9 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Intake.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index ffc8487..3c0d27c 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.0; //TODO: per robot swerve module offsets. - public static final double FRONT_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets. - public static final double BACK_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets. - public static final double BACK_RIGHT_ROT_OFFSET = 0.0; //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,4 +146,16 @@ 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; + } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index eeb3054..55e1c47 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.LED; import frc4388.robot.subsystems.SwerveDrive; @@ -48,7 +48,7 @@ public class RobotContainer { /* Subsystems */ // 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, @@ -56,7 +56,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); @@ -151,7 +152,21 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); // ? /* 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())); // ? /* Programer Buttons (Controller 3)*/ // * /* Auto Recording */ diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 25bf208..143d77c 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 edu.wpi.first.wpilibj.motorcontrol.Spark; // import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; @@ -55,6 +56,9 @@ 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); + 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); + } + +}