Basic Endeffector outline

src/main/java/frc/robot/RobotContainer.java

@@ -21,6 +21,7 @@
 import frc.robot.commands.ResetGyro;
 import frc.robot.commands.RunAlgaeIntake;
 import frc.robot.commands.RunCoralOuttake;
+import frc.robot.commands.RunEndEffectorIntake;
 import frc.robot.commands.SwerveCharacterization;
 import frc.robot.constants.ROBOT;
 import frc.robot.constants.SWERVE;
@@ -30,6 +31,7 @@
 import frc.robot.subsystems.AlgaeIntake;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.subsystems.CoralOuttake;
+import frc.robot.subsystems.EndEffector;
 import frc.robot.utils.SysIdUtils;
 import frc.robot.utils.Telemetry;
 
@@ -45,7 +47,7 @@ public class RobotContainer {
   private final Telemetry m_telemetry = new Telemetry();
   private final CoralOuttake m_coralOuttake = new CoralOuttake();
   private final AlgaeIntake m_algaeIntake = new AlgaeIntake();
-
+  private final EndEffector m_endEffector = new EndEffector();
   // Replace with CommandPS4Controller or CommandJoystick if needed
   private final Joystick leftJoystick = new Joystick(USB.leftJoystick);
   private final SendableChooser<Command> m_sysidChooser = new SendableChooser<>();
@@ -173,8 +175,9 @@ private void configureBindings() {
     m_driverController
         .rightBumper()
         .whileTrue(new RunCoralOuttake(m_coralOuttake, -0.15)); // intake
-    m_driverController.x().whileTrue(new RunAlgaeIntake(m_algaeIntake, 0.5)); // outtake
+    m_driverController.x().whileTrue(new RunAlgaeIntake(m_algaeIntake, 0.5)); // ou,ttake
     m_driverController.y().whileTrue(new RunAlgaeIntake(m_algaeIntake, -0.5)); // intake
+    m_driverController.leftTrigger().whileTrue(new RunEndEffectorIntake(m_endEffector, 0.4414)); //
   }
 
   /**

src/main/java/frc/robot/commands/RunEndEffectorIntake.java

@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.EndEffector;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class RunEndEffectorIntake extends Command {
+
+  private final EndEffector m_EndEffector;
+  private final double m_speed;
+
+  /** Creates a new SetAlgaeIntakeSpeed. */
+  public RunEndEffectorIntake(EndEffector endeffector, double speed) {
+    m_EndEffector = endeffector;
+    m_speed = speed;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(m_EndEffector);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_EndEffector.setPercentOutput(m_speed);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_EndEffector.setPercentOutput(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/constants/ALPHABOTCAN.java

@@ -22,4 +22,5 @@ public class ALPHABOTCAN {
 
   public static final int coralOuttakeMotor = 30;
   public static final int algaeMotor = 31;
+  public static final int endEffector = 35;
 }

src/main/java/frc/robot/constants/ENDEFFECTOR.java

@@ -0,0 +1,17 @@
+package frc.robot.constants;
+
+public class ENDEFFECTOR {
+  public static final double kP = 0.0;
+  public static final double kI = 0.0;
+  public static final double kD = 0.0;
+  public static final double endEffectorGearRatio = 1.0;
+
+  // Pivot motor stuff
+  public static final double kPivotP = 0.0;
+  public static final double kPivotI = 0.0;
+  public static final double kPivotD = 0.0;
+
+  public static final double endEffectorPivotGearRatio = 1.0;
+
+  public enum STATE {}
+}

src/main/java/frc/robot/constants/V2CAN.java

@@ -25,7 +25,7 @@ public class V2CAN {
 
   public static final int endEffectorPivotMotor = 32;
   public static final int endEffectorOuttakeMotor = 35;
-  public static final int carriagePivotCanCoder = 36;
+  public static final int endEffecotrPivotCanCoder = 36;
 
   public static final int elevatorMotor1 = 33;
   public static final int elevatorMotor2 = 34;

src/main/java/frc/robot/subsystems/EndEffector.java

@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ENDEFFECTOR;
+import frc.robot.constants.V2CAN;
+import frc.robot.utils.CtreUtils;
+
+public class EndEffector extends SubsystemBase {
+  private final TalonFX m_endEffectorMotor = new TalonFX(V2CAN.endEffectorOuttakeMotor);
+
+  /** Creates a new EndEffector. */
+  public EndEffector() {
+    TalonFXConfiguration m_endEffectorMotorconfig = new TalonFXConfiguration();
+    m_endEffectorMotorconfig.Slot0.kP = ENDEFFECTOR.kP;
+    m_endEffectorMotorconfig.Slot0.kI = ENDEFFECTOR.kI;
+    m_endEffectorMotorconfig.Slot0.kD = ENDEFFECTOR.kD;
+    m_endEffectorMotorconfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+    m_endEffectorMotorconfig.Feedback.SensorToMechanismRatio = ENDEFFECTOR.endEffectorGearRatio;
+    CtreUtils.configureTalonFx(m_endEffectorMotor, m_endEffectorMotorconfig);
+  }
+
+  public void setPercentOutput(double output) {
+    m_endEffectorMotor.set(output);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

src/main/java/frc/robot/subsystems/EndEffectorWrist.java

@@ -0,0 +1,40 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ENDEFFECTOR;
+import frc.robot.constants.V2CAN;
+
+public class EndEffectorWrist extends SubsystemBase {
+
+  private final TalonFX m_pivotMotor = new TalonFX(V2CAN.endEffectorPivotMotor);
+  private final CANcoder m_pivotEncoder = new CANcoder(V2CAN.endEffecotrPivotCanCoder);
+
+  private final NeutralModeValue m_neutralMode = NeutralModeValue.Brake;
+
+  private final MotionMagicTorqueCurrentFOC m_request = new MotionMagicTorqueCurrentFOC(0);
+
+  // private doulbe m_desiredRotations =
+  /** Creates a nepw EndEffectorWrist. */
+  public EndEffectorWrist() {
+    TalonFXConfiguration configuration = new TalonFXConfiguration();
+    configuration.Slot0.kP = ENDEFFECTOR.kP;
+    configuration.Slot0.kI = ENDEFFECTOR.kI;
+    configuration.Slot0.kD = ENDEFFECTOR.kD;
+    configuration.MotorOutput.NeutralMode = m_neutralMode;
+    configuration.Feedback.RotorToSensorRatio = ENDEFFECTOR.endEffectorGearRatio;
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}