diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6205e25..8507daf 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -123,13 +123,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 { @@ -146,6 +141,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 a254ac3..265f57b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -97,7 +97,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())); @@ -113,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)); @@ -156,19 +120,33 @@ 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) .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)); } /** 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 f14295b..18df225 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. */ @@ -64,21 +66,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(); } }