From 5871b1fcef6e7210c4ed4029f11665c546cc1c40 Mon Sep 17 00:00:00 2001 From: Deven Speyrer Date: Sat, 12 Oct 2024 16:49:06 -0600 Subject: [PATCH] [deploy] Adjust controls + add yaw fudge --- src/main/java/frc/Robot.java | 601 +++++++++++++------------- src/main/java/frc/cmd/AimSpeaker.java | 297 ++++++------- 2 files changed, 450 insertions(+), 448 deletions(-) diff --git a/src/main/java/frc/Robot.java b/src/main/java/frc/Robot.java index 03a7525..67fc84b 100644 --- a/src/main/java/frc/Robot.java +++ b/src/main/java/frc/Robot.java @@ -35,304 +35,305 @@ import frc.cmd.*; public class Robot extends TimedRobot { - public static boolean isRedAlliance = false; - - SendableChooser autonomousChooser = new SendableChooser<>(); - - // Controllers - private final CommandXboxController driver = new CommandXboxController(0); - private final CommandXboxController operator = new CommandXboxController(1); - - // Subsystems - private final NetworkTable subsystemsTable = NetworkTableInstance.getDefault().getTable("Subsystems"); - - private final Swerve drivetrain = SwerveConfig.swerve; - - private final Transit transit = new Transit(subsystemsTable); - private final Intake intake = new Intake(subsystemsTable); - private final Shooter shooter = new Shooter(subsystemsTable); - private final Pivot pivot = new Pivot(subsystemsTable); - - // public Robot() { - // ! DO NOT USE - // } - - // Utilities - static double squareInput(double input) { - return Math.copySign(input * input, input); - } - - // TODO: add comments - private class Commands { - Command rumble(RumbleType type, double strength, double duration, - CommandXboxController... controllers) { - return new Command() { - @Override - public void initialize() { - for (CommandXboxController controller : controllers) - controller.getHID().setRumble(type, strength); - } - - @Override - public void end(boolean interrupted) { - for (CommandXboxController controller : controllers) - controller.getHID().setRumble(type, 0); - } - }.withTimeout(duration); - } - - /** - *
    - *
  1. Runs both the intake and the transit in.
    2. - *
  2. the intake will stop once the note is in the transit and then it will - * rumble the driver and operator
    3. - *
  3. the transit will stop when it has finished its feed in procedure (runs - * it in till it seas a note than backs it out and then runs it back in until - * it sees the note)
    4. - *
- * - * @return Command that requires PIVOT, INTAKE and TRANSIT - */ - Command intakeNote() { - return pivot.toIntakeUntilAimed() - .alongWith( - intake.runIn().until(() -> transit.hasNote()) - .andThen(rumble(RumbleType.kBothRumble, .75, .5, driver, operator)), - transit.feedIn()); - } - - Command primeAmp() { - return pivot.toAmp().alongWith(shooter.shootAmp(), new Command() { - @Override - public void initialize() { - // Point the shooter towards positive y, the direction of the amp entrance. - drivetrain.setRotationOverride(Rotation2d.fromDegrees(90)); - } - - @Override - public void end(boolean interrupted) { - drivetrain.setRotationOverride(null); - } - }); - } - - Command primeSpeaker() { - return new AimSpeaker(drivetrain, pivot, shooter); - } - - Command primeSubwoofer() { - return pivot.toSubwoofer().alongWith(shooter.shootSpeaker()); - } - - Command waitThenFeed() { - return new WaitCommand(2).andThen(transit.feedOut()); - } - - /** - * Currently crashes the robot code; do not use. - * TODO: Why?! - * - * @return - */ - @Deprecated - Command shoot() { - return transit.feedOut().andThen( - rumble(RumbleType.kBothRumble, .75, .5, driver, operator), - pivot.toIntakeUntilAimed()); - } - - // Auto Commands - Command intakeNoteAuto() { - return pivot.toIntakeUntilAimed() - .andThen( - intake.runIn().alongWith(transit.runForwards())) - .until(() -> transit.hasNote()); - } - - Command primeSpeakerAuto() { - return primeSpeaker().alongWith( - drivetrain.driveDutyCycle(() -> 0, () -> 0, () -> 0)); - } - - Command shootSpeakerAuto() { - return primeSpeakerAuto() - .alongWith(new WaitCommand(1).andThen(transit.runForwards())) - .until((() -> !transit.hasNote())); - } - - Command shootSpeakerAuto2() { - return primeSpeakerAuto() - .raceWith( - new WaitUntilCommand(() -> drivetrain.isAimed() && pivot.isAimed()) - .andThen(transit.runForwards().until((() -> !transit.hasNote())))); - } - } - - private final Commands cmds = new Commands(); - - private void configButtonBindings() { - // ----------- DEFAULT COMMANDS ----------- - - // NOTE: it appears that default commands are immediately rescheduled if they - // finish. Looks like we'll have to implement some special logic to go to the - // intake by default. - // pivot.setDefaultCommand(pivot.toIntake()); - - shooter.setDefaultCommand(shooter.VelocityOpenLoopCmd(true, 500)); - - // ----------- DRIVER CONTROLS ----------- - - drivetrain.setDefaultCommand(drivetrain.driveDutyCycle( - isRedAlliance ? () -> squareInput(driver.getLeftY()) : () -> squareInput(-driver.getLeftY()), - isRedAlliance ? () -> squareInput(driver.getLeftX()) : () -> squareInput(-driver.getLeftX()), - // Note: rotation does not need to be inverted based on alliance side, since rotational direction is not orientation-dependent. - () -> -driver.getRightX())); - - driver.leftTrigger().whileTrue(cmds.intakeNote()); - driver.leftBumper().onTrue(drivetrain.zeroGyro()); - - // driver.rightBumper().onTrue(drivetrain.zeroGyroToSubwoofer()); - - driver.x().whileTrue(drivetrain.brake()); - - // ---------- OPERATOR CONTROLS ---------- - - pivot.setDefaultCommand(pivot.dutyCycleCtrl(operator::getLeftY)); - - // TODO: make button map printout for operator - operator.leftTrigger().whileTrue(cmds.primeSpeaker()); - // TODO: Pathfind to the amp using a PathfindToPose command - operator.leftBumper().whileTrue(cmds.primeAmp()); - - // operator.rightTrigger().whileTrue(cmds.shoot()); - operator.rightTrigger().whileTrue(transit.runForwards()); - operator.rightBumper().whileTrue(transit.runBackward()); - - operator.a().whileTrue(shooter.shootSpeaker()); - operator.b().whileTrue(pivot.toIntakeUntilAimed()); - operator.y().whileTrue(cmds.primeSubwoofer()); - operator.x().whileTrue(intake.runOut()); - - // Zeroes the pivot, assuming it is at intake position. - operator.start().onTrue(new InstantCommand(pivot::currentZeroingSequence)); - - // TODO: Add climb command - } - - private void configAutos() { - configNamedCommands(); - // ------------------ AUTOS ------------------ - - autonomousChooser.addOption("Shoot", - cmds.primeSpeaker().raceWith( - new WaitUntilCommand(() -> drivetrain.isAimed() && pivot.isAimed()) - .andThen(cmds.waitThenFeed())) - // TODO: configure the next two as default commands (not working) - .andThen(shooter.stop().alongWith(pivot.toIntakeUntilAimed()))); - - autonomousChooser.addOption("Shoot against subwoofer", - new WaitCommand(5).andThen(cmds.primeSubwoofer().raceWith(cmds.waitThenFeed()))); - - autonomousChooser.addOption("Drive 3m (Red)", - drivetrain.driveVelocity(() -> -1, () -> 0, () -> 0) - .until(() -> { - return drivetrain.pose().getX() < -3.0; - }).andThen(drivetrain.brake())); - - SmartDashboard.putData("auto", autonomousChooser); - } - - private void configNamedCommands() { - NamedCommands.registerCommand("intake", cmds.intakeNoteAuto()); - NamedCommands.registerCommand("feedIn", transit.feedIn()); - NamedCommands.registerCommand("shoot", cmds.shootSpeakerAuto()); - } - - @Override - public void robotInit() { - // Register alliance - isRedAlliance = DriverStation.getAlliance().filter(a -> a == Alliance.Red).isPresent(); - // Add to NetworkTables for verification - SmartDashboard.putBoolean("isRedAlliance", isRedAlliance); - configButtonBindings(); - - // ------------------- Logging ------------------- - DataLogManager.start(); - DriverStation.startDataLog(DataLogManager.getLog()); - - if (isSimulation()) { - DriverStation.silenceJoystickConnectionWarning(true); - } - - PortForwarder.add(5800, "photonvision.local", 5800); - configNamedCommands(); - autonomousChooser = AutoBuilder.buildAutoChooser(); - configAutos(); - } - - @Override - public void robotPeriodic() { - CommandScheduler.getInstance().run(); - // if (drivetrain.isCamConnected()) { - drivetrain.addPhotonVision(); - // } - - } - - @Override - public void disabledInit() { - } - - @Override - public void disabledPeriodic() { - } - - @Override - public void disabledExit() { - } - - private Command autonomousCommand; - - @Override - public void autonomousInit() { - autonomousCommand = autonomousChooser.getSelected(); - if (autonomousCommand != null) - autonomousCommand.schedule(); - } - - @Override - public void autonomousPeriodic() { - } - - @Override - public void autonomousExit() { - if (autonomousCommand != null) - autonomousCommand.cancel(); - } - - @Override - public void teleopInit() { - } - - @Override - public void teleopPeriodic() { - } - - @Override - public void teleopExit() { - } - - @Override - public void testInit() { - // TODO: Run an automated full systems check - CommandScheduler.getInstance().cancelAll(); - } - - @Override - public void testPeriodic() { - } - - @Override - public void testExit() { - } + public static boolean isRedAlliance = false; + + SendableChooser autonomousChooser = new SendableChooser<>(); + + // Controllers + private final CommandXboxController driver = new CommandXboxController(0); + private final CommandXboxController operator = new CommandXboxController(1); + + // Subsystems + private final NetworkTable subsystemsTable = NetworkTableInstance.getDefault().getTable("Subsystems"); + + private final Swerve drivetrain = SwerveConfig.swerve; + + private final Transit transit = new Transit(subsystemsTable); + private final Intake intake = new Intake(subsystemsTable); + private final Shooter shooter = new Shooter(subsystemsTable); + private final Pivot pivot = new Pivot(subsystemsTable); + + // public Robot() { + // ! DO NOT USE + // } + + // Utilities + static double squareInput(double input) { + return Math.copySign(input * input, input); + } + + // TODO: add comments + private class Commands { + Command rumble(RumbleType type, double strength, double duration, + CommandXboxController... controllers) { + return new Command() { + @Override + public void initialize() { + for (CommandXboxController controller : controllers) + controller.getHID().setRumble(type, strength); + } + + @Override + public void end(boolean interrupted) { + for (CommandXboxController controller : controllers) + controller.getHID().setRumble(type, 0); + } + }.withTimeout(duration); + } + + /** + *
    + *
  1. Runs both the intake and the transit in.
    2. + *
  2. the intake will stop once the note is in the transit and then it will + * rumble the driver and operator
    3. + *
  3. the transit will stop when it has finished its feed in procedure (runs + * it in till it seas a note than backs it out and then runs it back in until + * it sees the note)
    4. + *
+ * + * @return Command that requires PIVOT, INTAKE and TRANSIT + */ + Command intakeNote() { + return pivot.toIntakeUntilAimed() + .alongWith( + intake.runIn().until(() -> transit.hasNote()) + .andThen(rumble(RumbleType.kBothRumble, .75, .5, driver, operator)), + transit.feedIn()); + } + + Command primeAmp() { + return pivot.toAmp().alongWith(shooter.shootAmp(), new Command() { + @Override + public void initialize() { + // Point the shooter towards positive y, the direction of the amp entrance. + drivetrain.setRotationOverride(Rotation2d.fromDegrees(90)); + } + + @Override + public void end(boolean interrupted) { + drivetrain.setRotationOverride(null); + } + }); + } + + Command primeSpeaker() { + return new AimSpeaker(drivetrain, pivot, shooter); + } + + Command primeSubwoofer() { + return pivot.toSubwoofer().alongWith(shooter.shootSpeaker()); + } + + Command waitThenFeed() { + return new WaitCommand(2).andThen(transit.feedOut()); + } + + /** + * Currently crashes the robot code; do not use. + * TODO: Why?! + * + * @return + */ + @Deprecated + Command shoot() { + return transit.feedOut().andThen( + rumble(RumbleType.kBothRumble, .75, .5, driver, operator), + pivot.toIntakeUntilAimed()); + } + + // Auto Commands + Command intakeNoteAuto() { + return pivot.toIntakeUntilAimed() + .andThen( + intake.runIn().alongWith(transit.runForwards())) + .until(() -> transit.hasNote()); + } + + Command primeSpeakerAuto() { + return primeSpeaker().alongWith( + drivetrain.driveDutyCycle(() -> 0, () -> 0, () -> 0)); + } + + Command shootSpeakerAuto() { + return primeSpeakerAuto() + .alongWith(new WaitCommand(1).andThen(transit.runForwards())) + .until((() -> !transit.hasNote())); + } + + Command shootSpeakerAuto2() { + return primeSpeakerAuto() + .raceWith( + new WaitUntilCommand(() -> drivetrain.isAimed() && pivot.isAimed()) + .andThen(transit.runForwards().until((() -> !transit.hasNote())))); + } + } + + private final Commands cmds = new Commands(); + + private void configButtonBindings() { + // ----------- DEFAULT COMMANDS ----------- + + // NOTE: it appears that default commands are immediately rescheduled if they + // finish. Looks like we'll have to implement some special logic to go to the + // intake by default. + // pivot.setDefaultCommand(pivot.toIntake()); + + shooter.setDefaultCommand(shooter.VelocityOpenLoopCmd(true, 500)); + + // ----------- DRIVER CONTROLS ----------- + + drivetrain.setDefaultCommand(drivetrain.driveDutyCycle( + isRedAlliance ? () -> squareInput(driver.getLeftY()) : () -> squareInput(-driver.getLeftY()), + isRedAlliance ? () -> squareInput(driver.getLeftX()) : () -> squareInput(-driver.getLeftX()), + // Note: rotation does not need to be inverted based on alliance side, since + // rotational direction is not orientation-dependent. + () -> -driver.getRightX())); + + driver.leftTrigger().whileTrue(cmds.intakeNote()); + driver.rightTrigger().whileTrue(intake.runOut()); + + driver.leftBumper().onTrue(drivetrain.zeroGyro()); + + // driver.rightBumper().onTrue(drivetrain.zeroGyroToSubwoofer()); + + driver.x().whileTrue(drivetrain.brake()); + + // ---------- OPERATOR CONTROLS ---------- + + pivot.setDefaultCommand(pivot.dutyCycleCtrl(operator::getLeftY)); + + + // TODO: make button map printout for operator + operator.leftTrigger().whileTrue(cmds.primeSpeaker()); + // TODO: Pathfind to the amp using a PathfindToPose command + operator.leftBumper().whileTrue(cmds.primeAmp()); + + operator.rightTrigger().whileTrue(transit.runForwards()); + operator.rightBumper().whileTrue(transit.runBackward()); + + operator.a().whileTrue(shooter.shootSpeaker()); + operator.y().whileTrue(cmds.primeSubwoofer()); + + // Zeroes the pivot, assuming it is at intake position. + operator.start().onTrue(new InstantCommand(pivot::currentZeroingSequence)); + + // TODO: Add climb command + } + + private void configAutos() { + configNamedCommands(); + // ------------------ AUTOS ------------------ + + autonomousChooser.addOption("Shoot", + cmds.primeSpeaker().raceWith( + new WaitUntilCommand(() -> drivetrain.isAimed() && pivot.isAimed()) + .andThen(cmds.waitThenFeed())) + // TODO: configure the next two as default commands (not working) + .andThen(shooter.stop().alongWith(pivot.toIntakeUntilAimed()))); + + autonomousChooser.addOption("Shoot against subwoofer", + new WaitCommand(5).andThen(cmds.primeSubwoofer().raceWith(cmds.waitThenFeed()))); + + autonomousChooser.addOption("Drive 3m (Red)", + drivetrain.driveVelocity(() -> -1, () -> 0, () -> 0) + .until(() -> { + return drivetrain.pose().getX() < -3.0; + }).andThen(drivetrain.brake())); + + SmartDashboard.putData("auto", autonomousChooser); + } + + private void configNamedCommands() { + NamedCommands.registerCommand("intake", cmds.intakeNoteAuto()); + NamedCommands.registerCommand("feedIn", transit.feedIn()); + NamedCommands.registerCommand("shoot", cmds.shootSpeakerAuto()); + } + + @Override + public void robotInit() { + // Register alliance + isRedAlliance = DriverStation.getAlliance().filter(a -> a == Alliance.Red).isPresent(); + // Add to NetworkTables for verification + SmartDashboard.putBoolean("isRedAlliance", isRedAlliance); + configButtonBindings(); + + // ------------------- Logging ------------------- + DataLogManager.start(); + DriverStation.startDataLog(DataLogManager.getLog()); + + if (isSimulation()) { + DriverStation.silenceJoystickConnectionWarning(true); + } + + PortForwarder.add(5800, "photonvision.local", 5800); + configNamedCommands(); + autonomousChooser = AutoBuilder.buildAutoChooser(); + configAutos(); + } + + @Override + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + // if (drivetrain.isCamConnected()) { + drivetrain.addPhotonVision(); + // } + + } + + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + @Override + public void disabledExit() { + } + + private Command autonomousCommand; + + @Override + public void autonomousInit() { + autonomousCommand = autonomousChooser.getSelected(); + if (autonomousCommand != null) + autonomousCommand.schedule(); + } + + @Override + public void autonomousPeriodic() { + } + + @Override + public void autonomousExit() { + if (autonomousCommand != null) + autonomousCommand.cancel(); + } + + @Override + public void teleopInit() { + } + + @Override + public void teleopPeriodic() { + } + + @Override + public void teleopExit() { + } + + @Override + public void testInit() { + // TODO: Run an automated full systems check + CommandScheduler.getInstance().cancelAll(); + } + + @Override + public void testPeriodic() { + } + + @Override + public void testExit() { + } } diff --git a/src/main/java/frc/cmd/AimSpeaker.java b/src/main/java/frc/cmd/AimSpeaker.java index e3543f0..83bcbbb 100644 --- a/src/main/java/frc/cmd/AimSpeaker.java +++ b/src/main/java/frc/cmd/AimSpeaker.java @@ -16,152 +16,153 @@ import frc.system.Swerve; public class AimSpeaker extends Command { - private final String simpleName = this.getClass().getSimpleName(); - - // Subsystems - private final Swerve drivetrain; - private final Pivot pivot; - private final Shooter shooter; - - // Network - /** the sub table where all logging for Shooter should go */ - private final NetworkTable autoAimTable; - - /** - * the offset for the rotation of the pivot positive shoots higher/lower - */ - private final DoubleSubscriber rotationFudge; - /** offest the rotation of the robot RADS Positive rottes x */ - private final DoubleSubscriber yawFudge; - /** the offset for rpm of the shooter speed */ - private final DoubleSubscriber shooterFudge; - - private double yaw; - private double distance; - private double pitch; - private double rotations; - private double shooterSpeed; - - private final DoublePublisher yawPub; - private final DoublePublisher distancePub; - private final DoublePublisher rotationsPub; - private final DoublePublisher shooterSpeedPub; - - // Vars - private final Translation3d speakerPosition; - // private DoubleSubscriber fudgeFactor; - - public AimSpeaker(Swerve drivetrain, Pivot pivot, Shooter shooter) { - // Network tables - this.autoAimTable = NetworkTableInstance.getDefault().getTable("Commands").getSubTable(simpleName); - - this.autoAimTable.getDoubleTopic("RotationFudge").publish(); - rotationFudge = autoAimTable.getDoubleTopic("RotationFudge").subscribe(0); - this.autoAimTable.getDoubleTopic("yawFudge").publish(); - yawFudge = autoAimTable.getDoubleTopic("yawFudge").subscribe(0); - this.autoAimTable.getDoubleTopic("ShooterFudge").publish(); - shooterFudge = autoAimTable.getDoubleTopic("ShooterFudge").subscribe(0); - - yawPub = this.autoAimTable.getDoubleTopic("yaw").publish(); - distancePub = this.autoAimTable.getDoubleTopic("distance").publish(); - rotationsPub = this.autoAimTable.getDoubleTopic("rotations").publish(); - shooterSpeedPub = this.autoAimTable.getDoubleTopic("shooterSpeed").publish(); - - // Subsystems - this.drivetrain = drivetrain; - this.pivot = pivot; - this.shooter = shooter; - - // Vars - speakerPosition = Robot.isRedAlliance - ? - // Red speaker - new Translation3d(16.31, 5.55, 2.06) - // Blue speaker - : new Translation3d(0.24, 5.55, 2.06); - - addRequirements(pivot, shooter); - } - - public void doMath() { - // Rose to speaker - Pose2d mechanismPose = drivetrain.pose() - .plus(new Transform2d(pivot.origin.getX(), pivot.origin.getY(), - new Rotation2d())); - Translation3d mechanism = new Translation3d(mechanismPose.getX(), mechanismPose.getY(), - pivot.origin.getZ()); - Translation3d vectorToSpeaker = speakerPosition.minus(mechanism); - - // Drivetrain yaw - yaw = Math.atan2(vectorToSpeaker.getY(), vectorToSpeaker.getX()); - yaw += 0.175; - yaw += yawFudge.get(0); - - // pivot Angle - distance = vectorToSpeaker.getNorm(); - pitch = Math.asin(vectorToSpeaker.getZ() / distance); - rotations = Units - .radiansToRotations(pivot.armOffsetRad + Math.asin(pivot.exitDistance / - distance) - pitch) - + rotationFudge.get(0); - rotations += -0.005; - rotations += -0.00125 * distance; - //might want to add + .02 - - // Shooter Speed - shooterSpeed = 4000; - shooterSpeed += 100 * distance; - shooterSpeed += shooterFudge.get(0); - - if (shooterSpeed > 5000) - shooterSpeed = 5000; - - // TODO: switch to voltage - // shooterSpeed = 0.75; - // shooterSpeed += 0.05 * distance; - // shooterSpeed += shooterFudge.get(0); - } - - @Override - public void execute() { - doMath(); - log(); - - pivot.MMPositionCtrl(rotations); - drivetrain.setRotationOverride(Rotation2d.fromRadians(yaw)); - shooter.VelocityOpenLoop(true, shooterSpeed); - // TODO: switch to voltage - // shooter.voltageCtrl(shooterSpeed); - } - - @Override - public void end(boolean interrupted) { - drivetrain.setRotationOverride(null); - // shooter.disable(); - } - - public void log() { - // drivetrain.log(); - // pivot.log(); - - yawPub.set(yaw); - rotationsPub.set(rotations); - distancePub.set(distance); - shooterSpeedPub.set(shooterSpeed); - - } - - // public void close() { - // // Subsystems - // drivetrain.close(); - // pivot.close(); - - // // // Network Table - // // rotationFudge.close(); - // // yawFudge.close(); - - // // yaw.close(); - // // rotations.close(); - // // distance.close(); - // } + private final String simpleName = this.getClass().getSimpleName(); + + // Subsystems + private final Swerve drivetrain; + private final Pivot pivot; + private final Shooter shooter; + + // Network + /** the sub table where all logging for Shooter should go */ + private final NetworkTable autoAimTable; + + /** + * the offset for the rotation of the pivot positive shoots higher/lower + */ + private final DoubleSubscriber rotationFudge; + /** offest the rotation of the robot RADS Positive rottes x */ + private final DoubleSubscriber yawFudge; + /** the offset for rpm of the shooter speed */ + private final DoubleSubscriber shooterFudge; + + private double yaw; + private double distance; + private double pitch; + private double rotations; + private double shooterSpeed; + + private final DoublePublisher yawPub; + private final DoublePublisher distancePub; + private final DoublePublisher rotationsPub; + private final DoublePublisher shooterSpeedPub; + + // Vars + private final Translation3d speakerPosition; + // private DoubleSubscriber fudgeFactor; + + public AimSpeaker(Swerve drivetrain, Pivot pivot, Shooter shooter) { + // Network tables + this.autoAimTable = NetworkTableInstance.getDefault().getTable("Commands").getSubTable(simpleName); + + this.autoAimTable.getDoubleTopic("RotationFudge").publish(); + rotationFudge = autoAimTable.getDoubleTopic("RotationFudge").subscribe(0); + this.autoAimTable.getDoubleTopic("yawFudge").publish(); + yawFudge = autoAimTable.getDoubleTopic("yawFudge").subscribe(0); + this.autoAimTable.getDoubleTopic("ShooterFudge").publish(); + shooterFudge = autoAimTable.getDoubleTopic("ShooterFudge").subscribe(0); + + yawPub = this.autoAimTable.getDoubleTopic("yaw").publish(); + distancePub = this.autoAimTable.getDoubleTopic("distance").publish(); + rotationsPub = this.autoAimTable.getDoubleTopic("rotations").publish(); + shooterSpeedPub = this.autoAimTable.getDoubleTopic("shooterSpeed").publish(); + + // Subsystems + this.drivetrain = drivetrain; + this.pivot = pivot; + this.shooter = shooter; + + // Vars + speakerPosition = Robot.isRedAlliance + ? + // Red speaker + new Translation3d(16.31, 5.55, 2.06) + // Blue speaker + : new Translation3d(0.24, 5.55, 2.06); + + addRequirements(pivot, shooter); + } + + public void doMath() { + // Rose to speaker + Pose2d mechanismPose = drivetrain.pose() + .plus(new Transform2d(pivot.origin.getX(), pivot.origin.getY(), + new Rotation2d())); + Translation3d mechanism = new Translation3d(mechanismPose.getX(), mechanismPose.getY(), + pivot.origin.getZ()); + Translation3d vectorToSpeaker = speakerPosition.minus(mechanism); + + // Drivetrain yaw + yaw = Math.atan2(vectorToSpeaker.getY(), vectorToSpeaker.getX()); + yaw += 0.175; + yaw += -0.2; + yaw += yawFudge.get(0); + + // pivot Angle + distance = vectorToSpeaker.getNorm(); + pitch = Math.asin(vectorToSpeaker.getZ() / distance); + rotations = Units + .radiansToRotations(pivot.armOffsetRad + Math.asin(pivot.exitDistance / + distance) - pitch) + + rotationFudge.get(0); + rotations += -0.005; + rotations += -0.00125 * distance; + // might want to add + .02 + + // Shooter Speed + shooterSpeed = 4000; + shooterSpeed += 100 * distance; + shooterSpeed += shooterFudge.get(0); + + if (shooterSpeed > 5000) + shooterSpeed = 5000; + + // TODO: switch to voltage + // shooterSpeed = 0.75; + // shooterSpeed += 0.05 * distance; + // shooterSpeed += shooterFudge.get(0); + } + + @Override + public void execute() { + doMath(); + log(); + + pivot.MMPositionCtrl(rotations); + drivetrain.setRotationOverride(Rotation2d.fromRadians(yaw)); + shooter.VelocityOpenLoop(true, shooterSpeed); + // TODO: switch to voltage + // shooter.voltageCtrl(shooterSpeed); + } + + @Override + public void end(boolean interrupted) { + drivetrain.setRotationOverride(null); + // shooter.disable(); + } + + public void log() { + // drivetrain.log(); + // pivot.log(); + + yawPub.set(yaw); + rotationsPub.set(rotations); + distancePub.set(distance); + shooterSpeedPub.set(shooterSpeed); + + } + + // public void close() { + // // Subsystems + // drivetrain.close(); + // pivot.close(); + + // // // Network Table + // // rotationFudge.close(); + // // yawFudge.close(); + + // // yaw.close(); + // // rotations.close(); + // // distance.close(); + // } }