diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8d901c6..aefd7c5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,9 @@ package frc.robot; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Unit; +import edu.wpi.first.units.measure.Angle; import java.security.PublicKey; /** @@ -67,6 +70,21 @@ public final class Constants { public static final int RETRACT_CHANNEL = 1; public static final int INTAKE_MOTOR_ID = 2; } + + public static class ShooterConstants { + public static final int LIMIT_CHANNEL = 0; + public static final int TURRET_ID = 1; + public static final int LAUNCHER_ID = 2; + public static final int HOOD_CHANNEL = 3; + public static final int HOOD_ENCODER_ID = 4; + public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(265); + public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5); + public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1); + public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90); + public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0); + public static final double HOMING_SPEED = .35; + public static final double TURRET_GEAR_RATIO = 1; + } public static class HopperConstants { public static final int KICKER_MOTOR_ID = 0; diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java similarity index 96% rename from src/main/java/frc/robot/subsystems/Intake.java rename to src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 0ebfee1..ad0381d 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -14,9 +14,9 @@ import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj2.command.Command; -public class Intake extends TestableSubsystem { +public class IntakeSubsystem extends TestableSubsystem { /** Creates a new ExampleSubsystem. */ - public Intake() + public IntakeSubsystem() { extendRelay = new DigitalOutput(Constants.IntakeConstants.EXTEND_CHANNEL); retractRelay = new DigitalOutput(Constants.IntakeConstants.RETRACT_CHANNEL); diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java new file mode 100644 index 0000000..ccb29d0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -0,0 +1,147 @@ +// 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.subsystems; + +import org.fairportrobotics.frc.posty.TestableSubsystem; +import org.fairportrobotics.frc.posty.test.PostTest; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; + +import static org.fairportrobotics.frc.posty.assertions.Assertions.*; +import frc.robot.Constants; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj2.command.Command; + +public class TurretSubsystem extends TestableSubsystem { + public enum TurretState { + INIT, + HOMING, + MANUAL; + } + + private TurretState currentState; + private DigitalInput turretLimitSwitch; + private TalonFX turretMotor; + private TalonFX launcherMotor; + private Servo hood; + private CANcoder hoodEncoder; + private Angle limitPos; + private Angle limitNeg; + private boolean turretGoingPos; + private Angle targElev; + + public TurretSubsystem() { + turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.LIMIT_CHANNEL); + turretMotor = new TalonFX(Constants.ShooterConstants.TURRET_ID); + + launcherMotor = new TalonFX(Constants.ShooterConstants.LAUNCHER_ID); + + hood = new Servo(Constants.ShooterConstants.HOOD_CHANNEL); + hoodEncoder = new CANcoder(Constants.ShooterConstants.HOOD_ENCODER_ID); + currentState = TurretState.INIT; + limitNeg = Constants.ShooterConstants.LIMIT_AXIMUTH_NEG; + limitPos = Constants.ShooterConstants.LIMIT_AXIMUTH_POS; + turretGoingPos = false; + targElev = edu.wpi.first.units.Units.Degrees.of(45); + } + + public void setLauncher(double speed) + { + launcherMotor.set(speed); + } + + public void setTargetElevation(Angle elev) + { + if(elev.gt(Constants.ShooterConstants.LIMIT_ELEVATION_NEG) && elev.lt(Constants.ShooterConstants.LIMIT_ELEVATION_POS)) + { + targElev=elev; + } + } + + public void startHoming() + { + turretMotor.set(Constants.ShooterConstants.HOMING_SPEED); + currentState = TurretState.HOMING; + } + + public void startRotate(double speed) + { + turretGoingPos=speed>0; + + if(!passedLimit()) + { + turretMotor.set(speed); + } + else + { + turretMotor.set(0); + } + } + + public boolean passedLimit(){ + return (getTurretAngle().gt(limitPos) && turretGoingPos) || (getTurretAngle().lt(limitNeg) && !turretGoingPos); + } + + public Angle getTurretAngle() + { + return turretMotor.getPosition().refresh().getValue().times(Constants.ShooterConstants.TURRET_GEAR_RATIO); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + switch (currentState) { + case HOMING: + periodicHoming(); + break; + case MANUAL: + periodicManual(); + break; + + default: + break; + } + } + + private void periodicHoming() + { + if(turretLimitSwitch.get()) + { + currentState = TurretState.MANUAL; + turretMotor.set(0); + turretMotor.setPosition(0); + } + } + + private void periodicManual() + { + if(passedLimit()) + { + turretMotor.set(0); + } + + Angle curElev = hoodEncoder.getAbsolutePosition().getValue(); + if(targElev.minus(curElev).abs(Units.Degrees)>Constants.ShooterConstants.TARGET_ELEVATION_DIF.in(Units.Degrees)) + { + if(targElev.gt(curElev)) + { + hood.set(50); + } + else + { + hood.set(-50); + } + } + else + { + hood.set(0); + } + } +}