diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0e3f76f4..169a6fb4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,7 +10,6 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; import com.ctre.phoenix6.signals.NeutralModeValue; - import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import frc.robot.Constants.RobotMap.CAN; @@ -33,8 +32,8 @@ public class RobotMap { public class CAN { // Front Left private static final int kFrontLeftDriveMotorId = 1; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 31; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 31; // Front Right private static final int kFrontRightDriveMotorId = 3; @@ -50,7 +49,7 @@ public class CAN { private static final int kBackRightDriveMotorId = 5; private static final int kBackRightSteerMotorId = 6; private static final int kBackRightEncoderId = 32; - + public static final int PigeonId = 23; public static final int COLLECTOR_MOTOR = 9; public static final int FLYWHEEL_MOTOR_1 = 0; //TODO Get real @@ -163,6 +162,14 @@ public class VisionConstants { public static final Translation2d FIELD_LIMIT = new Translation2d(Units.feetToMeters(54.0), Units.feetToMeters(26.0)); public static final Translation2d VISION_LIMIT = new Translation2d(Units.feetToMeters(9), Units.feetToMeters(5)); } + + public class CollectorConstants { + public static final boolean COLLECTOR_MOTOR_INVERTED = false; + public static final int COLLECTOR_MOTOR_SUPPLY_CURRENT_LIMIT = 0; // TODO: make sure they are not set to 0 + public static final int COLLECTOR_MOTOR_STATOR_CURRENT_LIMIT = 0; + public static final NeutralModeValue COLLECTOR_MOTOR_NEUTRAL_MODE = NeutralModeValue.Coast; + } + public class FlywheelConstants { public static final boolean FLYWHEEL_MOTOR_1_INVERT = false; @@ -216,4 +223,4 @@ public enum SHOOTER_STATES { put(0d, 0d); }}; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a5ca09ea..5692eeec 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -57,15 +57,6 @@ protected void configureDefaultCommands() { .withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), 0.1) * drivetrainConstants.MaxSpeed) // Drive left with negative X (left) .withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(), 0.1) * drivetrainConstants.MaxAngularRate) // Drive counterclockwise with negative X (left) )); - - // Get collector entry beam break state, then run collector if object is present - collector.setDefaultCommand(new RunCommand(() -> { - if (!collector.getEntryBeamBreakState()) { - collector.setPower(1d); - } else { - collector.stop(); - } - }, collector)); // TODO teach not to do this (was temp for teaching rookies) } @Override diff --git a/src/main/java/frc/robot/command/RunCollectorOnBeamBreak.java b/src/main/java/frc/robot/command/RunCollectorOnBeamBreak.java new file mode 100644 index 00000000..97c98a97 --- /dev/null +++ b/src/main/java/frc/robot/command/RunCollectorOnBeamBreak.java @@ -0,0 +1,46 @@ +// 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.command; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Collector; + +public class RunCollectorOnBeamBreak extends Command { + + /* + * When the beam is interfered with by a note, the collector will run at full power forward. + * When the beam is not interfered with by a note, the collector will stop. + */ + + // Declare subsystems needed by command + private Collector collector; + + public RunCollectorOnBeamBreak(Collector collector) { + this.collector = collector; + addRequirements(collector); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + if (!collector.getEntryBeamBreakState()) { + collector.setPower(1d); + } else { + collector.stop(); + } + } + + @Override + public void end(boolean interrupted) { + collector.stop(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Collector.java b/src/main/java/frc/robot/subsystems/Collector.java index d039c759..e117982d 100644 --- a/src/main/java/frc/robot/subsystems/Collector.java +++ b/src/main/java/frc/robot/subsystems/Collector.java @@ -5,9 +5,13 @@ package frc.robot.subsystems; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.Constants.RobotMap; +import frc.robot.Constants.RobotMap.CAN; +import frc.robot.Constants.CollectorConstants; +import frc.thunder.config.FalconConfig; public class Collector extends SubsystemBase { @@ -17,8 +21,8 @@ public class Collector extends SubsystemBase { public Collector() { // Initialize collector hardware - collectorMotor = new TalonFX(Constants.RobotMap.CAN.COLLECTOR_MOTOR); - collectorEntryBeamBreak = new DigitalInput(Constants.RobotMap.COLLECTOR_ENTRY_BEAMBREAK); + collectorMotor = FalconConfig.createMotor(CAN.COLLECTOR_MOTOR, getName(), CollectorConstants.COLLECTOR_MOTOR_INVERTED, CollectorConstants.COLLECTOR_MOTOR_SUPPLY_CURRENT_LIMIT, CollectorConstants.COLLECTOR_MOTOR_STATOR_CURRENT_LIMIT, CollectorConstants.COLLECTOR_MOTOR_NEUTRAL_MODE); + collectorEntryBeamBreak = new DigitalInput(RobotMap.COLLECTOR_ENTRY_BEAMBREAK); } @Override diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index 9a8336c6..7d00830d 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit 9a8336c695d20a52bbdea110c08ec2139c25718e +Subproject commit 7d00830d0c0cf8027012566fc6abd5d1b311e574