Changes from testing week of 2/19 and 2/26

src/main/java/frc/robot/Robot.java

@@ -34,6 +34,7 @@
  * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
+
 public class Robot extends LoggedRobot {
 	private Command autonomousCommand;
 	private RobotContainer robotContainer;
@@ -155,6 +156,8 @@ public void teleopInit() {
 		if (autonomousCommand != null) {
 			autonomousCommand.cancel();
 		}
+
+		robotContainer.arm.setPosition(5);
 	}
 
 	/** This function is called periodically during operator control. */
@@ -185,4 +188,5 @@ public static boolean isBlue() {
 		// defaults to blue when unknown
 		return !DriverStation.getAlliance().isPresent() || DriverStation.getAlliance().get() != DriverStation.Alliance.Red;
 	}
+
 }

src/main/java/frc/robot/RobotContainer.java

@@ -1,11 +1,12 @@
 package frc.robot;
 
-import static frc.robot.constants.Constants.tuningMode;
 import static frc.robot.constants.Constants.OperatorConstants.*;
 
 import frc.robot.constants.Constants;
 import frc.robot.constants.Constants.OperatorConstants;
 import frc.robot.constants.Constants.ShuffleboardConstants;
+import frc.robot.constants.IntakeConstants;
+import frc.robot.constants.ShooterConstants;
 import frc.robot.io.GyroIO;
 import frc.robot.io.GyroIOPigeon2;
 import frc.robot.io.GyroIOSim;
@@ -14,16 +15,23 @@
 import frc.robot.subsystems.Shooter;
 import frc.robot.subsystems.Swerve;
 import frc.robot.subsystems.SystemAlerter;
+import frc.robot.subsystems.Vision;
 import frc.robot.util.Alert;
 import frc.robot.util.Alert.AlertType;
 
+import frc.robot.commands.arm.ManualArmControl;
+import frc.robot.commands.arm.SetArmPosition;
+import frc.robot.commands.intake.IntakeWithArm;
 import frc.robot.commands.intake.SetIntakeSpeed;
+import frc.robot.commands.shooter.SetShooterVelocity;
 import frc.robot.commands.shooter.StopShooter;
 import frc.robot.commands.swerve.SwerveAngleOffsetCalibration;
 import frc.robot.commands.swerve.TeleopSwerve;
 import frc.robot.commands.swerve.XStance;
