Shooter

Robot2024/lib/src/main/java/com/team2813/lib2813/control/encoders/CancoderWrapper.java

@@ -0,0 +1,37 @@
+package com.team2813.lib2813.control.encoders;
+import com.team2813.lib2813.control.DeviceInformation;
+import com.team2813.lib2813.control.Encoder;
+import com.team2813.lib2813.util.ConfigUtils;
+import com.ctre.phoenix6.hardware.CANcoder;
+
+public class CancoderWrapper implements Encoder {
+	private CANcoder cancoder;
+	private DeviceInformation info;
+	public CancoderWrapper(int id, String canbus) {
+		cancoder = new CANcoder(id, canbus);
+		info = new DeviceInformation(id, canbus);
+	}
+	public CancoderWrapper(int id) {
+		cancoder = new CANcoder(id);
+		info = new DeviceInformation(id);
+	}
+	@Override
+	public double position() {
+		return cancoder.getPosition().getValueAsDouble();
+	}
+
+	@Override
+	public void setPosition(double position) {
+		ConfigUtils.phoenix6Config(() -> cancoder.setPosition(position));
+	}
+	public CANcoder encoder() {
+		return cancoder;
+	}
+	@Override
+	public boolean equals(Object obj) {
+		if (!(obj instanceof CancoderWrapper))
+			return false;
+		CancoderWrapper other = (CancoderWrapper) obj;
+		return info.equals(other.info);
+	}
+}

Robot2024/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java

@@ -127,8 +127,6 @@ public void set(ControlMode controlMode, double demand, double feedForward) {
 			case MOTION_MAGIC:
 				demand = Units2813.motorRevsToTicks(demand, 2048);
 				break;
-			case DUTY_CYCLE:
-				break;
 			default:
 				break;
 		}

Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java

@@ -13,9 +13,9 @@
 /**
  * Defines PID controll over a motor, with values specified by an encoder
  * 
- * @param <T> the {@link MotorSubsystem.Position} type to use positions from. must be an enum
+ * @param <T> the {@link MotorSubsystem.Position} type to use positions from.
  */
