diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json index d714fd0..75163b2 100644 --- a/src/main/deploy/elastic-layout.json +++ b/src/main/deploy/elastic-layout.json @@ -1826,6 +1826,92 @@ } ] } + }, + { + "name": "Debug Hopper", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Hopper Speed", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/PARTs/Hopper/Hopper Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Debug Intake", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Intake State", + "x": 576.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/PARTs/Intake/Intake State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Pivot Position", + "x": 576.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/PARTs/Intake/Pivot Position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Speed", + "x": 448.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/PARTs/Intake/Pivot Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Intake Speed", + "x": 448.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/PARTs/Intake/Intake Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } } ] } \ 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 b76af54..52b78a3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -42,6 +42,9 @@ import frc.robot.subsystems.Hopper.Hopper; import frc.robot.subsystems.Hopper.HopperPhys; import frc.robot.subsystems.Hopper.HopperSim; +import frc.robot.subsystems.Intake.Intake; +import frc.robot.subsystems.Intake.IntakePhys; +import frc.robot.subsystems.Intake.IntakeSim; import frc.robot.subsystems.Kicker.Kicker; import frc.robot.subsystems.Kicker.KickerPhys; import frc.robot.subsystems.Kicker.KickerSim; @@ -95,10 +98,12 @@ public class RobotContainer { private final Kicker kicker = Robot.isReal() ? new KickerPhys() : new KickerSim(); private final Hopper hopper = Robot.isReal() ? new HopperPhys() : new HopperSim(); + + private final Intake intake = Robot.isReal() ? new IntakePhys() : new IntakeSim(); // private final ShooterSysid shooter = new ShooterSysid(); //for sysid private final ArrayList subsystems = new ArrayList( - Arrays.asList(candle, drivetrain, vision, shooter, turret, kicker, hopper)); + Arrays.asList(candle, drivetrain, vision, shooter, turret, kicker, hopper, intake)); //endregion End Subsystems diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java new file mode 100644 index 0000000..a5f1883 --- /dev/null +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -0,0 +1,6 @@ +package frc.robot.constants; + +public class IntakeConstants { + public static final int INTAKE_MOTOR_ID = 36; + public static final int PIVOT_MOTOR_ID = 38; +} diff --git a/src/main/java/frc/robot/states/IntakeState.java b/src/main/java/frc/robot/states/IntakeState.java new file mode 100644 index 0000000..8166198 --- /dev/null +++ b/src/main/java/frc/robot/states/IntakeState.java @@ -0,0 +1,18 @@ +package frc.robot.states; + +public enum IntakeState { + IDLE(0), + DISABLED(0), + INTAKING(0), + OUTTAKING(0); + private double speed; + + private IntakeState(double speed){ + this.speed = speed; + } + + public double getSpeed(){ + return speed; + } +} + diff --git a/src/main/java/frc/robot/states/PivotState.java b/src/main/java/frc/robot/states/PivotState.java new file mode 100644 index 0000000..e06a781 --- /dev/null +++ b/src/main/java/frc/robot/states/PivotState.java @@ -0,0 +1,16 @@ +package frc.robot.states; + +public enum PivotState { + HOME(0), + INTAKE(0); + + private double position; + + private PivotState(double position){ + this.position = position; + } + + public double getPosition(){ + return position; + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java new file mode 100644 index 0000000..42da017 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -0,0 +1,64 @@ +package frc.robot.subsystems.Intake; + +import org.parts3492.partslib.PARTsUnit; +import org.parts3492.partslib.PARTsUnit.PARTsUnitType; +import org.parts3492.partslib.command.PARTsSubsystem; + +import frc.robot.constants.RobotConstants; +import frc.robot.states.IntakeState; + +public abstract class Intake extends PARTsSubsystem { + + IntakeState state = IntakeState.IDLE; + + public Intake() { + super("Intake"); + + if (RobotConstants.DEBUGGING) { + partsNT.putDouble("Intake Speed", 0); + partsNT.putDouble("Pivot Speed", 0); + } + } + + @Override + public void outputTelemetry() { + partsNT.putDouble("Pivot Position", getPivotPosition().to(PARTsUnitType.Angle)); + partsNT.putDouble("Current Intake Speed", getIntakeSpeed()); + partsNT.putString("Intake State", state.toString()); + } + + @Override + public void stop() { + state = IntakeState.DISABLED; + } + + @Override + public void reset() { + state = IntakeState.IDLE; + } + + @Override + public void periodic() { + if (RobotConstants.DEBUGGING) { + setIntakeSpeed(partsNT.getDouble("Intake Speed")); + setPivotSpeed(partsNT.getDouble("Pivot Speed")); + } + } + + @Override + public void log() { + partsLogger.logDouble("Pivot Position", getPivotPosition().to(PARTsUnitType.Angle)); + partsLogger.logDouble("Intake Speed", getIntakeSpeed()); + partsLogger.logString("Intake State", state.toString()); + } + + public abstract void setIntakeSpeed(double speed); + + public abstract void setPivotSpeed(double speed); + + public abstract void setPivotPosition(PARTsUnit position); + + public abstract double getIntakeSpeed(); + + public abstract PARTsUnit getPivotPosition(); +} diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java b/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java new file mode 100644 index 0000000..9827ede --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java @@ -0,0 +1,86 @@ +package frc.robot.subsystems.Intake; + +import static edu.wpi.first.units.Units.Rotations; + +import org.parts3492.partslib.PARTsUnit; +import org.parts3492.partslib.PARTsUnit.PARTsUnitType; +import org.parts3492.partslib.network.PARTsNT; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; + +import edu.wpi.first.wpilibj.motorcontrol.Talon; + +import frc.robot.constants.IntakeConstants; + +public class IntakePhys extends Intake { + TalonFX intakeMotor; + TalonFX pivotMotor; + + public IntakePhys() { + super(); + TalonFXConfiguration intakeConfig = new TalonFXConfiguration(); + intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID); + intakeConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + intakeMotor.getConfigurator().apply(intakeConfig); + pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID); + } + + @Override + public void setIntakeSpeed(double speed) { + intakeMotor.set(speed); + } + + @Override + public void setPivotPosition(PARTsUnit position) { + pivotMotor.setPosition(position.to(PARTsUnitType.Rotations)); + } + + @Override + public double getIntakeSpeed() { + return intakeMotor.get(); + } + + @Override + public PARTsUnit getPivotPosition() { + return new PARTsUnit(pivotMotor.getPosition().getValueAsDouble(), PARTsUnitType.Rotations); + } + + @Override + public void outputTelemetry() { + super.outputTelemetry(); + partsNT.putDouble("Pivot/Output", pivotMotor.getMotorOutputStatus().getValueAsDouble()); + partsNT.putDouble("Pivot/Current", pivotMotor.getStatorCurrent().getValueAsDouble()); + + partsNT.putDouble("Intake/Output", intakeMotor.getMotorOutputStatus().getValueAsDouble()); + partsNT.putDouble("Intake/Current", intakeMotor.getStatorCurrent().getValueAsDouble()); + } + + @Override + public void stop() { + super.stop(); + intakeMotor.set(0); + pivotMotor.set(0); + } + + @Override + public void reset() { + super.reset(); + } + + @Override + public void log() { + super.log(); + partsLogger.logDouble("Pivot/Output", pivotMotor.getMotorOutputStatus().getValueAsDouble()); + partsLogger.logDouble("Pivot/Current", pivotMotor.getStatorCurrent().getValueAsDouble()); + + partsLogger.logDouble("Intake/Output", intakeMotor.getMotorOutputStatus().getValueAsDouble()); + partsLogger.logDouble("Intake/Current", intakeMotor.getStatorCurrent().getValueAsDouble()); + } + + @Override + public void setPivotSpeed(double speed) { + pivotMotor.set(speed); + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeSim.java b/src/main/java/frc/robot/subsystems/Intake/IntakeSim.java new file mode 100644 index 0000000..2f7ae9f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeSim.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.Intake; + +import org.parts3492.partslib.PARTsUnit; +import org.parts3492.partslib.PARTsUnit.PARTsUnitType; + +public class IntakeSim extends Intake { + + @Override + public void setIntakeSpeed(double speed) { + } + + @Override + public void setPivotSpeed(double speed) { + } + + @Override + public void setPivotPosition(PARTsUnit position) { + } + + @Override + public double getIntakeSpeed() { + return 0; + } + + @Override + public PARTsUnit getPivotPosition() { + return new PARTsUnit(0, PARTsUnitType.Angle); + } + +} diff --git a/src/main/java/frc/robot/subsystems/Kicker/Kicker.java b/src/main/java/frc/robot/subsystems/Kicker/Kicker.java index a0b61c5..c3f9e7b 100644 --- a/src/main/java/frc/robot/subsystems/Kicker/Kicker.java +++ b/src/main/java/frc/robot/subsystems/Kicker/Kicker.java @@ -47,22 +47,21 @@ public void log() { public void periodic() { if (RobotConstants.DEBUGGING) { setSpeed(partsNT.getDouble("Kicker Speed")); - } - else { + } else { switch (kickerState) { - case ROLLING: - setSpeed(kickerState.getSpeed()); - break; - case DISABLED: - setSpeed(kickerState.getSpeed()); - break; - case IDLE: - setSpeed(kickerState.getSpeed()); - break; - default: - setSpeed(0); - break; - } + case ROLLING: + setSpeed(kickerState.getSpeed()); + break; + case DISABLED: + setSpeed(kickerState.getSpeed()); + break; + case IDLE: + setSpeed(kickerState.getSpeed()); + break; + default: + setSpeed(0); + break; + } } }