diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 90ef485..0a5502a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -123,6 +123,7 @@ public final class Constants { public static final int PID_PRIMARY = 0; /* PID Gains */ + public static final double STORAGE_MIN_OUTPUT = -1.0; public static final Gains STORAGE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); } @@ -141,6 +142,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 26d1a69..56048eb 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -112,42 +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.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000)); - - new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whileHeld(new TrackTarget(m_robotShooter)); - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_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)); @@ -155,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) diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 2853242..8102f13 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,8 @@ public class TrackTarget extends CommandBase { NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); } + + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { @@ -65,6 +70,14 @@ 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); + 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 c71355a..d5e77b9 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -35,6 +35,7 @@ public class Storage extends SubsystemBase { Intake m_intake; + /** * Creates a new Storage. */ @@ -74,19 +75,19 @@ public class Storage extends SubsystemBase { public void runStoragePositionPID(double targetPos) { // Set PID Coefficients - m_storagePIDController.setP(storageGains.m_kP); - m_storagePIDController.setI(storageGains.m_kI); - m_storagePIDController.setD(storageGains.m_kD); - m_storagePIDController.setIZone(storageGains.m_kIzone); - m_storagePIDController.setFF(storageGains.m_kF); - m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kPeakOutput); + 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(); } /**