Added commands, lotta commands, im sleepy

TODOs.md

@@ -3,24 +3,24 @@
 These are the jobs to be completed first, to ensure we have minimal functionality for RIT
 
 - Add subsystems to robot
-    - Define unique IDs and channels for all devices in Constants.java IP Dom
+    - Define unique IDs and channels for all devices in Constants.java (IP Dom)
     - Add subsystems to the RobotContainer X
 
 - Updates to subsystems X
     - Update launcher wheel to a Neo/SparkMax X
 
 - Define basic commands for manual fire mode
-    - Y/Top button: Deploy the intake (and run the rollers)
+    - Y/Top button: Deploy the intake (and run the rollers) (TO MAP)
         - Could be a single command
         - Could be 2 commands in sequence
 
-    - B/Right button: Reverse the intake
+    - B/Right button: Reverse the intake (TO MAP)
         - Unclear if this should touch the deploy motor. ATM suggest leaving the deploy motor alone
 
-    - A/Bottom button: Retract the intake (and stop the rollers)
+    - A/Bottom button: Retract the intake (and stop the rollers) (TO MAP)
 
     - D-pad: Set deflector to pre-programmed angle
-        - Suggestion: Implement this once, and make the angle an member variable of the command
+        - Suggestion: Implement this once, and make the angle an member variable of the command (TO MAP)
 
     - Both Triggers: Spin up flywheel (if not already spun up) and then start the kicker.
         - Re: Sensing flywheel speed

src/main/java/frc/robot/commands/ManualHopperCommand.java

