Makework

src/main/java/frc4388/robot/commands/HoodAdjustPID.java

@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.Shooter;
-
-public class HoodAdjustPID extends CommandBase {
-  Shooter m_shooter;
-  /**
-   * Adjusts the hood based on the limelight target angle
-   * @param shooterSub The Shooter subsystem
-   */
-  public HoodAdjustPID(Shooter shooterSub) {
-    m_shooter = shooterSub;
-    addRequirements(m_shooter);
-  }
-
-  // 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() {
-    m_shooter.runAngleAdjustPID(m_shooter.addFireAngle());
-  }
-
-  // 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/frc4388/robot/commands/ShootFireGroup.java

@@ -24,11 +24,11 @@ public class ShootFireGroup extends ParallelRaceGroup {
    * @param m_storage The Storage subsytem
    */
   public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
-    super(
+    addCommands(
       new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())),
       new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())),
-      new HoldTarget(m_shooter, m_shooterAim),
-      new StorageRun(m_storage)
-      );
+      new HoldTarget(m_shooter, m_shooterAim)
+      //new StorageRun(m_storage)
+    );
   }
-}
+}

src/main/java/frc4388/robot/commands/ShootFullGroup.java

@@ -23,7 +23,7 @@ public class ShootFullGroup extends SequentialCommandGroup {
    * @param m_storage The Storage subsytem
    */
   public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
-    super(
+    addCommands(
       new ShootPrepGroup(m_shooter, m_shooterAim, m_storage), 
       new ShootFireGroup(m_shooter, m_shooterAim, m_storage)
     );

src/main/java/frc4388/robot/commands/ShootPrepGroup.java

@@ -24,11 +24,10 @@ public class ShootPrepGroup extends ParallelCommandGroup {
    * @param m_storage The Storage subsytem
    */
   public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
-    super(
+    addCommands(
       new TrackTarget(m_shooter, m_shooterAim),
-      new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()),
-      new HoodAdjustPID(m_shooter),
-      new StoragePrepAim(m_storage)
-      );
+      new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel())
+      //new StoragePrepAim(m_storage)
+    );
   }
 }

src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java

@@ -35,6 +35,7 @@ public class ShooterVelocityControlPID extends CommandBase {
   @Override
   public void execute() {
     m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
+    m_shooter.runAngleAdjustPID(m_shooter.addFireAngle());
     m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorVelocity();
   }
 

src/main/java/frc4388/robot/commands/TrackTarget.java

@@ -40,7 +40,7 @@ public class TrackTarget extends CommandBase {
   public TrackTarget(Shooter shooterSubsystem, ShooterAim aimSubsystem) {
     m_shooterAim = aimSubsystem;
     m_shooter = shooterSubsystem;
-    addRequirements(m_shooter);
+    addRequirements(m_shooterAim);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
   }
 
@@ -96,9 +96,7 @@ public class TrackTarget extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    double upperLimit = xAngle + 0.05;
-    double lowerLimit = xAngle - 0.05;
-    if (xAngle < upperLimit && xAngle > lowerLimit)
+    if (xAngle < 1 && xAngle > -1)
     {
       return true;
     }