diff --git a/src/main/java/frc/robot/constants/CAN.java b/src/main/java/frc/robot/constants/ALPHABOTCAN.java similarity index 97% rename from src/main/java/frc/robot/constants/CAN.java rename to src/main/java/frc/robot/constants/ALPHABOTCAN.java index b04a98b..53c69c9 100644 --- a/src/main/java/frc/robot/constants/CAN.java +++ b/src/main/java/frc/robot/constants/ALPHABOTCAN.java @@ -1,6 +1,6 @@ package frc.robot.constants; -public class CAN { +public class ALPHABOTCAN { public static final String rioCanbus = "rio"; public static String driveBaseCanbus = "drivebase"; diff --git a/src/main/java/frc/robot/constants/ENDEFFECTOR.java b/src/main/java/frc/robot/constants/ENDEFFECTOR.java index 64db4de..5462f6a 100644 --- a/src/main/java/frc/robot/constants/ENDEFFECTOR.java +++ b/src/main/java/frc/robot/constants/ENDEFFECTOR.java @@ -5,4 +5,15 @@ public class ENDEFFECTOR { 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 { + + } } diff --git a/src/main/java/frc/robot/constants/V2CAN.java b/src/main/java/frc/robot/constants/V2CAN.java new file mode 100644 index 0000000..e45a9bb --- /dev/null +++ b/src/main/java/frc/robot/constants/V2CAN.java @@ -0,0 +1,40 @@ +package frc.robot.constants; + +public class V2CAN { + public static final String rioCanbus = "rio"; + public static String driveBaseCanbus = "drivebase"; + + public static final int pigeon = 9; + + public static final int frontLeftCanCoder = 10; + public static final int frontRightCanCoder = 11; + public static final int backLeftCanCoder = 12; + public static final int backRightCanCoder = 13; + + public static final int frontLeftDriveMotor = 20; + public static final int frontLeftTurnMotor = 21; + public static final int frontRightDriveMotor = 22; + public static final int frontRightTurnMotor = 23; + public static final int backLeftDriveMotor = 24; + public static final int backLeftTurnMotor = 25; + public static final int backRightDriveMotor = 26; + public static final int backRightTurnMotor = 27; + + public static final int hopperIntakeMotor1 = 30; + public static final int hopperIntakeMotor2 = 31; + + public static final int endEffectorPivotMotor = 32; + public static final int endEffectorOuttakeMotor = 35; + public static final int endEffecotrPivotCanCoder = 36; + + public static final int elevatorMotor1 = 33; + public static final int elevatorMotor2 = 34; + + public static final int groundRollerMotor = 40; + public static final int groundPivotMotor = 41; + public static final int groundPivotCanCoder = 42; + + public static final int cageGrabMotor = 50; + public static final int cageArmMotor = 51; + public static final int cageArmCanCoder = 52; +} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 3f4c188..7dad432 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -12,7 +12,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; -import frc.robot.constants.CAN; +import frc.robot.constants.ALPHABOTCAN; import frc.robot.subsystems.CommandSwerveDrivetrain; // Generated by the Tuner X Swerve Project Generator for Software Bot/AlphaBot as of 1/6/2025 @@ -140,9 +140,9 @@ public class TunerConstants { .withDriveFrictionVoltage(kDriveFrictionVoltage); // Front Left - private static final int kFrontLeftDriveMotorId = CAN.frontLeftDriveMotor; - private static final int kFrontLeftSteerMotorId = CAN.frontLeftTurnMotor; - private static final int kFrontLeftEncoderId = CAN.frontLeftCanCoder; + private static final int kFrontLeftDriveMotorId = ALPHABOTCAN.frontLeftDriveMotor; + private static final int kFrontLeftSteerMotorId = ALPHABOTCAN.frontLeftTurnMotor; + private static final int kFrontLeftEncoderId = ALPHABOTCAN.frontLeftCanCoder; private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.263916015625); private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftEncoderInverted = false; @@ -151,9 +151,9 @@ public class TunerConstants { private static final Distance kFrontLeftYPos = Inches.of(13.75); // Front Right - private static final int kFrontRightDriveMotorId = CAN.frontRightDriveMotor; - private static final int kFrontRightSteerMotorId = CAN.frontRightTurnMotor; - private static final int kFrontRightEncoderId = CAN.frontRightCanCoder; + private static final int kFrontRightDriveMotorId = ALPHABOTCAN.frontRightDriveMotor; + private static final int kFrontRightSteerMotorId = ALPHABOTCAN.frontRightTurnMotor; + private static final int kFrontRightEncoderId = ALPHABOTCAN.frontRightCanCoder; private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.485595703125); private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = false; @@ -162,9 +162,9 @@ public class TunerConstants { private static final Distance kFrontRightYPos = Inches.of(-13.75); // Back Left - private static final int kBackLeftDriveMotorId = CAN.backLeftDriveMotor; - private static final int kBackLeftSteerMotorId = CAN.backLeftTurnMotor; - private static final int kBackLeftEncoderId = CAN.backLeftCanCoder; + private static final int kBackLeftDriveMotorId = ALPHABOTCAN.backLeftDriveMotor; + private static final int kBackLeftSteerMotorId = ALPHABOTCAN.backLeftTurnMotor; + private static final int kBackLeftEncoderId = ALPHABOTCAN.backLeftCanCoder; private static final Angle kBackLeftEncoderOffset = Rotations.of(0.0556640625); private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftEncoderInverted = false; @@ -173,9 +173,9 @@ public class TunerConstants { private static final Distance kBackLeftYPos = Inches.of(13.75); // Back Right - private static final int kBackRightDriveMotorId = CAN.backRightDriveMotor; - private static final int kBackRightSteerMotorId = CAN.backRightTurnMotor; - private static final int kBackRightEncoderId = CAN.backRightCanCoder; + private static final int kBackRightDriveMotorId = ALPHABOTCAN.backRightDriveMotor; + private static final int kBackRightSteerMotorId = ALPHABOTCAN.backRightTurnMotor; + private static final int kBackRightEncoderId = ALPHABOTCAN.backRightCanCoder; private static final Angle kBackRightEncoderOffset = Rotations.of(-0.201171875); private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightEncoderInverted = false; diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/AlgaeIntake.java index 1eff0ed..09cabae 100644 --- a/src/main/java/frc/robot/subsystems/AlgaeIntake.java +++ b/src/main/java/frc/robot/subsystems/AlgaeIntake.java @@ -9,12 +9,12 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ALGAE; -import frc.robot.constants.CAN; +import frc.robot.constants.ALPHABOTCAN; import frc.robot.utils.CtreUtils; public class AlgaeIntake extends SubsystemBase { - private final TalonFX m_algaeMotor = new TalonFX(CAN.algaeMotor); + private final TalonFX m_algaeMotor = new TalonFX(ALPHABOTCAN.algaeMotor); /** Creates a new Algae. */ public AlgaeIntake() { diff --git a/src/main/java/frc/robot/subsystems/CoralOuttake.java b/src/main/java/frc/robot/subsystems/CoralOuttake.java index 1b77d78..5adf0b9 100644 --- a/src/main/java/frc/robot/subsystems/CoralOuttake.java +++ b/src/main/java/frc/robot/subsystems/CoralOuttake.java @@ -25,14 +25,14 @@ import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.CAN; +import frc.robot.constants.ALPHABOTCAN; import frc.robot.constants.CORALOUTTAKE; import frc.robot.utils.CtreUtils; public class CoralOuttake extends SubsystemBase { private boolean m_isOuttaking = false; private LinearSystem outtakePlant = LinearSystemId.createDCMotorSystem(1, 1); - private final TalonFX outtakeMotor = new TalonFX(CAN.coralOuttakeMotor); + private final TalonFX outtakeMotor = new TalonFX(ALPHABOTCAN.coralOuttakeMotor); private final DCMotorSim outtakeMotorSim = new DCMotorSim(outtakePlant, CORALOUTTAKE.gearbox); // TODO implement sim code private double m_desiredPercentOutput; diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java index b489f62..bd28e00 100644 --- a/src/main/java/frc/robot/subsystems/EndEffector.java +++ b/src/main/java/frc/robot/subsystems/EndEffector.java @@ -8,12 +8,12 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.CAN; +import frc.robot.constants.V2CAN; import frc.robot.constants.ENDEFFECTOR; import frc.robot.utils.CtreUtils; public class EndEffector extends SubsystemBase { - private final TalonFX m_endEffectorMotor = new TalonFX(CAN.endEffector); + private final TalonFX m_endEffectorMotor = new TalonFX(V2CAN.endEffectorOuttakeMotor); /** Creates a new EndEffector. */ public EndEffector() { 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..f0da8de --- /dev/null +++ b/src/main/java/frc/robot/subsystems/EndEffectorWrist.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.subsystems; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicDutyCycle; +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 + } +} diff --git a/src/main/java/frc/robot/utils/CtreUtils.java b/src/main/java/frc/robot/utils/CtreUtils.java index 6b7316b..270e577 100644 --- a/src/main/java/frc/robot/utils/CtreUtils.java +++ b/src/main/java/frc/robot/utils/CtreUtils.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; -import frc.robot.constants.CAN; +import frc.robot.constants.ALPHABOTCAN; // TODO: This class is also a mess public final class CtreUtils { @@ -23,7 +23,7 @@ public static void initPhoenixServer() { new Alert("Starting Phoenix Server at: " + Timer.getFPGATimestamp(), AlertType.kInfo); alert.set(true); if (RobotBase.isReal()) { - TalonFX dummy = new TalonFX(0, CAN.driveBaseCanbus); + TalonFX dummy = new TalonFX(0, ALPHABOTCAN.driveBaseCanbus); Timer.delay(5); dummy.close(); dummy = null;