Merge pull request #13 from iamawesomecat/main

Created Launcher subsystem

src/main/java/frc/team2412/robot/Hardware.java

@@ -9,8 +9,9 @@ public class Hardware {
 
 	// climb [20 - 29]
 
-	// shooter [30 - 39]
-
+	// launcher [30 - 39]
+	public static final int LAUNCHER_TOP_MOTOR_ID = 30;
+	public static final int LAUNCHER_BOTTOM_MOTOR_ID = 31;
+	public static final int LAUNCHER_ANGLE_MOTOR_ID = 32;
 	// intake [40 - 49]
-
 }

src/main/java/frc/team2412/robot/Subsystems.java

@@ -3,6 +3,7 @@
 import static frc.team2412.robot.Subsystems.SubsystemConstants.*;
 
 import frc.team2412.robot.subsystems.DrivebaseSubsystem;
+import frc.team2412.robot.subsystems.LauncherSubsystem;
 
 public class Subsystems {
 	public static class SubsystemConstants {
@@ -12,17 +13,23 @@ public static class SubsystemConstants {
 		public static final boolean APRILTAGS_ENABLED = false;
 		public static final boolean LIMELIGHT_ENABLED = false;
 		public static final boolean CLIMB_ENABLED = false;
-		public static final boolean SHOOTER_ENABLED = false;
+		public static final boolean LAUNCHER_ENABLED = false;
 		public static final boolean INTAKE_ENABLED = false;
 		public static final boolean DRIVEBASE_ENABLED = true;
 	}
 
 	public final DrivebaseSubsystem drivebaseSubsystem;
+	public final LauncherSubsystem launcherSubsystem;
 
 	public Subsystems() {
 		// initialize subsystems here (wow thats wild)
 		if (DRIVEBASE_ENABLED) {
 			drivebaseSubsystem = new DrivebaseSubsystem();
 		}
+		if (LAUNCHER_ENABLED) {
+			launcherSubsystem = new LauncherSubsystem();
+		} else {
+			launcherSubsystem = null;
+		}
 	}
 }

src/main/java/frc/team2412/robot/commands/launcher/SetAngleCommand.java

@@ -0,0 +1,28 @@
+package frc.team2412.robot.commands.launcher;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.team2412.robot.subsystems.LauncherSubsystem;
+// this command adjusts the angle to a value that is input by the user.
+
+public class SetAngleCommand extends Command {
+	private final LauncherSubsystem launcherSubsystem;
+	private final double launcherAngle;
+
+	public SetAngleCommand(LauncherSubsystem launcherSubsystem, double angle) {
+		this.launcherSubsystem = launcherSubsystem;
+		this.launcherAngle = angle;
+		addRequirements(launcherSubsystem);
+	}
+
+	@Override
+	public void initialize() {
+		launcherSubsystem.setAngle(launcherAngle);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return (MathUtil.isNear(
+				launcherAngle, launcherSubsystem.getAngle(), LauncherSubsystem.ANGLE_TOLERANCE));
+	}
+}

src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java

@@ -0,0 +1,19 @@
+package frc.team2412.robot.commands.launcher;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.team2412.robot.subsystems.LauncherSubsystem;
+// this command can be used as a preset in controls, allowing the user to input a speed and angle
+// value when they keybind it multiple times.
+
+public class SetAngleLaunchCommand extends SequentialCommandGroup {
+	private double launcherSpeed;
+	private double launcherAngle;
+
+	public SetAngleLaunchCommand(LauncherSubsystem launcherSubsystem, double speed, double angle) {
+		launcherSpeed = speed;
+		launcherAngle = angle;
+		addCommands(
+				new SetAngleCommand(launcherSubsystem, launcherAngle),
+				new SetLaunchSpeedCommand(launcherSubsystem, launcherSpeed));
+	}
+}

src/main/java/frc/team2412/robot/commands/launcher/SetLaunchSpeedCommand.java

@@ -0,0 +1,26 @@
+package frc.team2412.robot.commands.launcher;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.team2412.robot.subsystems.LauncherSubsystem;
+// this command activates the launcher
+
+public class SetLaunchSpeedCommand extends Command {
+	private final LauncherSubsystem launcherSubsystem;
+	private final double launcherSpeed;
+
+	public SetLaunchSpeedCommand(LauncherSubsystem launcherSubsystem, double speed) {
+		launcherSpeed = speed;
+		this.launcherSubsystem = launcherSubsystem;
+		addRequirements(launcherSubsystem);
+	}
+
+	@Override
+	public void initialize() {
+		launcherSubsystem.launch(launcherSpeed);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return true;
+	}
+}

src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java

@@ -0,0 +1,127 @@
+package frc.team2412.robot.subsystems;
+
+import com.revrobotics.CANSparkBase.ControlType;
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkFlex;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkAbsoluteEncoder;
+import com.revrobotics.SparkAbsoluteEncoder.Type;
+import com.revrobotics.SparkPIDController;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.team2412.robot.Hardware;
+import java.util.Map;
+
+public class LauncherSubsystem extends SubsystemBase {
+	// CONSTANTS
+	// MOTOR SPEED VALUES
+	public static final double SPEAKER_SHOOT_SPEED = 0.5;
+	public static final double AMP_SHOOT_SPEED = 0.2;
+	public static final double ANGLE_CHANGE_SPEED = 0.15;
+	public static final double ANGLE_TOLERANCE = 0.5;
+
+	// HARDWARE
+	private final CANSparkFlex launcherTopMotor;
+	private final CANSparkFlex launcherBottomMotor;
+	private final CANSparkFlex launcherAngleMotor;
+	private final RelativeEncoder launcherTopEncoder;
+	private final RelativeEncoder launcherBottomEncoder;
+	private final SparkAbsoluteEncoder launcherAngleEncoder;
+	private final SparkPIDController launcherAnglePIDController;
+
+	private final GenericEntry setLauncherSpeedEntry =
+			Shuffleboard.getTab("Launcher")
+					.addPersistent("Launcher Speed setpoint", SPEAKER_SHOOT_SPEED)
+					.withSize(2, 1)
+					.withWidget(BuiltInWidgets.kNumberSlider)
+					.withProperties(Map.of("Min", -1, "Max", 1))
+					.getEntry();
+
+	private final GenericEntry launcherAngleEntry =
+			Shuffleboard.getTab("Launcher")
+					.add("Launcher angle", getAngle())
+					.withSize(1, 1)
+					.withWidget(BuiltInWidgets.kTextView)
+					.getEntry();
+	private final GenericEntry launcherSpeedEntry =
+			Shuffleboard.getTab("Launcher")
+					.add("Launcher Speed", 0)
+					.withSize(1, 1)
+					.withWidget(BuiltInWidgets.kTextView)
+					.getEntry();
+	// Constructor
+	public LauncherSubsystem() {
+
+		// MOTOR INSTANCE VARIBLES
+		// motors
+		launcherTopMotor = new CANSparkFlex(Hardware.LAUNCHER_TOP_MOTOR_ID, MotorType.kBrushless);
+		launcherBottomMotor = new CANSparkFlex(Hardware.LAUNCHER_BOTTOM_MOTOR_ID, MotorType.kBrushless);
+		launcherAngleMotor = new CANSparkFlex(Hardware.LAUNCHER_ANGLE_MOTOR_ID, MotorType.kBrushless);
+		// encoders
+		launcherTopEncoder = launcherTopMotor.getEncoder();
+		launcherBottomEncoder = launcherBottomMotor.getEncoder();
+		launcherAngleEncoder = launcherAngleMotor.getAbsoluteEncoder(Type.kDutyCycle);
+
+		// PID controllers
+		launcherAnglePIDController = launcherAngleMotor.getPIDController();
+		launcherAnglePIDController.setFeedbackDevice(launcherAngleEncoder);
+	}
+
+	public void configMotors() {
+		launcherTopMotor.restoreFactoryDefaults();
+		launcherBottomMotor.restoreFactoryDefaults();
+		launcherAngleMotor.restoreFactoryDefaults();
+		// idle mode (wow)
+		launcherTopMotor.setIdleMode(IdleMode.kCoast);
+		launcherBottomMotor.setIdleMode(IdleMode.kCoast);
+		launcherAngleMotor.setIdleMode(IdleMode.kBrake);
+		// inveritng the bottom motor lmao
+		launcherBottomMotor.setInverted(true);
+
+		// current limit
+		launcherTopMotor.setSmartCurrentLimit(20);
+		launcherBottomMotor.setSmartCurrentLimit(20);
+		launcherAngleMotor.setSmartCurrentLimit(20);
+
+		launcherTopMotor.burnFlash();
+		launcherBottomMotor.burnFlash();
+		launcherAngleMotor.burnFlash();
+	}
+
+	// stop launcher motors method
+	public void stopLauncher() {
+		launcherTopMotor.stopMotor();
+		launcherBottomMotor.stopMotor();
+	}
+
+	public void launch() {
+		double speed = setLauncherSpeedEntry.getDouble(SPEAKER_SHOOT_SPEED);
+		launcherTopMotor.set(speed);
+		launcherBottomMotor.set(speed);
+	}
+
+	public void launch(double speed) {
+		launcherTopMotor.set(speed);
+		launcherBottomMotor.set(speed);
+	}
+	// returns the degrees of the angle of the launcher
+	public double getAngle() {
+		// get position returns a double in the form of rotations
+		return Units.rotationsToDegrees(launcherAngleEncoder.getPosition());
+	}
+
+	public void setAngle(double angle) {
+		launcherAnglePIDController.setReference(Units.degreesToRotations(angle), ControlType.kPosition);
+	}
+
+	@Override
+	public void periodic() {
+		// .get will be replaced with .getVelocity once PID is established for flywheels :C
+		launcherAngleEntry.setDouble(getAngle());
+		launcherSpeedEntry.setDouble(launcherTopMotor.get());
+	}
+}