Shooter PREP

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

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+  /**
+   * Creates a new HoodAdjustPID.
+   */
+  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/ShootFullGroup.java

@@ -8,6 +8,9 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.subsystems.Storage;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
@@ -16,9 +19,9 @@ public class ShootFullGroup extends SequentialCommandGroup {
   /**
    * Creates a new ShootFullGroup.
    */
-  public ShootFullGroup() {
+  public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
     super(
-      new ShootPrepGroup(), 
+      new ShootPrepGroup(m_shooter, m_shooterAim, m_storage), 
       new ShootFireGroup()
     );
   }

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

@@ -8,16 +8,24 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.subsystems.Storage;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
 public class ShootPrepGroup extends ParallelCommandGroup {
   /**
-   * Creates a new ShootPrepGroup.
+   * Preps the Shooter to be fired
    */
-  public ShootPrepGroup() {
-    // Add your commands in the super() call, e.g.
-    // super(new FooCommand(), new BarCommand());super();
+  public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) {
+    super(
+      new TrackTarget(m_shooter, m_shooterAim),
+      new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()),
+      new HoodAdjustPID(m_shooter),
+      new StoragePrepAim(m_storage)
+      );
   }
 }

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

@@ -13,6 +13,7 @@ import frc4388.robot.subsystems.Shooter;
 public class ShooterVelocityControlPID extends CommandBase {
   Shooter m_shooter;
   double m_targetVel;
+  double m_actualVel;
   /**
    * Creates a new ShooterVelocityControlPID.
    */
@@ -31,8 +32,8 @@ public class ShooterVelocityControlPID extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    System.err.println(m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
     m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
+    m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorVelocity();
   }
 
   // Called once the command ends or is interrupted.
@@ -43,6 +44,14 @@ public class ShooterVelocityControlPID extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    //Tells wether the target velocity has been reached
+    double upperBound = m_targetVel + 300;
+    double lowerBound = m_targetVel - 300;
+    if (m_actualVel < upperBound && m_actualVel > lowerBound){
+      return true;
+    }
+    else{
+      return false;
+    }
   }
 }

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

@@ -68,7 +68,7 @@ public class TrackTarget extends CommandBase {
         //Deadzones
       else if(turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE){turnAmount = VisionConstants.MOTOR_DEAD_ZONE;} 
       else if(turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE){turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;}
-      m_shooterAim.runShooterWithInput(turnAmount/5);
+      m_shooterAim.runShooterWithInput(turnAmount/3);
 
         //Finding Distance
       distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
@@ -81,6 +81,7 @@ public class TrackTarget extends CommandBase {
       fireAngle = Math.atan(yVel/xVel);
       m_shooter.m_fireVel = fireVel;
       m_shooter.m_fireAngle = fireAngle;
+
     }
   }
 
@@ -95,6 +96,12 @@ 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)
+    {
+      return true;
+    }
     return false;
   }
 }

src/main/java/frc4388/robot/subsystems/Shooter.java

@@ -132,17 +132,6 @@ public class Shooter extends SubsystemBase {
     else{ //PID Based on targetVel
       m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
     }
-
-      //Tells wether the target velocity has been reached
-    double upperBound = targetVel + 300;
-    double lowerBound = targetVel - 300;
-    if (actualVel < upperBound && actualVel > lowerBound)
-    {
-      velReached = true;
-    }
-    else{
-      velReached = false;
-    }
   }
 
   public void resetGyroAngleAdj()