From 0e556e3d07adc5aa5a9b43a1ed8b4bdbb4637e4d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 24 Mar 2022 00:30:25 -0600 Subject: [PATCH] three ball auto (hopefully) --- .../java/frc4388/robot/RobotContainer.java | 71 +++++++++++++++---- .../DriveCommands/DriveWithInputForTime.java | 6 +- .../robot/commands/ShooterCommands/Seek.java | 11 +++ .../robot/commands/ShooterCommands/Shoot.java | 25 +++++-- src/main/java/frc4388/utility/Vector2D.java | 4 ++ 5 files changed, 98 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0d05a03..d9180ac 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -51,6 +51,7 @@ 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; @@ -64,6 +65,7 @@ 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; @@ -359,28 +361,71 @@ public class RobotContainer { public Command getAutonomousCommand() { // ! 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); + // // * 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. + // ! 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 - // ! 134 inches: 1.0 input, 1 second - // ! 48 inches: 0.75, 1 second - // ? positive leftY went left, negative leftY went right. + double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate. + + 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. - return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), + // ! 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 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 ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake - 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)) + 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.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 RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0)) + + // ); } diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java index 852ec38..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 @@ -45,7 +49,7 @@ public class DriveWithInputForTime extends CommandBase { public void execute() { System.out.println("RUNNING"); elapsed = System.currentTimeMillis() - start; - this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], false); + this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], true); } // Called once the command ends or is interrupted. 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/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}; + } }