From b0f420dc692cb06998c485971a219316b67ee9dc Mon Sep 17 00:00:00 2001 From: Kerem <81587@psdschools.org> Date: Tue, 18 Feb 2020 19:56:03 -0700 Subject: [PATCH 1/7] Added buttons --- src/main/java/frc4388/robot/RobotContainer.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 50cedc1..0a735c2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -82,7 +82,6 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs the drum shooter in idle mode - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter)); // drives the leveler with an axis input from the driver controller // m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); @@ -110,10 +109,10 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000)); - new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new TrackTarget(m_robotShooter)); - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .whenPressed(new RunExtenderOutIn(m_robotIntake)); /* PID Test Command */ @@ -135,11 +134,11 @@ public class RobotContainer { // sets solenoids into high gear - new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON) + new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); // sets solenoids into low gear - new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON) + new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); // interrupts any running command From 6742028ba9e742587f01e16be5f715004c6d13c0 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Wed, 19 Feb 2020 20:48:34 -0700 Subject: [PATCH 2/7] Add Shooting capabilities --- src/main/java/frc4388/robot/Constants.java | 1 + .../java/frc4388/robot/RobotContainer.java | 8 +- .../frc4388/robot/commands/ShootShooter.java | 75 +++++++++++++++++++ .../frc4388/robot/commands/TrackTarget.java | 16 ++++ 4 files changed, 98 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/ShootShooter.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c4e5917..a1a5901 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -137,6 +137,7 @@ public final class Constants { public static final double MOTOR_DEAD_ZONE = 0.3; public static final double DISTANCE_ERROR_EQUATION_M = 1.1279; public static final double DISTANCE_ERROR_EQUATION_B = -15.0684; + public static final double GRAV = 385.83; } public static final class OIConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0a735c2..493ab99 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -30,6 +30,7 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; +import frc4388.robot.commands.ShootShooter; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -106,8 +107,11 @@ public class RobotContainer { // .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) // .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000)); + new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5)); + + new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1)); new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new TrackTarget(m_robotShooter)); diff --git a/src/main/java/frc4388/robot/commands/ShootShooter.java b/src/main/java/frc4388/robot/commands/ShootShooter.java new file mode 100644 index 0000000..370c4b5 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShootShooter.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; + +import edu.wpi.first.wpilibj.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +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; + +public class ShootShooter extends CommandBase { + Shooter m_shooter; + Storage m_storage; + private long startTime; + private int ballNum; + /** + * Creates a new ShootAlll. + */ + public ShootShooter(Shooter shootSub, Storage storeSub, int numBall) { + 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()); + } + + // 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()) + { + new ShooterVelocityControlPID(m_shooter, track.addFireVel()); + } + + 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())); + } + } + + // 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() { + if (3000 + 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 2853242..3f806d4 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -7,6 +7,7 @@ package frc4388.robot.commands; +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Shooter; @@ -28,6 +29,8 @@ public class TrackTarget extends CommandBase { double yAngle = 0; double target = 0; public double distance; + public static double fireVel; + public static double fireAngle; /** * Uses the Limelight to track the target @@ -46,6 +49,13 @@ 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 public void execute() { @@ -65,6 +75,12 @@ public class TrackTarget extends CommandBase { //Finding Distance distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180)); SmartDashboard.putNumber("Distance to Target", distance); + + 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); } } From 01a0327cf3fe4f321b5f7eaf98a5ce0cc832287c Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Thu, 20 Feb 2020 20:21:28 -0700 Subject: [PATCH 3/7] Try to make shooter work --- src/main/java/frc4388/robot/Constants.java | 7 +---- .../frc4388/robot/commands/ShootShooter.java | 28 +++++++++---------- .../frc4388/robot/commands/TrackTarget.java | 9 ++---- .../frc4388/robot/subsystems/Shooter.java | 23 ++++++++++++++- .../frc4388/robot/subsystems/Storage.java | 22 ++++++++------- 5 files changed, 52 insertions(+), 37 deletions(-) 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(); } } From 7cdeb09908a6a89854045d321e7f4482feb7e5bc Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 21 Feb 2020 16:59:05 -0700 Subject: [PATCH 4/7] TODO Shooter --- .../java/frc4388/robot/RobotContainer.java | 9 +-- .../frc4388/robot/commands/ShootShooter.java | 75 ------------------- 2 files changed, 4 insertions(+), 80 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/ShootShooter.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 493ab99..77dc2a9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -30,7 +30,6 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; -import frc4388.robot.commands.ShootShooter; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -107,11 +106,11 @@ public class RobotContainer { // .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) // .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5)); + //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5)); - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1)); + //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1)); new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new TrackTarget(m_robotShooter)); diff --git a/src/main/java/frc4388/robot/commands/ShootShooter.java b/src/main/java/frc4388/robot/commands/ShootShooter.java deleted file mode 100644 index d9b2d1f..0000000 --- a/src/main/java/frc4388/robot/commands/ShootShooter.java +++ /dev/null @@ -1,75 +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.wpilibj.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -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.subsystems.Shooter; -import frc4388.robot.subsystems.Storage; - -public class ShootShooter extends CommandBase { - Shooter m_shooter; - Storage m_storage; - private long startTime; - private int ballNum; - public boolean velReach = false; - /** - * Creates a new ShootAlll. - */ - public ShootShooter(Shooter shootSub, Storage storeSub, int numBall) { - ballNum = numBall; - m_shooter = shootSub; - m_storage = storeSub; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - //new InstantCommand(() -> m_storage.storageAim()); - velReach = false; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - //Aiming - if (!m_shooter.velReached && velReach == false) //If the shooter is spooled - { - new ShooterVelocityControlPID(m_shooter, m_shooter.addFireVel()); - startTime = System.currentTimeMillis(); - } - - 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) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (startTime + (1000 * ballNum) < System.currentTimeMillis()){ - return true; - } - return false; - } -} From e7e91d9c4cde502ca53414e8a4b412c74a48c943 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 21 Feb 2020 17:02:21 -0700 Subject: [PATCH 5/7] add thing for later --- src/main/java/frc4388/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 77dc2a9..3015bac 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -113,7 +113,8 @@ public class RobotContainer { // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1)); new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new TrackTarget(m_robotShooter)); + .whileHeld(new TrackTarget(m_robotShooter)) + //.whileHeld(new RunCommand(() -> m_robotStorage.storeAim())); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .whenPressed(new RunExtenderOutIn(m_robotIntake)); From 84c83dd3e56c824540879bcda5357aecbf7c6e6f Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 21 Feb 2020 17:16:16 -0700 Subject: [PATCH 6/7] Fixed gradle fail --- src/main/java/frc4388/robot/RobotContainer.java | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3015bac..293123e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -113,7 +113,7 @@ public class RobotContainer { // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1)); new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new TrackTarget(m_robotShooter)) + .whileHeld(new TrackTarget(m_robotShooter)); //.whileHeld(new RunCommand(() -> m_robotStorage.storeAim())); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) @@ -154,9 +154,6 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber)); - /* Storage Neo PID Test */ - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new TrackTarget(m_robotShooter)); } /** From 9983b9354b5bb105e60a49bac9b776e95fbd6b9d Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 21 Feb 2020 17:25:17 -0700 Subject: [PATCH 7/7] FIX --- .../java/frc4388/robot/RobotContainer.java | 62 ++++++------------- 1 file changed, 20 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4d5a858..265f57b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -112,46 +112,7 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(0), m_robotDrive)); - - /* Operator Buttons */ - // activates "Lit Mode" - //new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - // .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) - // .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - - //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5)); - - //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1)); - - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new TrackTarget(m_robotShooter)); - //.whileHeld(new RunCommand(() -> m_robotStorage.storeAim())); - new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whenPressed(new RunExtenderOutIn(m_robotIntake)); - - /* PID Test Command */ - // runs velocity PID while driving straight - new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 500)) - .whenReleased(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); - - //new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileHeld(new DriveWithJoystickAuxPID(m_robotDrive, getDriverController())); - - // resets the yaw of the pigeon - new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whileHeld(new RunCommand(() -> m_robotDrive.tankDriveVelocity(9, 3), m_robotDrive)); - - // turn 45 degrees - new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new RunCommand(() -> m_robotDrive.driveWithInputAux(0.2, 0), m_robotDrive)); - - // sets solenoids into high gear new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); @@ -159,10 +120,27 @@ public class RobotContainer { // sets solenoids into low gear new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); + + + /* Operator Buttons */ + + //TODO: Shooter Buttons + // shoots until released + //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5)); + + // shoots one ball + //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 1)); + + // aims the turret + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + .whileHeld(new TrackTarget(m_robotShooter)); + //.whenPressed(new RunCommand(() -> m_robotStorage.storeAim())); - // interrupts any running command - new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) - .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); + // extends or retracts the extender + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + .whenPressed(new RunExtenderOutIn(m_robotIntake)); // safety for climber and leveler new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)