From 881c3d90762411194480682ee8e1e34a28046b4c Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 22 Feb 2020 15:03:10 -0700 Subject: [PATCH] Makework --- src/main/java/frc4388/robot/Constants.java | 6 +-- .../java/frc4388/robot/RobotContainer.java | 13 +++--- .../frc4388/robot/commands/HoodAdjustPID.java | 45 ------------------- .../robot/commands/ShootFireGroup.java | 10 ++--- .../robot/commands/ShootFullGroup.java | 2 +- .../robot/commands/ShootPrepGroup.java | 9 ++-- .../commands/ShooterVelocityControlPID.java | 1 + .../frc4388/robot/commands/TrackTarget.java | 6 +-- .../frc4388/robot/subsystems/Shooter.java | 4 +- 9 files changed, 23 insertions(+), 73 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/HoodAdjustPID.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3a44301..481a7f5 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -82,9 +82,9 @@ public final class Constants { public static final class ShooterConstants { /* Motor IDs */ - public static final int SHOOTER_FALCON_ID = -1; - public static final int SHOOTER_ANGLE_ADJUST_ID = -1; - public static final int SHOOTER_ROTATE_ID = 10; + public static final int SHOOTER_FALCON_ID = 8; + public static final int SHOOTER_ANGLE_ADJUST_ID = 10; + public static final int SHOOTER_ROTATE_ID = 9; /* PID Constants Shooter */ public static final int SHOOTER_SLOT_IDX = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 01c4be8..8e0dad0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -34,6 +34,7 @@ import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID; +import frc4388.robot.commands.HoldTarget; import frc4388.robot.commands.DriveWithJoystickDriveStraight; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; @@ -127,7 +128,7 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); - + /* Operator Buttons */ // shoots until released @@ -137,11 +138,6 @@ public class RobotContainer { // shoots one ball new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false); - - // aims the turret - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new TrackTarget(m_robotShooter, m_robotShooterAim)) - .whenPressed(new StoragePrepAim(m_robotStorage)); // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) @@ -152,9 +148,10 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber)); - /* Storage Neo PID Test */ + // starts tracking target new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new TrackTarget(m_robotShooter, m_robotShooterAim)); + .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)); + //.whenPressed(new StoragePrepAim(m_robotStorage)); //Prepares storage for intaking new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) diff --git a/src/main/java/frc4388/robot/commands/HoodAdjustPID.java b/src/main/java/frc4388/robot/commands/HoodAdjustPID.java deleted file mode 100644 index f08db0d..0000000 --- a/src/main/java/frc4388/robot/commands/HoodAdjustPID.java +++ /dev/null @@ -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; - } -} diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java index 0175198..060334d 100644 --- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java +++ b/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) + ); } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/ShootFullGroup.java b/src/main/java/frc4388/robot/commands/ShootFullGroup.java index 63bc7ec..55557b8 100644 --- a/src/main/java/frc4388/robot/commands/ShootFullGroup.java +++ b/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) ); diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java index ffbc2b5..169640e 100644 --- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java +++ b/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) + ); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index 4da0fe3..1cac750 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/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(); } diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index e892956..db5e3a1 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/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; } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 5f9507c..69d0721 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -127,10 +127,10 @@ public class Shooter extends SubsystemBase { runSpeed = runSpeed/targetVel; //Convert to percent if (actualVel < targetVel - 1000){ //PID Based on ramp up amount - m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed); + m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed/3); } else{ //PID Based on targetVel - m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID + m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel/3); //Init PID } }