small random changes

src/main/java/frc/robot/RobotSubsystemState.java

@@ -5,7 +5,7 @@
 import frc.robot.subsystems.Shooter.ShooterState;
 
 public enum RobotSubsystemState {
-  kHandoff(IntakeState.kDown, ArmState.kIntake, ShooterState.kIntake),
+  kIntake(IntakeState.kDown, ArmState.kIntake, ShooterState.kIntake),
   kAmp(IntakeState.kDown, ArmState.kAmp, ShooterState.kAmp),
   kShoot(IntakeState.kDown, ArmState.kShoot, ShooterState.kShoot),
   kTravel(IntakeState.kMid, ArmState.kStow, ShooterState.kShoot);

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

@@ -0,0 +1,46 @@
+// 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 edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotSubsystemState;
+import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.Shooter;
+
+public class SetAllSubsystemStates extends Command {
+  private Shooter shooter;
+  private Intake intake;
+  private Arm arm;
+  private RobotSubsystemState state;
+
+  /** Creates a new SetAllSubsystemStates. */
+  public SetAllSubsystemStates(RobotSubsystemState state) {
+    this.state = state;
+    addRequirements(shooter, intake, arm);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    shooter.setSetpoint(state.shooterState);
+    intake.setSetpoint(state.intakeState);
+    arm.setSetpoint(state.armState);
+  }
+
+  // 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) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/shooter/ShootSpeaker.java

@@ -56,9 +56,11 @@ public void execute() {
     shooter.setVelocity(v0);
     Pose2d robotPose = RobotContainer.swerveDrive.getPose();
 
+    // theta phi time
     double[] optimalParams = shooter.optimizeShooterOrientation(1,
         Math.atan2(targetY - robotPose.getY(), targetX - robotPose.getX()), 0.1, targetX, targetY, targetZ);
 
+    // center of the exit of the shooter (middle of shooter)
     Translation3d shooterPoseRobotRelative = shooter.shooterExitRobotRelative(optimalParams[0]);
     Translation3d shooterPose = shooter.shooterExitFieldRelative(robotPose, shooterPoseRobotRelative);
 
@@ -67,6 +69,7 @@ public void execute() {
         RobotContainer.swerveDrive.vY + (v0 * Math.sin(Math.PI / 2 - optimalParams[0]) * Math.sin(optimalParams[1])),
         v0 * Math.cos(Math.PI / 2 - optimalParams[0]) };
 
+    // we may not want to actually do any of this if it can't run in real time
     double[][] trajectory = shooter.propagateWholeTrajectory3d(in, optimalParams[2], 10);
     double[] finalPoint = trajectory[trajectory.length - 1];
 
@@ -76,9 +79,9 @@ public void execute() {
       goodTrajectory = true;
     }
 
-    if (goodTrajectory) {
-      shooter.goToAngle(optimalParams[0] / (2 * Math.PI));
-    }
+
+    shooter.goToAngle(optimalParams[0] / (2 * Math.PI));
+
 
     Rotation2d angleSetpoint = Rotation2d.fromRadians(optimalParams[1]).rotateBy(Rotation2d.fromDegrees(180));
 
@@ -112,7 +115,7 @@ public void execute() {
   // Make this return true when this Command no longer needs to run execute()
   @Override
   public boolean isFinished() {
-    if (!hasAlliance || timer.hasElapsed(1) || shooter.hasNote()) {
+    if (!hasAlliance || timer.hasElapsed(1) || !shooter.hasNote()) {
       return true;
     }
     return false;

src/main/java/frc/robot/hid/OperatorStick.java

@@ -19,19 +19,19 @@ public boolean intakeUp() {
     return this.getRawButton(5);
   }
 
-  public boolean intakeSetpoint() {
+  public boolean armIntakeSetpoint() {
     return this.getRawButton(9);
   }
 
-  public boolean shootSetpoint() {
+  public boolean armShootSetpoint() {
     return this.getRawButton(7);
   }
 
-  public boolean ampSetpoint() {
+  public boolean armAmpSetpoint() {
     return this.getRawButton(6);
   }
 
-  public boolean stowSetpoint() {
+  public boolean armStowSetpoint() {
     return this.getRawButton(8);
   }
 
@@ -66,4 +66,20 @@ public boolean intake() {
   public boolean outtake() {
     return getPOV() == 0;
   }
+
+  public boolean intakeSetpoint() {
+    return false;
+  }
+
+  public boolean shootSetpoint() {
+    return false;
+  }
+
+  public boolean stowSetpoint() {
+    return false;
+  }
+
+  public boolean ampSetpoint() {
+    return false;
+  }
 }

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

@@ -58,7 +58,7 @@ public void setVoltage(double voltage) {
   }
 
   public void goToSetpoint(ArmState setpoint) {
-    setSetpoint(setpoint.angle);
+    setSetpoint(setpoint);
   }
 
   public double getPivotAngle() {
@@ -82,8 +82,8 @@ public void resetPID() {
     this.armPid.reset();
   }
 
-  public void setSetpoint(double setpoint) {
-    armPosition = setpoint;
+  public void setSetpoint(ArmState setpoint) {
+    armPosition = setpoint.angle;
     resetPID();
   }