+import frc.robot.commands.vision.GamePieceVision;
 
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Relay.Value;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.SendableCameraWrapper;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -73,10 +81,10 @@ public static RobotContainer getInstance() {
 		: new GyroIOSim();
 
 	/* subsystems */
-	// public final Vision vision = new Vision();
+	public final Vision vision = new Vision();
 	public final Arm arm = new Arm();
 	public final Intake intake = new Intake();
-	public final Swerve swerve = new Swerve(gyro/* , vision */);
+	public final Swerve swerve = new Swerve(gyro, vision);
 	public final Shooter shooter = new Shooter();
 
 	/** The container for the robot. Contains subsystems, OI devices, and commands. */
@@ -122,6 +130,8 @@ public RobotContainer() {
 			autoChooser.addOption("sysid swerve drive", sysIdTestSet(swerve.driveSysIdRoutine));
 			autoChooser.addOption("sysid swerve angle", sysIdTestSet(swerve.angleSysIdRoutine));
 			autoChooser.addOption("sysid shooter", sysIdTestSet(shooter.sysIdRoutine));
+
+			vision.setPriorityId(7);
 		}
 
 		// shouldn't require tuningMode as we might run it randomly in comp
@@ -139,32 +149,55 @@ public RobotContainer() {
 	 * joysticks}.
 	 */
 	private void configureBindings() {
+		// *DRIVER CONTROLS*
+		//
 		var teleopSwerve = new TeleopSwerve(
 			() -> -driverController.getLeftY(),
 			() -> -driverController.getLeftX(),
 			() -> -driverController.getRightX(),
-			driverController.getHID()::getLeftBumper, // override speed
-			() -> driverController.getLeftTriggerAxis() > OperatorConstants.boolTriggerThreshold, // preceise mode
+			() -> false, // no overdrive functionality
+			// driverController.getHID()::getLeftBumper, // override speed
+			() -> driverController.getLeftTriggerAxis() > OperatorConstants.boolTriggerThreshold, // precise mode
 			swerve);
 		swerve.setDefaultCommand(teleopSwerve);
 
 		driverController.rightTrigger(OperatorConstants.boolTriggerThreshold)
 			.whileTrue(teleopSwerve.holdToggleFieldRelative());
-		// driverController.rightBumper()
-		// .whileTrue(teleopSwerve.holdRotateAroundPiece());
-
+		// right bumper -> rotate to speaker apriltag
+		// driverController.rightBumper().whileTrue(new AprilTagVision(vision, swerve));
+		// left bumper -> rotate to note
+		driverController.leftBumper().whileTrue(new GamePieceVision(vision, swerve));
 		driverController.a().onTrue(teleopSwerve.toggleFieldRelative());
+		// b -> trap/climb align maybe?
 		driverController.x().whileTrue(new XStance(swerve));
 		driverController.y().onTrue(teleopSwerve.zeroYaw());
 
-		// operatorController.y().onTrue(new SetShooterVelocity(shooter, shooterVelocity));
+		// dpad left -> call for coopertition (lights)
+		// dpad right -> call for amplify (lights)
+
+		// *OPERATOR CONTROLS*
+		//
+		operatorController.a()
+			.onTrue(Commands.parallel(new SetArmPosition(arm, 84), new SetShooterVelocity(shooter, 1750)));
+		operatorController.b().onTrue(Commands.parallel(new SetArmPosition(arm, -1.5),
+			new SetShooterVelocity(shooter, ShooterConstants.subwooferVelocity_rpm)));
 		operatorController.x().onTrue(new StopShooter(shooter));
-		operatorController.rightBumper().whileTrue(new SetIntakeSpeed(intake, false));
+		// y -> set shooter speed and arm angle based on limelight
+		operatorController.leftBumper().onTrue(new SetArmPosition(arm, 4));
+		operatorController.rightBumper().onTrue(new SetArmPosition(arm, 84));
+		operatorController.leftTrigger(boolTriggerThreshold).whileTrue(new IntakeWithArm(intake, arm));
 		operatorController.rightTrigger(boolTriggerThreshold).whileTrue(new SetIntakeSpeed(intake, true));
+		// dpad up -> climber up
+		// dpad down -> climber down
+
+		operatorController.leftStick().whileTrue(new SetIntakeSpeed(intake, -IntakeConstants.intakeSpeed, true));
+		operatorController.rightStick().whileTrue(new ManualArmControl(arm, operatorController::getLeftY));
 
+		// *TESTING CONTROLS*
+		//
 		// When in tuning mode, create multiple testing options on shuffleboard as well as bind commands to a unique
 		// 'testing' controller
-		if (tuningMode) {
+		if (Constants.tuningMode) {
 			var indexSpeedEntry = Shuffleboard.getTab("Test").add("Index Speed", 0)
 				.withWidget(BuiltInWidgets.kNumberSlider)
 				.withProperties(Map.of("min", -1, "max", 1)).getEntry();
@@ -175,19 +208,30 @@ private void configureBindings() {
 				.withWidget(BuiltInWidgets.kNumberSlider)
 				.withProperties(Map.of("min", -1, "max", 1)).getEntry();
 			var shooterVelocityEntry = Shuffleboard.getTab("Test").add("Shooter Velocity", 0).getEntry();
+			var armPositionEntry = Shuffleboard.getTab("Test").add("Arm Position (degrees)", 0).getEntry();
 
 			testController.x().onTrue(new StopShooter(shooter));
+			testController.y()
+				.onTrue(Commands.runOnce(() -> arm.setPosition(armPositionEntry.getDouble(boolTriggerThreshold)), arm));
 			testController.b()
 				.onTrue(new InstantCommand(() -> shooter.setVelocity(shooterVelocityEntry.getDouble(0)), shooter));
 			testController.a()
 				.onTrue(new InstantCommand(() -> shooter.setPercentOutput(shooterSpeedEntry.getDouble(0)), shooter));
+
 			testController.rightBumper()
 				.whileTrue(Commands
-					.runEnd(() -> intake.setMotorVoltage(intakeSpeedEntry.getDouble(0) * 12), () -> intake.stopMotor(), intake)
+					.runEnd(() -> intake.setMotorSpeed(intakeSpeedEntry.getDouble(0)), () -> intake.stopMotor(), intake)
 					.until(() -> intake.inputs.noteSensor).unless(() -> intake.inputs.noteSensor));
 			testController.leftBumper().whileTrue(
-				new StartEndCommand(() -> intake.setMotorVoltage(indexSpeedEntry.getDouble(0) * 12), () -> intake.stopMotor(),
+				new StartEndCommand(() -> intake.setMotorSpeed(indexSpeedEntry.getDouble(0)), () -> intake.stopMotor(),
 					intake));
+
+			testController.leftStick().whileTrue(new SetIntakeSpeed(intake, -IntakeConstants.intakeSpeed, true));
+			testController.rightStick().whileTrue(new ManualArmControl(arm, testController::getLeftY));
+
+			testController.povDown().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kForward), arm));
+			testController.povUp().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kReverse), arm));
+			testController.povLeft().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kOff), arm));
 		}
 	}
 

