Merge pull request #33 from 4201VitruvianBots/End-Effector

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.commands.TestAuto1;
 import frc.robot.constants.ROBOT;
@@ -31,6 +32,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;
 
@@ -46,7 +48,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<>();
@@ -177,6 +179,7 @@ private void configureBindings() {
         .whileTrue(new RunCoralOuttake(m_coralOuttake, -0.15)); // intake
     m_driverController.x().whileTrue(new RunAlgaeIntake(m_algaeIntake, 0.5)); // outtake
     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/ENDEFFECTOR.java

@@ -0,0 +1,39 @@
+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 minAngleDegrees = 270;
+  public static final double maxAngleDegrees = 3;
+
+  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 = 70.0;
+
+  public enum WRIST_SETPOINT {
+    STOWED(0),
+    L4(0),
+    L3_L2(0);
+
+    private final double angle;
+
+    WRIST_SETPOINT(double angle) {
+      this.angle = angle;
+    }
+
+    public double get() {
+      return angle;
+    }
+  }
+
+  public enum STATE {
+    STILL,
+    MOVING
+  }
+}

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,67 @@
+// 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.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.MathUtil;
+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();
+
+  private double m_desiredRotations;
+  private boolean m_pivotState;
+
+  /** 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.endEffectorPivotGearRatio;
+  }
+
+  public void setState(boolean state) {
+    m_pivotState = state;
+  }
+
+  public boolean getState() {
+    return m_pivotState;
+  }
+
+  public void setPosition(double rotations) {
+    m_desiredRotations =
+        MathUtil.clamp(rotations, ENDEFFECTOR.minAngleDegrees, ENDEFFECTOR.maxAngleDegrees);
+  }
+
+  public double getPosition() {
+    return m_desiredRotations;
+  }
+
+  public void setPercentOutput(double speed) {
+    m_pivotMotor.set(speed);
+  }
+
+  public double getPercentOutput() {
+    return m_pivotMotor.get();
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}