From fcf46c9cd7cf115d1cdb44b6e9d3c4b79c23458d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 24 Mar 2022 17:51:18 -0600 Subject: [PATCH] oaifh --- src/main/java/frc4388/robot/Robot.java | 1 + .../java/frc4388/robot/RobotContainer.java | 27 +++++++++--------- .../commands/ShooterCommands/TrackTarget.java | 28 +++++++++---------- 3 files changed, 28 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 036bfa9..16c1b31 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -140,6 +140,7 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); + System.out.println((180.0 / Math.PI) * Math.atan2(-(219.25 / 2.00) - 40.44 + 10.00, -(82.83 / 2.00) + 15.56)); // current = // m_robotContainer.m_robotBoomBoom.getCurrent() + // m_robotContainer.m_robotClimber.getCurrent(); //+ diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4c1ddec..62fb678 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -129,8 +129,6 @@ public class RobotContainer { */ public RobotContainer() { - - // 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 @@ -475,22 +473,23 @@ public class RobotContainer { 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), 2.0) - // )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. + 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), 2.0) + )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. // return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target // weirdAutoShootingGroup, // * shoot // new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // * stop running storage - // ! SHOOT FIRST BALL, THEN DRIVE OFF LINE (HOPEFULLY) - return new SequentialCommandGroup( //new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target - // weirdAutoShootingGroup, // * shoot - // new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage - - new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving - new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.6, 0.0, 0.0}, 0.5))); // * drive off line + // ! DRIVE OFF LINE, THEN SHOOT BALL (HOPEFULLY) + return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving + new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.5, 0.0, 0.0}, 1.0), // * drive out of tarmac + new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake + new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(-(219.25 / 2.00) - turretDistanceFromFront, -(82.83 / 2.00) + 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target + weirdAutoShootingGroup, // * shoot + new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage + ); // ! TWO BALL AUTO (HOPEFULLY) // return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 20aec65..97e1913 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -35,7 +35,7 @@ public class TrackTarget extends CommandBase { BoomBoom m_boomBoom; Hood m_hood; - // boolean isAuto; + boolean isAuto; static double velocity; static double hoodPosition; @@ -55,22 +55,22 @@ public class TrackTarget extends CommandBase { long startTime; private double timeTolerance; - public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry/*, boolean isAuto*/) { + public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, boolean isAuto) { m_turret = turret; m_boomBoom = boomBoom; m_hood = hood; m_visionOdometry = visionOdometry; - // this.isAuto = isAuto; + this.isAuto = isAuto; this.timeTolerance = 1000; addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry); SmartDashboard.putNumber("Distance Adjust", -35); } - // public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { - // this(turret, boomBoom, hood, visionOdometry, false); - // } + public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { + this(turret, boomBoom, hood, visionOdometry, false); + } // Called when the command is initially scheduled. @Override @@ -195,14 +195,14 @@ 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 { + if (this.isAuto) { + if (targetLocked && !timerStarted) { + timerStarted = true; + startTime = System.currentTimeMillis(); + } + return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + } else { return false; - // } + } } }