src/main/java/frc/robot/commands/arm/AutoAim.java

@@ -0,0 +1,56 @@
+// 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.commands.arm;
+
+import frc.robot.constants.VisionConstants;
+import frc.robot.io.FieldVisionIOInputsAutoLogged;
+import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.Vision;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import java.util.Optional;
+
+public class AutoAim extends Command {
+	private Arm arm;
+	private Vision vision; // needed just to set the priority tag to stare at
+	private FieldVisionIOInputsAutoLogged fieldvisionio; // the data we take from
+	Optional<Alliance> alliance = DriverStation.getAlliance();
+
+	/** Creates a new AutoAim. */
+	public AutoAim(Arm arm, Vision vision) {
+		this.arm = arm;
+		this.vision = vision;
+	}
+
+	// Called when the command is initially scheduled.
+	@Override
+	public void initialize() {
+		if (alliance.get() == Alliance.Blue) {
+			vision.setPriorityId(VisionConstants.blueSpeakerTag);
+		}
+		if (alliance.get() == Alliance.Red) {
+			vision.setPriorityId(VisionConstants.redSpeakerTag);
+		}
+	}
+
+	@Override
+	public void execute() {
+		if (vision.findDistance() > 1)
+			;
+		// add if statements for the range now
+	}
+
+	@Override
+	public void end(boolean interrupted) {}
+
+	// Returns true when the command should end.
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
+}

src/main/java/frc/robot/commands/arm/ManualArmControl.java

@@ -0,0 +1,53 @@
+package frc.robot.commands.arm;
+
+import frc.robot.constants.ArmConstants;
+import frc.robot.subsystems.Arm;
+import frc.robot.util.ControllerUtil;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+import java.util.function.DoubleSupplier;
+
+public class ManualArmControl extends Command {
+	public DoubleSupplier speedSupplier;
+	public Arm arm;
+
+	/**
+	 * Constructs a ManualArmControl command, using the provided function to supply speed numbers
+	 * @param arm Arm subsystem to be passed in
+	 * @param speedSupplier function that supplies the speed used to run the arm
+	 */
+	public ManualArmControl(Arm arm, DoubleSupplier speedSupplier) {
+		this.arm = arm;
+		this.speedSupplier = speedSupplier;
+		addRequirements(arm);
+	}
+
+	// Called when the command is initially scheduled.
+	@Override
+	public void initialize() {}
+
+	// Called every time the scheduler runs while the command is scheduled.
+	@Override
+	public void execute() {
+		// apply deadband to input. Run stationary feedforward ("stopped") if banded input is 0
+		double deadbandInput = ControllerUtil.scaleAxis(speedSupplier.getAsDouble());
+		if (deadbandInput == 0) {
+			arm.stop();
+		} else { // else, scale banded input by max manual output and then convert to volts (linearly with max of 12 V)
+			arm.setMotorVoltage(-(ArmConstants.manualMaxOutput * deadbandInput) * 12);
+		}
+	}
+
+	// Called once the command ends or is interrupted.
+	@Override
+	public void end(boolean interrupted) {
+		arm.stop();
+	}
+
+	// Returns true when the command should end.
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
+}