@@ -0,0 +1,50 @@
+// 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;
+
+import frc.robot.subsystems.HopperSubsystem;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/** An example command that uses an example subsystem. */
+public class ManualHopperCommand extends Command {
+  private final HopperSubsystem m_subsystem;
+  private final XboxController m_xboxController;
+
+  public ManualHopperCommand(HopperSubsystem subsystem, XboxController xboxController) {
+    m_subsystem = subsystem;
+    m_xboxController = xboxController;
+    addRequirements(subsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_subsystem.spin(.5);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.5)
+    {
+      m_subsystem.feed();
+    }
+    else
+    {
+      m_subsystem.stop();
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @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/ManualShootCommand.java

@@ -0,0 +1,40 @@
+// 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;
+
+import frc.robot.subsystems.TurretSubsystem;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/** An example command that uses an example subsystem. */
+public class ManualShootCommand extends Command {
+  private final TurretSubsystem m_subsystem;
+  private final XboxController m_controller;
+
+  public ManualShootCommand(TurretSubsystem subsystem, XboxController controller) {
+    m_subsystem = subsystem;
+    m_controller = controller;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(subsystem);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if(m_controller.getLeftTriggerAxis()>.5 || m_controller.getRightTriggerAxis()>.5)
+    {
+      m_subsystem.setLauncher(1);
+    }
+    else
+    {
+      m_subsystem.setLauncher(0);
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/SetDeflectorCommand.java

@@ -0,0 +1,32 @@
+// 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;
+
+import frc.robot.subsystems.TurretSubsystem;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/** An example command that uses an example subsystem. **/
+public class SetDeflectorCommand extends Command {
+  private final TurretSubsystem m_subsystem;
+  private Angle deployAngle;
+
+  public SetDeflectorCommand(TurretSubsystem subsystem, Angle deployAngle) {
+    m_subsystem = subsystem;
+    this.deployAngle = deployAngle;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_subsystem.setTargetElevation(deployAngle);
+  }
+  
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/frc/robot/subsystems/HopperSubsystem.java

@@ -6,12 +6,10 @@ package frc.robot.subsystems;
 
 import org.fairportrobotics.frc.posty.TestableSubsystem;
 import org.fairportrobotics.frc.posty.test.PostTest;
-
 import com.ctre.phoenix6.hardware.TalonFX;
-
 import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
 import frc.robot.Constants;
-import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.commands.ManualHopperCommand;
 
 public class HopperSubsystem extends TestableSubsystem {
 
@@ -23,55 +21,13 @@ public class HopperSubsystem extends TestableSubsystem {
     kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
   } 
 
-  public void feed() {
-    kickerMotor.set(0.5);
-  }
+  public void feed() {kickerMotor.set(0.5);}
 
-  public void reverse() {
-    kickerMotor.set(-0.5);
-  } 
+  public void reverse() {kickerMotor.set(-0.5);} 
 
-  public void stop() {
-    kickerMotor.set(0);
-  }
+  public void stop() {kickerMotor.set(0);}
 
-  public void spin(double speed) {
-    spindexerMotor.set(speed);
-  }
-
-  /**
-   * Example command factory method.
-   *
-   * @return a command
-   */
-  public Command exampleMethodCommand() {
-    // Inline construction of command goes here.
-    // Subsystem::RunOnce implicitly requires `this` subsystem.
-    return runOnce(
-        () -> {
-          /* one-time action goes here */
-        });
-  }
-
-  /**
-   * An example method querying a boolean state of the subsystem (for example, a digital sensor).
-   *
-   * @return value of some boolean subsystem state, such as a digital sensor.
-   */
-  public boolean exampleCondition() {
-    // Query some boolean state, such as a digital sensor.
-    return false;
-  }
-
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-  }
-
-  @Override
-  public void simulationPeriodic() {
-    // This method will be called once per scheduler run during simulation
-  }
+  public void spin(double speed) {spindexerMotor.set(speed);}
 
   @PostTest(name = "A friendly name", enabled = true)
   public void myFailingPostTest(){
@@ -82,4 +38,4 @@ public class HopperSubsystem extends TestableSubsystem {
   public void myPassingPostTest(){
     assertThat("Hello World").endWith("World");
   }
-}
+}

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -7,15 +7,21 @@ package frc.robot.subsystems;
 import org.fairportrobotics.frc.posty.TestableSubsystem;
 import org.fairportrobotics.frc.posty.test.PostTest;
 
+import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
+import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.ctre.phoenix6.hardware.TalonFX;
 import frc.robot.Constants;
 import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
 
+import java.time.chrono.ThaiBuddhistChronology;
+
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.wpilibj.DigitalOutput;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 
 public class IntakeSubsystem extends TestableSubsystem {
   /** Creates a new ExampleSubsystem. */
@@ -27,6 +33,8 @@ public class IntakeSubsystem extends TestableSubsystem {
   {
     intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID);
     deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID);
+    deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
+    deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
   }
 
   public void neutral()
@@ -49,38 +57,29 @@ public class IntakeSubsystem extends TestableSubsystem {
     intakeMotor.set(speed);
   }
 
-  /**
-   * Example command factory method.
-   *
-   * @return a command
-   */
-  public Command exampleMethodCommand() {
-    // Inline construction of command goes here.
-    // Subsystem::RunOnce implicitly requires `this` subsystem.
-    return runOnce(
-        () -> {
-          /* one-time action goes here */
-        });
+  public boolean isDeployed()
+  {
+    return deployMotor.isFwdLimitSwitchClosed()==1;
   }
 
-  /**
-   * An example method querying a boolean state of the subsystem (for example, a digital sensor).
-   *
-   * @return value of some boolean subsystem state, such as a digital sensor.
-   */
-  public boolean exampleCondition() {
-    // Query some boolean state, such as a digital sensor.
-    return false;
+  public boolean isRetracted()
+  {
+    return deployMotor.isRevLimitSwitchClosed()==1;
   }
 
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
+  public Command deployCommand()
+  {
+    return new FunctionalCommand(this::extend, this::extend, interrupted -> neutral(), this::isDeployed, this).andThen(Commands.startEnd(() -> setSpeed(50), () -> setSpeed(0), this));
   }
 
-  @Override
-  public void simulationPeriodic() {
-    // This method will be called once per scheduler run during simulation
+  public Command reverseCommand()
+  {
+    return new FunctionalCommand(this::extend, this::extend, interrupted -> neutral(), this::isDeployed, this).andThen(Commands.startEnd(() -> setSpeed(-50), () -> setSpeed(0), this));
+  }
+
+  public Command retractCommand()
+  {
+    return this.runOnce(() -> setSpeed(0)).andThen(new FunctionalCommand(this::retract, this::retract, interrupted -> neutral(), this::isRetracted, this));
   }
 
   @PostTest(name = "A friendly name", enabled = true)

src/main/java/frc/robot/subsystems/TurretSubsystem.java

@@ -54,10 +54,7 @@ public class TurretSubsystem extends TestableSubsystem {
     targElev = edu.wpi.first.units.Units.Degrees.of(45);
   }
 
-  public void setLauncher(double speed)
-  {
-    launcherMotor.set(speed);
-  }
+  public void setLauncher(double speed) {launcherMotor.set(speed);}
 
   public void setTargetElevation(Angle elev)
   {