From 77e536d2b15d5144e796378f60aefaca118895c5 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 22 Feb 2020 13:01:50 -0700 Subject: [PATCH] Shooter PREP --- .../frc4388/robot/commands/HoodAdjustPID.java | 44 +++++++++++++++++++ .../robot/commands/ShootFullGroup.java | 7 ++- .../robot/commands/ShootPrepGroup.java | 16 +++++-- .../commands/ShooterVelocityControlPID.java | 13 +++++- .../frc4388/robot/commands/TrackTarget.java | 9 +++- .../frc4388/robot/subsystems/Shooter.java | 11 ----- 6 files changed, 80 insertions(+), 20 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/HoodAdjustPID.java diff --git a/src/main/java/frc4388/robot/commands/HoodAdjustPID.java b/src/main/java/frc4388/robot/commands/HoodAdjustPID.java new file mode 100644 index 0000000..2815dd5 --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc4388/robot/commands/ShootFullGroup.java b/src/main/java/frc4388/robot/commands/ShootFullGroup.java index 1bb7956..9da770d 100644 --- a/src/main/java/frc4388/robot/commands/ShootFullGroup.java +++ b/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() ); } diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java index 58ab9f1..1b17fda 100644 --- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java +++ b/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) + ); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index 67b5cb1..f4aeae8 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/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; + } } } diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 374f04c..e892956 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/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; } } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index ecd2a88..14d4080 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/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()