diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 758db764..7562f56a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -45,6 +45,7 @@ public void robotPeriodic() { // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. + m_robotContainer.robotPeriodic(); CommandScheduler.getInstance().run(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1434cb85..be58d9ee 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -75,6 +75,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import frc.robot.commands.Drive; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Translation2d; @@ -83,6 +84,8 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PS4Controller; +import edu.wpi.first.wpilibj.PowerDistribution; +import frc.robot.commands.ManualShoot; import frc.robot.commands.AngleShooter; import frc.robot.commands.IntakeCommand; import frc.robot.settings.IntakeDirection; @@ -125,6 +128,8 @@ public class RobotContainer { private SendableChooser climbSpotChooser; private SendableChooser autoChooser; private DoubleSupplier angleSup; + private PowerDistribution PDP; + // Replace with CommandPS4Controller or CommandJoystick if needed /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -149,6 +154,7 @@ public RobotContainer() { driverController = new PS4Controller(DRIVE_CONTROLLER_ID); operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID); pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID); + PDP = new PowerDistribution(1, ModuleType.kRev); // = new PathPlannerPath(null, DEFAUL_PATH_CONSTRAINTS, null, climberExists); limelightInit(); @@ -249,11 +255,14 @@ private void configureBindings() { () -> modifyAxis(-driverController.getRawAxis(Y_AXIS), DEADBAND_NORMAL), () -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL), driverController::getL2Button)); - new Trigger(driverController::getOptionsButton).onTrue(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", true))).onFalse(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", false))); - new Trigger(driverController::getSquareButton).onTrue(new SequentialCommandGroup( - new CollectNote(driveTrain, limelight), - new DriveTimeCommand(-2, 0, 0, 0.5, driveTrain) + + if(Preferences.getBoolean("Detector Limelight", false)) { + new Trigger(driverController::getR1Button).onTrue(new SequentialCommandGroup( + new CollectNote(driveTrain, limelight), + new DriveTimeCommand(-2, 0, 0, 0.5, driveTrain) )); + } + new Trigger(driverController::getOptionsButton).onTrue(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", true))).onFalse(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", false))); new Trigger(driverController::getTouchpadPressed).onTrue(new InstantCommand(driveTrain::stop, driveTrain)); SmartDashboard.putData("force update limelight position", new InstantCommand(()->driveTrain.forceUpdateOdometryWithVision(), driveTrain)); if(angleShooterExists) { @@ -436,7 +445,15 @@ public void teleopPeriodic() { SmartDashboard.putBoolean("shooter in range", RobotState.getInstance().ShooterInRange); SmartDashboard.putBoolean("shooter ready", RobotState.getInstance().ShooterReady); } - + + public void logPower(){ + for(int i = 0; i < 16; i++) { + SmartDashboard.putNumber("PDP Current " + i, PDP.getCurrent(i)); + } + } + public void robotPeriodic() { + logPower(); + } public void disabledPeriodic() { } diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 71eeb947..3527a1e1 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -209,6 +209,8 @@ private DriveConstants() { public static final double k_DRIVE_FF_V = 0; public static final double DRIVE_DEADBAND_MPS = 0.01; public static final double DRIVE_MOTOR_RAMP = 0.1; + public static final double DRIVE_CURRENT_LIMIT = 200; + // Steer Motor /** * The maximum velocity of the steer motor.

@@ -267,6 +269,8 @@ public static final class ShooterConstants{ public static final double ALLOWED_ANGLE_ERROR = 1.5; public static final double ALLOWED_SPEED_ERROR = 4; + public static final double CURRENT_LIMIT = 200; //amps the motor is limited to + public static final double AUTO_AIM_ROBOT_kP = 0.125; public static final double AUTO_AIM_ROBOT_kI = 0.00; public static final double AUTO_AIM_ROBOT_kD = 0.00; @@ -328,6 +332,7 @@ public static final class ClimberConstants{ } public static final class IndexerConstants{ public static final int INDEXER_MOTOR = 11; + public static final int CURRENT_LIMIT = 50; public static final double INDEXER_INTAKE_SPEED = 0.5;//should be 0.5 TODO change to positive public static final double HUMAN_PLAYER_INDEXER_SPEED = -0.5;//should be 0.5 TODO change to positive public static final double INDEXER_SHOOTING_SPEED = 1; @@ -380,6 +385,8 @@ public CTREConfigs() { driveMotorConfig.Voltage.PeakForwardVoltage = 12; driveMotorConfig.Voltage.PeakReverseVoltage = -12; driveMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveMotorConfig.CurrentLimits.SupplyCurrentLimit = DriveConstants.DRIVE_CURRENT_LIMIT; + driveMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; // Steer encoder. steerEncoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; diff --git a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java index 75eb8436..ff65a191 100644 --- a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAnalogSensor; @@ -10,14 +12,23 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.settings.Constants.IndexerConstants; +import frc.robot.settings.Constants.ShooterConstants; public class IndexerSubsystem extends SubsystemBase { TalonFX m_IndexerMotor; SparkAnalogSensor m_DistanceSensor; + CurrentLimitsConfigs currentLimitsConfigs; + TalonFXConfigurator configurator; public IndexerSubsystem() { m_IndexerMotor = new TalonFX(IndexerConstants.INDEXER_MOTOR); m_IndexerMotor.setInverted(false); + + currentLimitsConfigs = new CurrentLimitsConfigs(); + currentLimitsConfigs.SupplyCurrentLimit = IndexerConstants.CURRENT_LIMIT; + currentLimitsConfigs.SupplyCurrentLimitEnable = true; + configurator = m_IndexerMotor.getConfigurator(); + configurator.apply(currentLimitsConfigs); } public void on() { @@ -36,5 +47,6 @@ public void set(double speed) { } @Override public void periodic() { + SmartDashboard.putNumber("indexer current", m_IndexerMotor.getSupplyCurrent().getValueAsDouble()); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 7e4f4d5a..c2a10719 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -4,10 +4,12 @@ package frc.robot.subsystems; import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; @@ -41,6 +43,7 @@ public class ShooterSubsystem extends SubsystemBase { double currentHeading; double m_DesiredShooterAngle; + CurrentLimitsConfigs currentLimitConfigs; Slot0Configs PIDconfigs = new Slot0Configs(); double kP = Constants.ShooterConstants.kP; double kI = Constants.ShooterConstants.kI; @@ -75,6 +78,12 @@ public ShooterSubsystem(double runSpeed) { configuratorR = shooterR.getConfigurator(); configuratorL = shooterL.getConfigurator(); + currentLimitConfigs = new CurrentLimitsConfigs(); + currentLimitConfigs.SupplyCurrentLimit = ShooterConstants.CURRENT_LIMIT; + currentLimitConfigs.SupplyCurrentLimitEnable = true; + configuratorL.apply(currentLimitConfigs); + configuratorR.apply(currentLimitConfigs); + PIDconfigs.kP = kP; PIDconfigs.kI = kI; PIDconfigs.kD = kD; @@ -124,8 +133,8 @@ public boolean validShot() { return runsValid >= Constants.LOOPS_VALID_FOR_SHOT; } public void turnOff(){ - shooterR.set(0); - shooterL.set(0); + shooterR.setControl(new DutyCycleOut(0)); + shooterL.setControl(new DutyCycleOut(0)); } // public double getSpeedRPS() { // return shooterR.getVelocity().asSupplier().get(); @@ -133,6 +142,8 @@ public void turnOff(){ @Override public void periodic() { SmartDashboard.putNumber("TESTING shooter speed error", getError()); + SmartDashboard.putNumber("shooter current right", shooterR.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("shooter current left", shooterL.getSupplyCurrent().getValueAsDouble()); if(getError()