diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index cabf7b1..ea8f489 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -9,17 +9,10 @@ package frc4388.robot; import com.ctre.phoenix6.hardware.TalonFX; -import org.photonvision.PhotonCamera; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; - import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.swerve.SwerveDrivetrain; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DigitalInput; //import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.SimConstants; -import frc4388.robot.constants.Constants.VisionConstants; import frc4388.robot.subsystems.intake.IntakeConstants; import frc4388.robot.subsystems.intake.IntakeIO; import frc4388.robot.subsystems.intake.IntakeReal; @@ -31,10 +24,7 @@ import frc4388.robot.subsystems.shooter.ShooterReal; import frc4388.robot.subsystems.swerve.SwerveDriveConstants; import frc4388.robot.subsystems.swerve.SwerveIO; import frc4388.robot.subsystems.swerve.SwerveReal; -import frc4388.robot.subsystems.vision.VisionIO; -import frc4388.robot.subsystems.vision.VisionReal; import frc4388.utility.status.FaultCANCoder; -import frc4388.utility.status.FaultPhotonCamera; import frc4388.utility.status.FaultPidgeon2; import frc4388.utility.status.FaultTalonFX; @@ -93,9 +83,9 @@ public class RobotMap { // Configure Shooter 22,23,24 - TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id); - TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id); - TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id); + // TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id); + // TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id); + // TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id); //Configure Intake 20,21 TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id); @@ -105,7 +95,9 @@ public class RobotMap { // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - shooterIO = new ShooterReal(shooter1, shooter2, indexer); + + shooterIO = new ShooterIO() {}; + // shooterIO = new ShooterReal(shooter1, shooter2, indexer); intakeIO = new IntakeReal(arm, roller); @@ -113,9 +105,9 @@ public class RobotMap { FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); - FaultTalonFX.addDevice(shooter1, "Shooter1"); - FaultTalonFX.addDevice(shooter2, "Shooter2"); - FaultTalonFX.addDevice(indexer, "Indexer"); + // FaultTalonFX.addDevice(shooter1, "Shooter1"); + // FaultTalonFX.addDevice(shooter2, "Shooter2"); + // FaultTalonFX.addDevice(indexer, "Indexer"); FaultTalonFX.addDevice(arm, "Arm"); FaultTalonFX.addDevice(roller, "Roller"); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index e49a662..0a393b8 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 34; - public static final String GIT_SHA = "2d9ed527bebbf79d7643452fe4344ad32b3fab49"; - public static final String GIT_DATE = "2026-02-07 14:51:05 MST"; - public static final String GIT_BRANCH = "Subsystem-Boilerplate"; - public static final String BUILD_DATE = "2026-02-07 14:51:24 MST"; - public static final long BUILD_UNIX_TIME = 1770501084043L; - public static final int DIRTY = 0; + public static final int GIT_REVISION = 35; + public static final String GIT_SHA = "684e18ddcc5438f14f05c5902506d14cc4cbab19"; + public static final String GIT_DATE = "2026-02-07 14:52:21 MST"; + public static final String GIT_BRANCH = "arm-test"; + public static final String BUILD_DATE = "2026-02-09 16:43:56 MST"; + public static final long BUILD_UNIX_TIME = 1770680636562L; + public static final int DIRTY = 1; private BuildConstants(){} } diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index c2ecd18..4e87f34 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -33,11 +33,11 @@ public class Intake extends SubsystemBase { public void setMode(IntakeMode mode) { switch (mode) { case Up: - // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); + io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP); break; case Down: - // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); + io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY); break; } @@ -60,17 +60,11 @@ public class Intake extends SubsystemBase { @Override public void periodic() { - - - // FaultReporter.register(this); // TODO Implement fault reporter Logger.processInputs("Intake", state); - Pose2d pose = m_swervePoseSupplier.get(); - Angle robotRot = pose.getRotation().getMeasure(); - io.updateInputs(state); } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 197a789..77570c1 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -17,14 +17,14 @@ import frc4388.utility.status.CanDevice; public class IntakeConstants { // Motor conversions - public static final double ARM_MOTOR_GEAR_RATIO = 1.; - public static final double ROLLER_MOTOR_GEAR_RATIO = 1.; + public static final double ARM_MOTOR_GEAR_RATIO = 1/100; + public static final double ROLLER_MOTOR_GEAR_RATIO = 1/3; //IDs - public static final CanDevice ARM_ID = new CanDevice("SHOOTER 1", 20); - public static final CanDevice ROLLER_ID = new CanDevice("SHOOTER 2", 21); + public static final CanDevice ARM_ID = new CanDevice("ARM", 20); + public static final CanDevice ROLLER_ID = new CanDevice("INTAKE_ROLLER", 21); // Limits @@ -44,7 +44,7 @@ public class IntakeConstants { .withKI(0.0) .withKD(10.0); - public static final Slot1Configs ROLLER_PID = new Slot1Configs() + public static final Slot0Configs ROLLER_PID = new Slot0Configs() .withKP(2.0) .withKI(0.0) .withKD(10.0); @@ -65,19 +65,6 @@ public class IntakeConstants { .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); - public static final class IDs { - public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); - } - // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() - // .withCurrentLimits( - // new CurrentLimitsConfigs() - // .withStatorCurrentLimit(40) // TODO: tune??? - // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means - // ).withMotorOutput( - // new MotorOutputConfigs() - // .withNeutralMode(NeutralModeValue.Brake) // Brake so it stop - // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - // ); public static final TalonFXConfiguration ROLLER_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index c19d527..83ec7cf 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -20,6 +20,9 @@ public class IntakeReal implements IntakeIO { TalonFX m_armMotor; TalonFX m_rollerMotor; + PositionDutyCycle armPosition = new PositionDutyCycle(0); + VelocityDutyCycle rollerVelocity = new VelocityDutyCycle(0); + public IntakeReal( TalonFX armMotor, @@ -35,6 +38,9 @@ public class IntakeReal implements IntakeIO { m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG); m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID); m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); + + armPosition.Slot = 0; + rollerVelocity.Slot = 0; } private Angle clampAng(Angle x, Angle min, Angle max){ @@ -53,9 +59,11 @@ public class IntakeReal implements IntakeIO { public void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) { state.rollerTargetVelocity = angularVelocity; // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) - double motorSpeed = angularVelocity.in(RotationsPerSecond) / IntakeConstants.ROLLER_MOTOR_GEAR_RATIO; - VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); - m_rollerMotor.setControl(velocity); + AngularVelocity motorSpeed = angularVelocity.div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO); + + // m_rollerMotor.set(motorSpeed); + // VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); + m_rollerMotor.setControl(rollerVelocity.withVelocity(motorSpeed)); } @Override @@ -63,26 +71,39 @@ public class IntakeReal implements IntakeIO { state.armTargetAngle = angle; // Assume that the angle is always accurate, because I think we will use a shaft encoder // Assume that 0 degrees = forwards. Might need an offset here - - Angle boundedAngle = clampAng(angle, IntakeConstants.ARM_LIMIT_LOWER, IntakeConstants.ARM_LIMIT_UPPER); + + // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT - double motorTargetAngle = boundedAngle.in(Rotations) / IntakeConstants.ARM_MOTOR_GEAR_RATIO; - PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); - m_armMotor.setControl(posRequest); + Angle motorAngle = angle.div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + + // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); + m_armMotor.setControl(armPosition.withPosition(motorAngle)); } + ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.2); + ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KP", 0); + ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KP", 0); + + ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2); + ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0); + ConfigurableDouble roller_kD = new ConfigurableDouble("Roller KD", 0); + @Override public void updateInputs(IntakeState state) { state.armAngle = m_armMotor.getPosition().getValue().times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); state.armMotorCurrent = m_armMotor.getStatorCurrent(false).getValue(); - // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO); - // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); - - // state.armAngle = m_armMotor.getPosition().getValue(); - // state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue(); - state.rollerVelocity = m_rollerMotor.getVelocity().getValue(); state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue(); + + IntakeConstants.ARM_PID.kP = arm_kP.get(); + IntakeConstants.ARM_PID.kI = arm_kI.get(); + IntakeConstants.ARM_PID.kD = arm_kD.get(); + m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG); + + IntakeConstants.ROLLER_PID.kP = roller_kP.get(); + IntakeConstants.ROLLER_PID.kI = roller_kI.get(); + IntakeConstants.ROLLER_PID.kD = roller_kD.get(); + m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 159c70b..0a39b1f 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -3,14 +3,11 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.InchesPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; -import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.utility.configurable.ConfigurableDouble; public class ShooterReal implements ShooterIO { @@ -27,9 +24,9 @@ public class ShooterReal implements ShooterIO { TalonFX shooter2Motor, TalonFX indexerMotor ) { - m_shooter1Motor= shooter1Motor; - m_shooter2Motor= shooter2Motor; - m_indexerMotor = indexerMotor; + m_shooter1Motor = shooter1Motor; + m_shooter2Motor = shooter2Motor; + m_indexerMotor = indexerMotor; m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); m_indexerMotor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);