Merge pull request #8 from FairportRobotics/Shooty

Shooty

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;

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);

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);
+    }
+  }
+}