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] 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); } }