From 306901b451a6cc271d41809ffec8e02b15aeef21 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 24 Mar 2022 16:58:29 -0600 Subject: [PATCH] yes this is the most updated --- src/main/java/frc4388/robot/Robot.java | 6 +-- .../java/frc4388/robot/RobotContainer.java | 40 ++++++++++++------- .../commands/ShooterCommands/TrackTarget.java | 3 +- .../robot/subsystems/VisionOdometry.java | 2 +- 4 files changed, 30 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index bf6acb6..036bfa9 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -44,7 +44,7 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - private SendableChooser autoChooser = new SendableChooser(); + // private SendableChooser autoChooser = new SendableChooser(); // private double current; @@ -124,7 +124,7 @@ public class Robot extends TimedRobot { }); // desmosServer.start(); - m_robotContainer.m_robotVisionOdometry.setLEDs(false); + m_robotContainer.m_robotVisionOdometry.setLEDs(true); // DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0}); } @@ -186,7 +186,7 @@ public class Robot extends TimedRobot { LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); } - m_robotContainer.m_robotVisionOdometry.setLEDs(false); + m_robotContainer.m_robotVisionOdometry.setLEDs(true); } @Override diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bdc78d6..3a484e1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -103,8 +103,7 @@ public class RobotContainer { private enum ClimberMode { MANUAL, AUTONOMOUS }; private ClimberMode currentClimberMode = ClimberMode.MANUAL; - private Map autoChoices; - private SendableChooser quickAutoChooser = new SendableChooser<>(); + private SendableChooser quickAutoChooser = new SendableChooser<>(); /** * SmartDash @@ -130,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 @@ -200,14 +197,12 @@ public class RobotContainer { ))); - autoChoices = Map.of( - "justShoot", justShoot, - "offTheLine", offTheLine, - "twoBall", twoBall, - "threeBall", threeBall - ); + quickAutoChooser.setDefaultOption("justShoot", justShoot); + quickAutoChooser.addOption("offTheLine", offTheLine); + quickAutoChooser.addOption("twoBall", twoBall); + quickAutoChooser.addOption("threeBall", threeBall); - SmartDashboard.putData(quickAutoChooser); + SmartDashboard.putData("__QUICK AUTO CHOOSE__", quickAutoChooser); @@ -466,10 +461,25 @@ public class RobotContainer { // ! 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 - String selection = quickAutoChooser.getSelected(); - if (selection == null) - return new PrintCommand("---------- NO AUTO SELECTED --------------"); - return autoChoices.get(quickAutoChooser.getSelected()); + + return Objects.requireNonNullElseGet(quickAutoChooser.getSelected(), () -> new PrintCommand("---------- NO AUTO SELECTED --------------")); + + // 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. + + // 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 + // makeTheWeirdGroup(), // * 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, 1.0, 0.0, 0.0}, 0.25))); // * drive off line + } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 422ac23..facf491 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -111,7 +111,7 @@ public class TrackTarget extends CommandBase { double regressedDistance = getDistance(average.y); // ! no longer a +30 lol -aarav - double distAdj = SmartDashboard.getNumber("Distance Adjust", 20); + double distAdj = SmartDashboard.getNumber("Distance Adjust", -35); velocity = m_boomBoom.getVelocity(regressedDistance + distAdj); hoodPosition = m_boomBoom.getHood(regressedDistance + distAdj); @@ -190,7 +190,6 @@ public class TrackTarget extends CommandBase { @Override public void end(boolean interrupted) { m_visionOdometry.setLEDs(false); - m_visionOdometry.setDriverMode(true); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index 4711063..85f4b82 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -49,7 +49,7 @@ public class VisionOdometry extends SubsystemBase { m_shooter = shooter; setLEDs(false); - setDriverMode(true); + setDriverMode(false); } /** Gets the vision points from the limelight