-public abstract class MotorSubsystem<T extends Enum<T> & MotorSubsystem.Position> extends PIDSubsystem {
+public abstract class MotorSubsystem<T extends MotorSubsystem.Position> extends PIDSubsystem {
 
 	protected final Motor motor;
 	protected final Encoder encoder;
@@ -40,8 +40,8 @@ public static class MotorSubsystemConfiguration {
 		 * The default starting position if one is not defined
 		 */
 		public static final double DEFAULT_STARTING_POSITION = 0.0;
-		private Motor motor;
-		private Encoder encoder;
+		protected Motor motor;
+		protected Encoder encoder;
 		private PIDController controller;
 		private double acceptableError;
 		private double startingPosition;
@@ -115,6 +115,11 @@ public MotorSubsystemConfiguration startingPosition(Position startingPosition) {
 			this.startingPosition = startingPosition.getPos();
 			return this;
 		}
+
+		public MotorSubsystemConfiguration acceptableError(double acceptableError) {
+			this.acceptableError = acceptableError;
+			return this;
+		}
 	}
 
 	/**
@@ -165,10 +170,12 @@ public void set(ControlMode mode, double demand) {
 		motor.set(mode, demand);
 	}
 
+	@Override
 	protected void useOutput(double output, double setpoint) {
 		motor.set(ControlMode.DUTY_CYCLE, output);
 	}
 
+	@Override
 	protected double getMeasurement() {
 		return encoder.position();
 	}

Robot2024/lib/src/main/java/com/team2813/lib2813/util/ConfigUtils.java

@@ -3,6 +3,7 @@
 import java.util.function.Supplier;
 
 import com.ctre.phoenix.ErrorCode;
+import com.ctre.phoenix6.StatusCode;
 import com.revrobotics.REVLibError;
 
 import edu.wpi.first.wpilibj.DriverStation;
@@ -26,6 +27,17 @@ public static void revConfig(Supplier<REVLibError> configMethod) {
 		}
 	}
 
+	public static void phoenix6Config(Supplier<StatusCode> configMethod) {
+		StatusCode errorCode = configMethod.get();
+		for (int i = 1; i <= ATTEMPTS && errorCode != StatusCode.OK; i++) {
+			DriverStation.reportError(String.format("%s: Config Attempt %d Failed", errorCode.toString(), i), false);
+			errorCode = configMethod.get();
+		}
+		if (errorCode != StatusCode.OK) {
+			DriverStation.reportError(String.format("%s: Config Failed", errorCode.toString()), false);
+		}
+	}
+
 	public static void ctreConfig(Supplier<ErrorCode> configMethod) {
 		ErrorCode errorCode = configMethod.get();
 		if (errorCode != ErrorCode.OK) {

Robot2024/src/main/java/com/team2813/Constants.java

@@ -51,11 +51,12 @@ public static class DriverConstants {
     //Mechanism CAN IDs
     public static final int INTAKE = 14;
     public static final int KICKER = 15;
-    public static final int SHOOTER = 16;
-    public static final int SHOOTER_PIVOT= 17;
-    public static final int SHOOTER_ENCODER = 18;
-    public static final int CLIMBER = 19;
-    public static final int MAGAZINE = 20;
-	public static final int INTAKE_ENCODER = 21;
-	public static final int INTAKE_PIVOT = 22;
+    public static final int SHOOTER_1 = 16;
+	public static final int SHOOTER_2 = 17;
+    public static final int SHOOTER_PIVOT= 18;
+    public static final int SHOOTER_ENCODER = 19;
+    public static final int CLIMBER = 20;
+    public static final int MAGAZINE = 21;
+	public static final int INTAKE_ENCODER = 22;
+	public static final int INTAKE_PIVOT = 23;
 }

Robot2024/src/main/java/com/team2813/RobotContainer.java

@@ -14,21 +14,28 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
 import com.team2813.commands.DefaultDriveCommand;
+import com.team2813.commands.DefaultShooterCommand;
 import com.team2813.subsystems.Drive;
+import com.team2813.subsystems.Shooter;
+
 import static com.team2813.Constants.*;
 import static com.team2813.Constants.DriverConstants.*;
+import static com.team2813.Constants.OperatorConstants.*;
 
 public class RobotContainer {
 	private final SendableChooser<Command> autoChooser;
 	private final Drive drive = new Drive();
+	private final Shooter shooter = new Shooter();
 	private final XboxController driverController = new XboxController(driverControllerPort);
+	private final XboxController operatorController = new XboxController(operatorControllerPort);
 	public RobotContainer() {
 		drive.setDefaultCommand(new DefaultDriveCommand(
                 () -> -modifyAxis(driverController.getLeftY()) * Drive.MAX_VELOCITY,
                 () -> -modifyAxis(driverController.getLeftX()) * Drive.MAX_VELOCITY,
                 () -> -modifyAxis(driverController.getRightX()) * Drive.MAX_ANGULAR_VELOCITY,
                 drive
         ));
+		shooter.setDefaultCommand(new DefaultShooterCommand(shooter, operatorController::getRightY));
 		configureBindings();
 		autoChooser = AutoBuilder.buildAutoChooser();
 		SmartDashboard.putData("Auto", autoChooser);

Robot2024/src/main/java/com/team2813/commands/DefaultShooterCommand.java

@@ -0,0 +1,34 @@
+package com.team2813.commands;
+
+import java.util.function.DoubleSupplier;
+
+import com.team2813.lib2813.control.ControlMode;
+import com.team2813.subsystems.Shooter;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class DefaultShooterCommand extends Command {
+	private Shooter shooter;
+	private DoubleSupplier controll;
+	public DefaultShooterCommand(Shooter shooter, DoubleSupplier controll) {
+		this.shooter = shooter;
+		this.controll = controll;
+	}
+	/**
+	 * The number to multiply results from {@link #controll}. Multiplies by {@value #MULTIPLIER}
+	 */
+	private static final double MULTIPLIER = 1;
+	private static final double DEADZONE = 0.01;
+	/**
+	 * Sets the motor to the value from the {@link DoubleSupplier} {@link #controll}.
+	 * Multiplies the result by {@value #MULTIPLIER}. The return from {@link #controll} MUST be less than or equal to 1.
+	 */
+	public void excecute() {
+		double val = controll.getAsDouble();
+		if (Math.abs(val) > DEADZONE) {
+			shooter.set(ControlMode.DUTY_CYCLE, val * MULTIPLIER);
+		} else {
+			shooter.set(ControlMode.DUTY_CYCLE, 0);
+		}
+	}
+}

Robot2024/src/main/java/com/team2813/subsystems/Shooter.java

@@ -0,0 +1,44 @@
+package com.team2813.subsystems;
+
+import com.team2813.lib2813.control.ControlMode;
+import com.team2813.lib2813.control.Motor;
+import com.team2813.lib2813.control.encoders.CancoderWrapper;
+import com.team2813.lib2813.control.motors.TalonFXWrapper;
+import com.team2813.lib2813.subsystems.MotorSubsystem;
+import static com.team2813.Constants.*;
+
+import com.ctre.phoenix.motorcontrol.TalonFXInvertType;
+
+public class Shooter extends MotorSubsystem<Shooter.Angle> {
+	Motor shooterMotor;
+	public Shooter() {
+		// TODO: fix invert type
+		super(new MotorSubsystemConfiguration(
+			new TalonFXWrapper(SHOOTER_PIVOT, TalonFXInvertType.Clockwise),
+			new CancoderWrapper(SHOOTER_ENCODER)
+			));
+		TalonFXWrapper m = new TalonFXWrapper(SHOOTER_1, TalonFXInvertType.Clockwise);
+		m.addFollower(SHOOTER_2, TalonFXInvertType.OpposeMaster);
+		shooterMotor = m;
+		setSetpoint(Angle.TEST);
+	}
+	public void stop() {
+		shooterMotor.set(ControlMode.DUTY_CYCLE, 0);
+	}
+
+	public void run(double demand) {
+		shooterMotor.set(ControlMode.DUTY_CYCLE, demand);
+	}
+
+	public static enum Angle implements MotorSubsystem.Position {
+		TEST(0.0);
+		private final double pos;
+		Angle(double pos) {
+			this.pos = pos;
+		}
+		@Override
+		public double getPos() {
+			return pos;
+		}
+	}
+}