Merge pull request #76 from 2491-NoMythic/Voltages

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();
   }
 

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<String> climbSpotChooser;
   private SendableChooser<Command> 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() {
 
   }

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. <p> 
@@ -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;

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());
     }
 }

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,15 +133,17 @@ 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();
   // }
 @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()<ShooterConstants.ALLOWED_SPEED_ERROR) {
       runsValid++;
     } else {