From 9288d2b934d11fa3d76a0147447f93f8698b562d Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 3 Mar 2020 01:12:07 -0700 Subject: [PATCH] Rework Command Groups to use Prep Checker --- .../java/frc4388/robot/RobotContainer.java | 1 - .../robot/commands/shooter/HoldTarget.java | 121 ------------------ .../commands/shooter/HoodPositionPID.java | 4 +- .../robot/commands/shooter/PrepChecker.java | 75 +++++++++++ .../commands/shooter/ShootFireGroup.java | 3 +- .../commands/shooter/ShootPrepGroup.java | 12 +- .../shooter/ShooterVelocityControlPID.java | 8 +- .../robot/commands/shooter/TrackTarget.java | 7 +- .../commands/storage/StoragePrepAim.java | 7 +- .../frc4388/robot/subsystems/Shooter.java | 2 +- .../frc4388/robot/subsystems/ShooterAim.java | 2 + .../frc4388/robot/subsystems/ShooterHood.java | 2 + .../frc4388/robot/subsystems/Storage.java | 1 + 13 files changed, 105 insertions(+), 140 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/shooter/HoldTarget.java create mode 100644 src/main/java/frc4388/robot/commands/shooter/PrepChecker.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 98b0a18..68dcd13 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -36,7 +36,6 @@ import frc4388.robot.commands.drive.DriveWithJoystick; import frc4388.robot.commands.drive.TurnDegrees; import frc4388.robot.commands.intake.RunIntakeWithTriggers; import frc4388.robot.commands.shooter.CalibrateShooter; -import frc4388.robot.commands.shooter.HoldTarget; import frc4388.robot.commands.shooter.ShootFullGroup; import frc4388.robot.commands.shooter.ShootPrepGroup; import frc4388.robot.commands.shooter.TrimShooter; diff --git a/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java b/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java deleted file mode 100644 index 7ee1ff4..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/HoldTarget.java +++ /dev/null @@ -1,121 +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.shooter; - -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; -import frc4388.utility.controller.IHandController; - -public class HoldTarget extends CommandBase { - // Setup - NetworkTableEntry xEntry; - ShooterAim m_shooterAim; - Shooter m_shooter; - ShooterHood m_shooterHood; - IHandController m_driverController; - // Aiming - double turnAmount = 0; - double xAngle = 0; - double yAngle = 0; - double target = 0; - public double distance; - public double fireVel; - public double fireAngle; - - public double m_hoodTrim; - public double m_turretTrim; - - /** - * Uses the Limelight to track the target - * @param shooterSubsystem The Shooter subsystem - * @param aimSubsystem The ShooterAim subsystem - */ - public HoldTarget(ShooterAim aimSubsystem) { - m_shooterAim = aimSubsystem; - m_shooter = m_shooterAim.m_shooterSubsystem; - m_shooterHood = m_shooter.m_shooterHoodSubsystem; - addRequirements(m_shooterAim); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - //Vision Processing Mode - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); - } - - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - target = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0); - xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); - yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); - - if (target == 1.0) { // If target in view - // Aiming Left/Right - turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE; - if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { - turnAmount = 0; - } // Angle Error Zone - // 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 - m_shooter.shooterTrims.m_turretTrim); - - // Finding Distance - distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); - SmartDashboard.putNumber("Distance to Target", distance); - - //START Equation Code - /* - double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT); - double xVel = (distance * VisionConstants.GRAV) / (yVel); - - fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2))); - fireAngle = Math.atan(yVel/xVel) * (180/Math.PI); - */ - //END Equation Code - - //START CSV Code - fireVel = m_shooter.m_shooterTable.getVelocity(distance); - fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different - //fireAngle = 33; - //END CSV Code - - - m_shooter.m_fireVel = fireVel; - m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java index d5d807d..ae373d4 100644 --- a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java @@ -48,7 +48,9 @@ public class HoodPositionPID extends CommandBase { public boolean isFinished() { double encoderPos = m_shooterHood.m_angleAdjustMotor.getEncoder().getPosition(); if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){ - return true; + m_shooterHood.m_isHoodReady = true; + } else { + m_shooterHood.m_isHoodReady = false; } return false; } diff --git a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java new file mode 100644 index 0000000..c0ef7a6 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* 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.shooter; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; +import frc4388.robot.subsystems.Storage; + +public class PrepChecker extends CommandBase { + Shooter m_shooter; + ShooterAim m_shooterAim; + ShooterHood m_shooterHood; + Storage m_storage; + + boolean m_isDrumReady = false; + boolean m_isAimReady = false; + boolean m_isHoodReady = false; + boolean m_isStorageReady = false; + + /** + * Creates a new PrepChecker. + * @param shooter used to read all shooter subsystems. Not used as a requirement so don't expect it to interrupt other commands. + * @param storage reads storage in a similar way to shooter. Not used as a requirement. + */ + public PrepChecker(Shooter shooter, Storage storage) { + m_shooter = shooter; + m_shooterAim = m_shooter.m_shooterAimSubsystem; + m_shooterHood = m_shooter.m_shooterHoodSubsystem; + m_storage = storage; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_isDrumReady = false; + m_isAimReady = false; + m_isHoodReady = false; + m_isStorageReady = false; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_isDrumReady = m_shooter.m_isDrumReady; SmartDashboard.putBoolean("ShooterVelocityPID Finished", m_isDrumReady); + m_isAimReady = m_shooterAim.m_isAimReady; SmartDashboard.putBoolean("TrackTarget Finished", m_isAimReady); + m_isHoodReady = m_shooterHood.m_isHoodReady; SmartDashboard.putBoolean("HoodPosition Finished", m_isHoodReady); + m_isStorageReady = m_storage.m_isStorageReadyToFire; SmartDashboard.putBoolean("StoragePrepAim Finished", m_isStorageReady); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_shooter.m_isDrumReady = false; + m_shooterAim.m_isAimReady = false; + m_shooterHood.m_isHoodReady = false; + m_storage.m_isStorageReadyToFire = false; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_isDrumReady && m_isAimReady && m_isHoodReady && m_isStorageReady) { + return true; + } + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java index b0c5af5..88fea47 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java @@ -26,11 +26,10 @@ public class ShootFireGroup extends ParallelRaceGroup { * @param m_storage The Storage subsytem */ public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { - addCommands( new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()), m_shooter), new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()), m_shooterHood), - new HoldTarget(m_shooterAim), + new TrackTarget(m_shooterAim), new StorageRun(m_storage) ); } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java index 3efed56..428d220 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java @@ -7,7 +7,10 @@ package frc4388.robot.commands.shooter; +import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import frc4388.robot.commands.storage.StoragePrepAim; import frc4388.robot.subsystems.Shooter; @@ -18,20 +21,21 @@ 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 { +public class ShootPrepGroup extends ParallelDeadlineGroup { /** * Prepares the Shooter to be fired * @param m_shooter The Shooter subsytem * @param m_shooterAim The ShooterAim subsystem * @param m_storage The Storage subsytem */ - public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { - addCommands( + public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { + super( + new PrepChecker(m_shooter, m_storage), new TrackTarget(m_shooterAim), new ShooterVelocityControlPID(m_shooter), new HoodPositionPID(m_shooterHood), - //new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())), new StoragePrepAim(m_storage) + //new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel())), ); } } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java index 8634fb5..8e5e6e2 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java @@ -54,12 +54,12 @@ public class ShooterVelocityControlPID extends CommandBase { double upperBound = m_targetVel + ShooterConstants.DRUM_VELOCITY_BOUND; double lowerBound = m_targetVel - ShooterConstants.DRUM_VELOCITY_BOUND; if (m_actualVel < upperBound && m_actualVel > lowerBound){ - SmartDashboard.putBoolean("ShooterVelocityPID Finished", true); - return true; + m_shooter.m_isDrumReady = true; } else{ - SmartDashboard.putBoolean("ShooterVelocityPID Finished", false); - return false; + m_shooter.m_isDrumReady = false; } + + return false; } } diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index 58ea976..828232b 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -116,10 +116,11 @@ public class TrackTarget extends CommandBase { public boolean isFinished() { if (xAngle < 1 && xAngle > -1 && target == 1) { - SmartDashboard.putBoolean("TrackTarget Finished", true); - return true; + m_shooterAim.m_isAimReady = true; + } else { + m_shooterAim.m_isAimReady = false; } - SmartDashboard.putBoolean("TrackTarget Finished", false); + return false; } } diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java b/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java index 268f39e..912b87c 100644 --- a/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java +++ b/src/main/java/frc4388/robot/commands/storage/StoragePrepAim.java @@ -50,10 +50,11 @@ public class StoragePrepAim extends CommandBase { @Override public boolean isFinished() { if (!m_storage.getBeam(1) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){ - SmartDashboard.putBoolean("StoragePrepAim Finished", true); + m_storage.m_isStorageReadyToFire = true; return true; + } else { + m_storage.m_isStorageReadyToFire = false; + return false; } - SmartDashboard.putBoolean("StoragePrepAim Finished", false); - return false; } } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 00e54d7..681fe35 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -32,7 +32,7 @@ public class Shooter extends SubsystemBase { public ShooterTables m_shooterTable; - public boolean velReached; + public boolean m_isDrumReady = false; public double m_fireVel; public Trims shooterTrims; diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index 086103c..aed97fa 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -28,6 +28,8 @@ public class ShooterAim extends SubsystemBase { public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS; CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit; + public boolean m_isAimReady = false; + // Configure PID Controllers CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); diff --git a/src/main/java/frc4388/robot/subsystems/ShooterHood.java b/src/main/java/frc4388/robot/subsystems/ShooterHood.java index ae0ee66..d60ae3a 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterHood.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterHood.java @@ -33,6 +33,8 @@ public class ShooterHood extends SubsystemBase { public CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); + public boolean m_isHoodReady = false; + public double m_fireAngle; public CANDigitalInput m_hoodUpLimit, m_hoodDownLimit; diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 67d515d..3fe6ccd 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -31,6 +31,7 @@ public class Storage extends SubsystemBase { Intake m_intake; + public boolean m_isStorageReadyToFire = false; /** * Creates a new Storage.