diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 0606fd9..75a2675 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -64,9 +64,6 @@ public class Robot extends TimedRobot { LOGGER.log(Level.FINE, "Logging Test 6/8"); LOGGER.log(Level.FINER, "Logging Test 7/8"); LOGGER.log(Level.FINEST, "Logging Test 8/8"); - Errors.log().run(() -> { - throw new Throwable("Exception Test"); - }); // var path = // PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e5fdb9d..d9180ac 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -40,6 +40,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.NotifierCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -47,8 +48,10 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.PathRecorder; +import frc4388.robot.commands.RunCommandForTime; import frc4388.robot.commands.DriveCommands.DriveWithInputForTime; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; +import frc4388.robot.commands.ShooterCommands.Shoot; import frc4388.robot.commands.ShooterCommands.TrackTarget; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Claws; @@ -62,8 +65,10 @@ import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.VisionOdometry; import frc4388.utility.PathPlannerUtil; +import frc4388.utility.Vector2D; import frc4388.utility.controller.ButtonBox; import frc4388.utility.controller.DeadbandedRawXboxController; +import frc4388.utility.controller.DeadbandedXboxController; /** * This class is where the bulk of the robot should be declared. Since @@ -96,8 +101,8 @@ public class RobotContainer { /* Autonomous */ private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive); // Controllers - private final static XboxController m_driverXbox = new DeadbandedRawXboxController(OIConstants.XBOX_DRIVER_ID); - private final static XboxController m_operatorXbox = new DeadbandedRawXboxController(OIConstants.XBOX_OPERATOR_ID); + private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); + private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID); public static boolean softLimits = true; @@ -354,67 +359,74 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // if (loadedPathTrajectory != null) { - // PIDController xController = SwerveDriveConstants.X_CONTROLLER; - // PIDController yController = SwerveDriveConstants.Y_CONTROLLER; - // ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; - // thetaController.enableContinuousInput(-Math.PI, Math.PI); - // PathPlannerState initialState = loadedPathTrajectory.getInitialState(); - // Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation); - // return new SequentialCommandGroup( - // new InstantCommand(m_robotSwerveDrive.m_gyro::reset), - // new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)), - // new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry, - // m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, - // m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive), - // new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path"); - // } else { - // LOGGER.severe("No auto selected."); - // return new RunCommand(() -> { - // }).withName("No Autonomous Path"); - // } + // ! ways to not coast + // // * 1. try zero joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.0, 0.0, false), m_robotSwerveDrive); + // * 2. try opposite joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive); + // * 3a. try permanently setting drive motors to brake, not coast, in RobotMap.java, and ask the driver how it feels. + // * 3b. try to only set the drive motors to brake if in auto mode. - // ! this will run each of the specified PathPlanner paths in sequence. - // * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")); + // ! 1.0 input, 1 second: 134 inches + // ! 0.75 input, 1 second: 48 inches + // ? positive leftY went left, negative leftY went right? + // TODO: if line 372 is true, switch joystick inputs accordingly - // ! this will run each of the specified PathPlanner paths in sequence, while simultaneously running the intake throughout all the paths. - // * return new ParallelCommandGroup(buildAuto(null, - // * null, - // * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")), - // * new RunCommand(() -> m_robotIntake.runAtOutput(0.5)))); - - // return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset(), m_robotSwerveDrive), - // // new InstantCommand(() -> this.resetOdometry(new Pose2d())), - // new InstantCommand(() -> m_robotSwerveDrive.setModuleRotationsToAngle(0.0), m_robotSwerveDrive), - // "Diamond")); + double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate. - // * assume turret is already pointed towards target. + double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0 + double offset = 10.0; // * distance (in inches) from ball that we actually want to stop + + Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub. + Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub. + Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs. + + SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + new ParallelCommandGroup( + new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0) + )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. + + // ! THREE BALL AUTO (HOPEFULLY) + return new SequentialCommandGroup( new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), // * aim with turret to target + weirdAutoShootingGroup, // * shoot + new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage + + new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake + + new ParallelCommandGroup( new RunCommand(() -> m_robotIntake.runAtOutput(1.0), m_robotIntake), // * run intake all throughout path + + new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving + + new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, (40.44 - offset) / distancePerSecond), // * drive to first ball + new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 363) + + new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), // * aim with turret to target + weirdAutoShootingGroup, // * shoot + new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage + + new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y, true), m_robotSwerveDrive), // * rotate so intake points towards second ball + new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-firstToSecond.unit().x, -firstToSecond.unit().y, 0.0, 0.0}, (firstToSecond.magnitude() - offset) / distancePerSecond), // * drive to second ball + new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(firstToSecond.unit().x, firstToSecond.unit().y, 0.0, 0.0, true)), // * brake (see line 363) + + new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, secondBallPosition.toDoubleArray()), // * aim to target + weirdAutoShootingGroup, // * shoot + new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage + + ))); + // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), - // new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0), - // new ParallelRaceGroup( + + // new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, 1.0), + // new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive), + + // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), + + // new ParallelCommandGroup( // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), - // new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage) - // )); + // new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0)) - // return new RunCommandForTime(new RunCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, true), m_robotSwerveDrive), 1.0, true); + // ); - return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), - new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.3, 0.0, 0.0}, 1.0));//, - // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), - // new ParallelCommandGroup( - // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), - // new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0) - //)); - - // * aim with RotateUntilTarget - // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), - // new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0), - // new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5), - // new ParallelRaceGroup( - // new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), - // new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage) - // )); } public static XboxController getDriverController() { diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java index a2ce001..bd78995 100644 --- a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java +++ b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java @@ -27,6 +27,10 @@ public class DriveWithInputForTime extends CommandBase { public DriveWithInputForTime(SwerveDrive swerve, double[] inputs, double duration) { // Use addRequirements() here to declare subsystem dependencies. + if (inputs.length != 4) { + throw new IllegalArgumentException(); + } + this.swerve = swerve; this.inputs = inputs; this.duration = duration * 1000; // ! convert seconds to milliseconds, duh diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java index 310b77e..7d3c7f6 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java @@ -26,4 +26,15 @@ public class Seek extends SequentialCommandGroup { // addCommands(new FooCommand(), new BarCommand()); addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry)); } + + /** Seeks. + * Seeking -> Sought + * @author Aarav Shah + * @blame Aarav Shah (thomas did this) + */ + public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, double[] fakeOdo) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, fakeOdo, false, false), new TrackTarget(turret, drum, hood, visionOdometry)); + } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 7055573..906d05d 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -4,8 +4,11 @@ package frc4388.robot.commands.ShooterCommands; +import java.util.Objects; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants; @@ -60,6 +63,8 @@ public class Shoot extends CommandBase { private boolean endsWithLimelight; + private double[] fakeOdo; + /** * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball * @@ -94,13 +99,23 @@ public class Shoot extends CommandBase { isAimedInTolerance = false; this.inverted = 1; + + this.fakeOdo = null; addRequirements(this.swerve, this.drum, this.turret, this.hood, this.visionOdometry); } - // public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { - // this(swerve, drum, turret, hood, false); - // } + public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, double[] fakeOdo, boolean toShoot, boolean endsWithLime) { + this(swerve, drum, turret, hood, visionOdometry, toShoot, endsWithLime); + if (fakeOdo.length != 2) { + throw new IllegalArgumentException(); + } + this.fakeOdo = fakeOdo; + } + + public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, double[] fakeOdo) { + this(swerve, drum, turret, hood, visionOdometry, false, false); + } /** * Updates error for custom PID. @@ -127,8 +142,8 @@ public class Shoot extends CommandBase { timerStarted = false; startTime = 0; - this.odoX = -this.swerve.getOdometry().getY(); // 3.2766 - this.odoY = -this.swerve.getOdometry().getX(); // -0.9398 + this.odoX = Objects.requireNonNullElse(Units.inchesToMeters(this.fakeOdo[0]), -this.swerve.getOdometry().getY()); + this.odoY = Objects.requireNonNullElse(Units.inchesToMeters(this.fakeOdo[1]), -this.swerve.getOdometry().getX()); this.distance = Math.hypot(odoX, odoY); diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 0428899..622376d 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -79,15 +79,14 @@ public class TrackTarget extends CommandBase { velocity = 0; hoodPosition = 0; + m_visionOdometry.setDriverMode(false); + m_visionOdometry.setLEDs(true); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { try { - m_visionOdometry.setDriverMode(false); - m_visionOdometry.setLEDs(true); - points = m_visionOdometry.getTargetPoints(); // points = getFakePoints(); //// points = filterPoints(points); @@ -114,8 +113,8 @@ public class TrackTarget extends CommandBase { velocity = m_boomBoom.getVelocity(regressedDistance); hoodPosition = m_boomBoom.getHood(regressedDistance); - m_boomBoom.runDrumShooterVelocityPID(velocity); - m_hood.runAngleAdjustPID(hoodPosition); + m_boomBoom.runDrumShooterVelocityPID(velocity + 20); + m_hood.runAngleAdjustPID(hoodPosition + 20); double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity(); double currentHood = this.m_hood.getEncoderPosition(); @@ -195,18 +194,18 @@ public class TrackTarget extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - //// if (this.isAuto) { - //// if (targetLocked& !timerStarted) { - //// timerStarted = true; - //// startTime = System.currentTimeMillis(); - //// } - //// return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); - //// } else { - //// return false; - //// } - // // return isExecuted && Math.abs(output) < .1; - //// } + if (this.isAuto) { + if (targetLocked& !timerStarted) { + timerStarted = true; + startTime = System.currentTimeMillis(); + } + return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + } else { + return false; + } + // return isExecuted && Math.abs(output) < .1; + // } - return false; + // return false; } } diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index f5fcc1b..486f5a4 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -30,12 +30,14 @@ public class Claws extends SubsystemBase { private double m_rightOffset; private boolean m_open; + private boolean clawsOpen; public static enum ClawType {LEFT, RIGHT} public Claws(Servo leftClaw, Servo rightClaw) { m_leftClaw = leftClaw; m_rightClaw = rightClaw; m_open = false; + clawsOpen = false; } /** @@ -46,11 +48,11 @@ public class Claws extends SubsystemBase { if(open) { m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 900); m_rightClaw.setRaw(ClawConstants.TOP_LIMIT + 100); - SmartDashboard.putBoolean("Claws Closed", false); + clawsOpen = false; } else { m_leftClaw.setRaw(ClawConstants.TOP_LIMIT - 400); m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 400); - SmartDashboard.putBoolean("Claws Closed", true); + clawsOpen = true; } } @@ -62,5 +64,6 @@ public class Claws extends SubsystemBase { @Override public void periodic() { + SmartDashboard.putBoolean("Claws Closed", clawsOpen); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 7f81242..239af21 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -64,7 +64,7 @@ public class Hood extends SubsystemBase { // SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition()); // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). - runVelocityRamping(); + // runVelocityRamping(); } public void runVelocityRamping() { diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index 5a9bfcd..56bfe47 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -155,4 +155,8 @@ public class Vector2D extends Vector2d { public String toString() { return "<" + this.x + ", " + this.y + ">"; } + + public double[] toDoubleArray() { + return new double[] {this.x, this.y}; + } }