add changes

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_LEFT_ROT_OFFSET  = -0.363525390625 - 0.25; //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 FRONT_RIGHT_ROT_OFFSET = 0.365966796875 + 0.25; //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_LEFT_ROT_OFFSET   = 0.02392578125 - 0.25; //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 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;
+    }
 }

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,6 +56,7 @@ public class RobotContainer {
                                               
                                                                   m_robotMap.gyro);
 
+    
                                                                   /* Controllers */
     private final DeadbandedXboxController m_driverXbox   = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
     private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);    
@@ -152,6 +153,20 @@ 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()));
         // ? /* Programer Buttons (Controller 3)*/
 
         // * /* Auto Recording */

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);

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);
+    }
+
+}