diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ba1826e..9109cb3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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 m_sysidChooser = new SendableChooser<>(); @@ -175,6 +177,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)); // } /** diff --git a/src/main/java/frc/robot/commands/RunEndEffectorIntake.java b/src/main/java/frc/robot/commands/RunEndEffectorIntake.java new file mode 100644 index 0000000..a9530ca --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc/robot/constants/ENDEFFECTOR.java b/src/main/java/frc/robot/constants/ENDEFFECTOR.java new file mode 100644 index 0000000..0cb7a83 --- /dev/null +++ b/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 + } +} diff --git a/src/main/java/frc/robot/constants/V2CAN.java b/src/main/java/frc/robot/constants/V2CAN.java index d5d6b8c..e45a9bb 100644 --- a/src/main/java/frc/robot/constants/V2CAN.java +++ b/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; diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java new file mode 100644 index 0000000..58afe04 --- /dev/null +++ b/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 + } +} diff --git a/src/main/java/frc/robot/subsystems/EndEffectorWrist.java b/src/main/java/frc/robot/subsystems/EndEffectorWrist.java new file mode 100644 index 0000000..012bdf9 --- /dev/null +++ b/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 + } +} diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-25.2.1.json similarity index 85% rename from vendordeps/Phoenix6-25.1.0.json rename to vendordeps/Phoenix6-25.2.1.json index 473f6a8..1397da1 100644 --- a/vendordeps/Phoenix6-25.1.0.json +++ b/vendordeps/Phoenix6-25.2.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.1.0.json", + "fileName": "Phoenix6-25.2.1.json", "name": "CTRE-Phoenix (v6)", - "version": "25.1.0", + "version": "25.2.1", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.1.0" + "version": "25.2.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.2.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -196,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -228,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -260,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -276,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -292,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -324,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -337,10 +351,26 @@ ], "simMode": "swsim" }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.1", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -356,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -372,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.2.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true,