diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a1a5901..3f9f627 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -114,13 +114,8 @@ public final class Constants { public static final int PID_PRIMARY = 0; /* PID Gains */ - public static final double storP = 0.1; - public static final double storI = 1e-4; - public static final double storD = 1.0; - public static final double storIz = 0.0; - public static final double storF = 0.0; - public static final double storkmaxOutput = 1.0; public static final double storkminOutput = -1.0; + public static final Gains STORAGE_GAINS = new Gains(0.1, 0, 1, 0, 0, 1); } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/commands/ShootShooter.java b/src/main/java/frc4388/robot/commands/ShootShooter.java index 370c4b5..d9b2d1f 100644 --- a/src/main/java/frc4388/robot/commands/ShootShooter.java +++ b/src/main/java/frc4388/robot/commands/ShootShooter.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.commands.TrackTarget; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Storage; @@ -23,6 +22,7 @@ public class ShootShooter extends CommandBase { Storage m_storage; private long startTime; private int ballNum; + public boolean velReach = false; /** * Creates a new ShootAlll. */ @@ -30,35 +30,35 @@ public class ShootShooter extends CommandBase { ballNum = numBall; m_shooter = shootSub; m_storage = storeSub; - addRequirements(m_shooter); - addRequirements(m_storage); } // Called when the command is initially scheduled. @Override public void initialize() { - startTime = System.currentTimeMillis(); - //new InstantCommand(() -> m_storage.storageAiming()); + //new InstantCommand(() -> m_storage.storageAim()); + velReach = false; } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - TrackTarget track = new TrackTarget(m_shooter); //Used to pull velocity and angle from TrackTarget.java //Aiming - if (startTime + 3000 > System.currentTimeMillis()) + if (!m_shooter.velReached && velReach == false) //If the shooter is spooled { - new ShooterVelocityControlPID(m_shooter, track.addFireVel()); + new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()); + startTime = System.currentTimeMillis(); } - else if(3000 + startTime + (1000 * ballNum) > System.currentTimeMillis()){ - new RunCommand(() -> m_storage.runStorage(0.5)); - new ShooterVelocityControlPID(m_shooter, track.addFireVel()); - new RunCommand(() -> m_shooter.runAngleAdjustPID(track.addFireAngle())); + else { + velReach = true; + new ParallelCommandGroup( + new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()), + new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())), + new RunCommand(() -> m_storage.runStoragePositionPID(m_storage.getEncoderPos() + (2*ballNum))) + ); } } - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { @@ -67,7 +67,7 @@ public class ShootShooter extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (3000 + startTime + (1000 * ballNum) > System.currentTimeMillis()){ + if (startTime + (1000 * ballNum) < System.currentTimeMillis()){ return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 3f806d4..8102f13 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -49,12 +49,7 @@ public class TrackTarget extends CommandBase { NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); } - public double addFireVel() { - return fireVel; - } - public double addFireAngle() { - return fireAngle; - } + // Called every time the scheduler runs while the command is scheduled. @Override @@ -81,6 +76,8 @@ public class TrackTarget extends CommandBase { fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2))); fireAngle = Math.atan(yVel/xVel); + m_shooter.m_fireVel = fireVel; + m_shooter.m_fireAngle = fireAngle; } } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index a18aca5..c4487ce 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -48,6 +48,9 @@ public class Shooter extends SubsystemBase { double velP; double input; + public boolean velReached; + public double m_fireVel; + public double m_fireAngle; /* * Creates a new Shooter subsystem. @@ -79,6 +82,13 @@ public class Shooter extends SubsystemBase { SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV); } + public double addFireVel() { + return m_fireVel; + } + public double addFireAngle() { + return m_fireAngle; + } + /** * Runs drum shooter motor. * @param speed Speed to set the motor at @@ -105,15 +115,26 @@ public class Shooter extends SubsystemBase { public void runDrumShooterVelocityPID(double targetVel, double actualVel) { velP = actualVel/targetVel; //Percent of target double runSpeed = actualVel + (12000*velP); //Ramp up equation - //if (runSpeed > targetVel) {runSpeed = targetVel;} SmartDashboard.putNumber("shoot", actualVel); runSpeed = runSpeed/targetVel; //Convert to percent + if (actualVel < targetVel - 1000){ //PID Based on ramp up amount m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed); } 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; + } } diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 790e59e..50628c6 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -18,7 +18,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj2.command.SubsystemBase; - +import frc4388.robot.Gains; import frc4388.robot.Constants.StorageConstants; public class Storage extends SubsystemBase { @@ -29,6 +29,8 @@ public class Storage extends SubsystemBase { CANEncoder m_encoder = m_storageMotor.getEncoder(); + public static Gains m_storageGains = StorageConstants.STORAGE_GAINS; + /** * Creates a new Storage. */ @@ -65,21 +67,21 @@ public class Storage extends SubsystemBase { } /* Storage PID Control */ - public void runStoragePositionPID(double targetPos, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput) + public void runStoragePositionPID(double targetPos) { // Set PID Coefficients - m_storagePIDController.setP(kP); - m_storagePIDController.setI(kI); - m_storagePIDController.setD(kD); - m_storagePIDController.setIZone(kIz); - m_storagePIDController.setFF(kF); - m_storagePIDController.setOutputRange(kminOutput, kmaxOutput); + m_storagePIDController.setP(m_storageGains.m_kP); + m_storagePIDController.setI(m_storageGains.m_kI); + m_storagePIDController.setD(m_storageGains.m_kD); + m_storagePIDController.setIZone(m_storageGains.m_kIzone); + m_storagePIDController.setFF(m_storageGains.m_kF); + m_storagePIDController.setOutputRange(StorageConstants.storkminOutput, m_storageGains.m_kmaxOutput); m_storagePIDController.setReference(targetPos, ControlType.kPosition); } - public void getEncoderPos() + public double getEncoderPos() { - m_encoder.getPosition(); + return m_encoder.getPosition(); } }