src/main/java/frc/robot/commands/arm/setArmPosition.java → src/main/java/frc/robot/commands/arm/SetArmPosition.java

@@ -4,18 +4,24 @@
 
 package frc.robot.commands.arm;
 
+import frc.robot.constants.ArmConstants;
 import frc.robot.subsystems.Arm;
 
 import edu.wpi.first.wpilibj2.command.Command;
 
-public class setArmPosition extends Command {
+public class SetArmPosition extends Command {
 	private Arm arm;
 	protected double targetPosition_rad; // did I do this right?
 
-	/** Creates a new setArmPosition. */
-	public setArmPosition(Arm arm, double targetPosition_rad) {
+	/**
+	 * Command to set the arm position, cancels the command and moves on 
+	 * after being within the range given by {@link ArmConstants#accuracyTolerance_deg accuracyTolerance}
+	 * @param arm Arm subsystem
+	 * @param targetPosition_deg Position in degrees to set the arm to (0 is horizontal to robot base)
+	 */
+	public SetArmPosition(Arm arm, double targetPosition_deg) {
 		this.arm = arm;
-		this.targetPosition_rad = targetPosition_rad;
+		this.targetPosition_rad = targetPosition_deg;
 		addRequirements(arm);
 	}
 
@@ -29,7 +35,10 @@ public void execute() {}
 
 	@Override
 	public void end(boolean interrupted) {
-		arm.stopArm();
+		// only stop if interrupted. Otherwise, continue adjusting arm position just move on to another command
+		if (interrupted) {
+			arm.stop();
+		}
 	}
 
 	// Returns true when the command should end.

src/main/java/frc/robot/commands/intake/IntakeWithArm.java

@@ -0,0 +1,44 @@
+package frc.robot.commands.intake;
+
+import frc.robot.constants.IntakeConstants;
+import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.Intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class IntakeWithArm extends Command {
+	private Intake intake;
+	private Arm arm;
+
+	public IntakeWithArm(Intake intake, Arm arm) {
+		this.intake = intake;
+		this.arm = arm;
+		addRequirements(intake, arm);
+	}
+
+	// Called when the command is initially scheduled.
+	@Override
+	public void initialize() {
+		if (!intake.inputs.noteSensor) {
+			arm.setPosition(-1.5);
+			intake.setMotorSpeed(IntakeConstants.intakeSpeed);
+		}
+	}
+
+	// Called every time the scheduler runs while the command is scheduled.
+	@Override
+	public void execute() {}
+
+	// Called once the command ends or is interrupted.
+	@Override
+	public void end(boolean interrupted) {
+		arm.setPosition(4);
+		intake.stopMotor();
+	}
+
+	// Returns true when the command should end.
+	@Override
+	public boolean isFinished() {
+		return intake.inputs.noteSensor;
+	}
+}

src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java

@@ -41,8 +41,9 @@ public SetIntakeSpeed(Intake intake, boolean isIndexing) {
 	public void initialize() {
 		// Run motor only if we are either indexing or don't detect a note. Or, if we are intaking (not indexing) and we
 		// detect a note, don't run
-		if (isIndexing || !intake.detectNote())
-			intake.setMotorSpeed(speed);
+		if (isIndexing || !intake.isHoldingNote())
+			;
+		intake.setMotorSpeed(speed);
 	}
 
 	// Called every time the scheduler runs while the command is scheduled.
@@ -60,6 +61,6 @@ public void end(boolean interrupted) {
 	public boolean isFinished() {
 		// If we are intaking and we detect a note, cancel automatically. If we are indexing, the command will only be
 		// canceled on interrupt
-		return (!isIndexing && intake.detectNote());
+		return (!isIndexing && intake.inputs.noteSensor);
 	}
 }