diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4c4ef0a..7725175 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -4,6 +4,7 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import edu.wpi.first.math.controller.PIDController; @@ -129,6 +130,10 @@ public final class Constants { // CAN IDs public static final int INTAKE_MOTOR = 15; public static final int EXTENDER_MOTOR = 16; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE = new SupplyCurrentLimitConfiguration( + false, 10, 0, 0); //Find + public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_INTAKE = new StatorCurrentLimitConfiguration( + false, 15, 0, 0); } public static final class StorageConstants { public static final int STORAGE_CAN_ID = 18; @@ -160,8 +165,10 @@ public final class Constants { public static final int SHOOTER_TIMEOUT_MS = 32; public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; - public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration( - true, 60, 40, 0.5); + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER = new SupplyCurrentLimitConfiguration( + true, 10, 0, 0); + public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_SHOOTER = new StatorCurrentLimitConfiguration( + true, 27, 0, 0); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; public static final double TURRET_SPEED_MULTIPLIER = 0.4d; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e9d690b..51ed393 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -43,6 +43,7 @@ public class Robot extends TimedRobot { private SendableChooser odoChooser = new SendableChooser(); private HashMap odoChoices = new HashMap<>(); private Pose2d selectedOdo; + private double current; /** * This function is run when the robot is first started up and should be @@ -130,6 +131,16 @@ public class Robot extends TimedRobot { m_robotTime.updateTimes(); SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); + current = + m_robotContainer.m_robotBoomBoom.getCurrent() + + m_robotContainer.m_robotClimber.getCurrent() + + m_robotContainer.m_robotHood.getCurrent() + + m_robotContainer.m_robotIntake.getCurrent() + + m_robotContainer.m_robotSerializer.getCurrent() + + m_robotContainer.m_robotStorage.getCurrent() + + m_robotContainer.m_robotSwerveDrive.getCurrent() + + m_robotContainer.m_robotTurret.getCurrent(); + SmartDashboard.putNumber("Total Robot Current Draw", current); // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled // commands, running already-scheduled commands, removing finished or diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ec1630e..2235b7b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -107,7 +107,7 @@ public class RobotContainer { private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30); private final WPI_TalonFX testSoulderMotor = new WPI_TalonFX(31); - private final Climber m_robotClimber = new Climber(testElbowMotor); + public final Climber m_robotClimber = new Climber(testElbowMotor); /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 0e13aac..58fa705 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -16,6 +16,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.networktables.NetworkTableType; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.IntakeConstants; @@ -186,7 +187,9 @@ public class RobotMap { ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, + shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS); // RIGHT FALCON @@ -201,7 +204,9 @@ public class RobotMap { ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, + shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.follow(shooterFalconLeft); @@ -230,6 +235,15 @@ public class RobotMap { public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); + void configureIntakeMotors(){ + intakeMotor.configFactoryDefault(); + intakeMotor.setNeutralMode(NeutralMode.Coast); + intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE, + ShooterConstants.SHOOTER_TIMEOUT_MS); + intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE, + ShooterConstants.SHOOTER_TIMEOUT_MS); + } + /* Storage Subsystem */ public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); // public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 80a7cfd..b7eaced 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -180,6 +180,8 @@ public class BoomBoom extends SubsystemBase { public void runDrumShooter(double speed) { m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2); SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2); + SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent()); + SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent()); } @@ -208,4 +210,8 @@ public class BoomBoom extends SubsystemBase { { speed2 = speed2 + amount; } + + public double getCurrent(){ + return m_shooterFalconLeft.getSupplyCurrent() + m_shooterFalconRight.getSupplyCurrent(); + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index a55ece0..052d024 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -23,4 +23,8 @@ public class Climber extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run } + + public double getCurrent() { + return m_climberElbow.getSupplyCurrent(); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 8ab8a9f..0a4b39c 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -95,4 +95,8 @@ public double m_fireAngle; public double getAnglePositionDegrees(){ return 0.0;//((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90; } + + public double getCurrent(){ + return m_angleAdjusterMotor.getOutputCurrent(); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index fa11c34..719891e 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -10,6 +10,7 @@ import com.revrobotics.SparkMaxLimitSwitch; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.CANSparkMax; @@ -52,6 +53,8 @@ public class Intake extends SubsystemBase { */ public void runWithTriggers(double leftTrigger, double rightTrigger) { m_intakeMotor.set((rightTrigger - leftTrigger) * 0.3); + SmartDashboard.putNumber("Intake Current Supply", m_intakeMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Intake Current Stator", m_intakeMotor.getStatorCurrent()); } /** * Runs The Extender- @@ -74,4 +77,8 @@ public class Intake extends SubsystemBase { toggle = !toggle; runExtender(toggle); } + + public double getCurrent(){ + return m_intakeMotor.getSupplyCurrent() + m_extenderMotor.getOutputCurrent(); + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index e46be14..7a24a0c 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -57,4 +57,5 @@ public class LED extends SubsystemBase { public LEDPatterns getPattern() { return m_currentPattern; } + } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java index e3f2468..54df449 100644 --- a/src/main/java/frc4388/robot/subsystems/Serializer.java +++ b/src/main/java/frc4388/robot/subsystems/Serializer.java @@ -48,4 +48,7 @@ public class Serializer extends SubsystemBase{ public boolean getSerializerState() { return serializerState; } + public double getCurrent(){ + return m_serializerBelt.getOutputCurrent(); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index dcaddd7..0a0dc22 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -65,4 +65,7 @@ public class Storage extends SubsystemBase { public void periodic() { //manageStorage(); } + public double getCurrent(){ + return m_storageMotor.getOutputCurrent(); + } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 36dc773..f0419d3 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -258,4 +258,8 @@ public class SwerveDrive extends SubsystemBase { speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; } } + + public double getCurrent(){ + return m_leftFront.getCurrent() + m_rightFront.getCurrent() + m_rightBack.getCurrent() + m_leftBack.getCurrent(); + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 1a3ebbc..299ddd4 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -120,7 +120,7 @@ public class SwerveModule extends SubsystemBase { double desiredTicks = currentTicks + deltaTicks; if (!ignoreAngle) { - + angleMotor.set(TalonFXControlMode.Position, desiredTicks); } @@ -192,5 +192,7 @@ public class SwerveModule extends SubsystemBase { canCoder.setPositionToAbsolute(); // canCoder.configSensorInitializationStrategy(initializationStrategy) } - + public double getCurrent(){ + return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 1a97942..b133d6f 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -109,4 +109,8 @@ public class Turret extends SubsystemBase { / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; } + public double getCurrent(){ + return m_boomBoomRotateMotor.getOutputCurrent(); + } + } \ No newline at end of file