From 1ffa7bd57b547915fa3e2046dde36b5e7127f3bc Mon Sep 17 00:00:00 2001 From: Aidan <> Date: Sat, 10 Feb 2024 15:18:49 -0800 Subject: [PATCH] make conveyor subsystem --- .../java/frc/robot/config/CompConfig.java | 8 ++ .../java/frc/robot/config/PracticeConfig.java | 8 ++ .../java/frc/robot/config/RobotConfig.java | 5 +- .../java/frc/robot/conveyor/ConveyorMode.java | 13 +++ .../frc/robot/conveyor/ConveyorSubsystem.java | 91 +++++++++++++++++++ .../robot/swerve/CommandSwerveDrivetrain.java | 13 +-- .../util/scheduling/SubsystemPriority.java | 2 + 7 files changed, 130 insertions(+), 10 deletions(-) create mode 100644 src/main/java/frc/robot/conveyor/ConveyorMode.java create mode 100644 src/main/java/frc/robot/conveyor/ConveyorSubsystem.java diff --git a/src/main/java/frc/robot/config/CompConfig.java b/src/main/java/frc/robot/config/CompConfig.java index ec93755b..fc4a151d 100644 --- a/src/main/java/frc/robot/config/CompConfig.java +++ b/src/main/java/frc/robot/config/CompConfig.java @@ -18,6 +18,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.config.RobotConfig.ClimberConfig; +import frc.robot.config.RobotConfig.ConveyorConfig; import frc.robot.config.RobotConfig.IMUConfig; import frc.robot.config.RobotConfig.IntakeConfig; import frc.robot.config.RobotConfig.LightsConfig; @@ -143,6 +144,13 @@ class CompConfig { .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(20)) .withMotorOutput( new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive))), + new ConveyorConfig( + 0, + 0, + new TalonFXConfiguration() + .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1)) + .withSlot0(new Slot0Configs().withKP(0).withKV(0)) + .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(30))), new SwerveConfig( new CurrentLimitsConfigs().withStatorCurrentLimit(60), new CurrentLimitsConfigs().withStatorCurrentLimit(60), diff --git a/src/main/java/frc/robot/config/PracticeConfig.java b/src/main/java/frc/robot/config/PracticeConfig.java index f4f05990..e4a4791e 100644 --- a/src/main/java/frc/robot/config/PracticeConfig.java +++ b/src/main/java/frc/robot/config/PracticeConfig.java @@ -17,6 +17,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.config.RobotConfig.ClimberConfig; +import frc.robot.config.RobotConfig.ConveyorConfig; import frc.robot.config.RobotConfig.IMUConfig; import frc.robot.config.RobotConfig.IntakeConfig; import frc.robot.config.RobotConfig.LightsConfig; @@ -132,6 +133,13 @@ class PracticeConfig { .withMotorOutput( new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive))), + new ConveyorConfig( + 0, + 0, + new TalonFXConfiguration() + .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1)) + .withSlot0(new Slot0Configs().withKP(0).withKV(0)) + .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(30))), new SwerveConfig( new CurrentLimitsConfigs().withStatorCurrentLimit(80), new CurrentLimitsConfigs().withStatorCurrentLimit(80), diff --git a/src/main/java/frc/robot/config/RobotConfig.java b/src/main/java/frc/robot/config/RobotConfig.java index 93b56fa1..7a4523f7 100644 --- a/src/main/java/frc/robot/config/RobotConfig.java +++ b/src/main/java/frc/robot/config/RobotConfig.java @@ -18,6 +18,7 @@ public record RobotConfig( ClimberConfig climber, ShoulderConfig shoulder, IntakeConfig intake, + ConveyorConfig conveyor, SwerveConfig swerve, IMUConfig imu, LightsConfig lights) { @@ -39,6 +40,8 @@ public record IntakeConfig( TalonFXConfiguration topMotorConfig, TalonFXConfiguration bottomMotorConfig) {} + public record ConveyorConfig(int motorID, int sensorID, TalonFXConfiguration motorConfig) {} + public record ShoulderConfig( int rightMotorID, int leftMotorID, @@ -67,7 +70,7 @@ public record SwerveConfig( // TODO: Change this to false during events public static final boolean IS_DEVELOPMENT = true; - private static final String PRACTICE_BOT_SERIAL_NUMBER = "0322443D"; //TODO: get serial number + private static final String PRACTICE_BOT_SERIAL_NUMBER = "0322443D"; // TODO: get serial number public static final String SERIAL_NUMBER = System.getenv("serialnum"); public static final boolean IS_PRACTICE_BOT = SERIAL_NUMBER != null && SERIAL_NUMBER.equals(PRACTICE_BOT_SERIAL_NUMBER); diff --git a/src/main/java/frc/robot/conveyor/ConveyorMode.java b/src/main/java/frc/robot/conveyor/ConveyorMode.java new file mode 100644 index 00000000..ccf41907 --- /dev/null +++ b/src/main/java/frc/robot/conveyor/ConveyorMode.java @@ -0,0 +1,13 @@ +// 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.conveyor; + +public enum ConveyorMode { + IDLE, + PASSING_NOTE, + CONVEYORING, + AMP_OUTTAKE, + PASS_TO_INTAKE; +} diff --git a/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java b/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java new file mode 100644 index 00000000..8d68fc96 --- /dev/null +++ b/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java @@ -0,0 +1,91 @@ +// 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.conveyor; + +import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.util.scheduling.LifecycleSubsystem; +import frc.robot.util.scheduling.SubsystemPriority; + +public class ConveyorSubsystem extends LifecycleSubsystem { + private final TalonFX motor; + private final DigitalInput sensor; + private MotionMagicVelocityDutyCycle velocityRequest = + new MotionMagicVelocityDutyCycle(0.00).withSlot(0); + + private ConveyorMode goalMode = ConveyorMode.IDLE; + private boolean hasNote = false; + private double goalVelocity = 0.00; + + public ConveyorSubsystem(TalonFX motor, DigitalInput sensor) { + super(SubsystemPriority.CONVEYOR); + + this.motor = motor; + this.sensor = sensor; + } + + @Override + public void enabledPeriodic() { + if (goalMode == ConveyorMode.IDLE) { + motor.disable(); + } else { + velocityRequest.withVelocity(goalVelocity); + } + + setHasNote(sensorHasNote()); + } + + @Override + public void robotPeriodic() { + if (goalMode == ConveyorMode.AMP_OUTTAKE) { + goalVelocity = 0.20; + } else if (goalMode == ConveyorMode.CONVEYORING) { + goalVelocity = 0.35; + } else if (goalMode == ConveyorMode.PASSING_NOTE) { + goalVelocity = 0.30; + } else if (goalMode == ConveyorMode.PASS_TO_INTAKE) { + goalVelocity = -0.30; + } else { + goalVelocity = 0.00; + } + } + + public void setMode(ConveyorMode mode) { + goalMode = mode; + } + + public ConveyorMode getMode() { + return goalMode; + } + + public boolean atGoal(ConveyorMode mode) { + if (goalMode != mode) { + return false; + } + if (goalMode == ConveyorMode.IDLE) { + return true; + } + if (hasNote) { + return true; + } + if (!hasNote && Math.abs(goalVelocity) > 0.00) { + return true; + } + return false; + } + + public void setHasNote(boolean bool) { + hasNote = bool; + } + + public boolean gethasNote() { + return hasNote; + } + + private boolean sensorHasNote() { + return sensor.get(); + } +} diff --git a/src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java index 09af86d1..a3e72695 100644 --- a/src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java @@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.config.RobotConfig; import frc.robot.generated.PracticeBotTunerConstants; import java.util.function.Supplier; @@ -22,14 +21,10 @@ * in command-based projects easily. */ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { - public static final SwerveModuleConstants BackRight = - PracticeBotTunerConstants.BackRight; - public static final SwerveModuleConstants BackLeft = - PracticeBotTunerConstants.BackLeft; - public static final SwerveModuleConstants FrontRight = - PracticeBotTunerConstants.FrontRight; - public static final SwerveModuleConstants FrontLeft = - PracticeBotTunerConstants.FrontLeft; + public static final SwerveModuleConstants BackRight = PracticeBotTunerConstants.BackRight; + public static final SwerveModuleConstants BackLeft = PracticeBotTunerConstants.BackLeft; + public static final SwerveModuleConstants FrontRight = PracticeBotTunerConstants.FrontRight; + public static final SwerveModuleConstants FrontLeft = PracticeBotTunerConstants.FrontLeft; public static final SwerveDrivetrainConstants DrivetrainConstants = PracticeBotTunerConstants.DrivetrainConstants; private static final double kSimLoopPeriod = 0.005; // 5 ms diff --git a/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java b/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java index 6293632d..3e776198 100644 --- a/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java +++ b/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java @@ -20,6 +20,8 @@ public enum SubsystemPriority { LIGHTS(10), INTAKE(10), + CONVEYOR(10), + FMS(0), RUMBLE_CONTROLLER(0);