namedcommand for spin aimbot

hammerhead226 · Apr 13, 2024 · b528f50 · b528f50
1 parent 2c6ae63
commit b528f50
Show file tree

Hide file tree

Showing 2 changed files with 245 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -34,6 +34,7 @@
 import frc.robot.Constants.NoteState;
 import frc.robot.Constants.SHOOT_STATE;
 import frc.robot.commands.AimbotAuto;
+import frc.robot.commands.AimbotStatic;
 import frc.robot.commands.AimbotTele;
 import frc.robot.commands.AlignToNoteAuto;
 import frc.robot.commands.AlignToNoteTele;
@@ -444,7 +445,7 @@ public RobotContainer() {
             .andThen(
                 new WaitCommand(1.5).andThen(new InstantCommand(() -> shooter.stopFeeders()))));
     NamedCommands.registerCommand(
-        "AimbotStatic", new AimbotTele(drive, driveController, shooter, pivot, led));
+        "AimbotStatic", new AimbotStatic(drive, driveController, shooter, pivot, led));
     NamedCommands.registerCommand("AimbotMoving", new AimbotAuto(drive, shooter, pivot, led));
 
     NamedCommands.registerCommand(
@@ -690,7 +691,8 @@ private void driverControls() {
     driveController
         .leftBumper()
         .onFalse(
-            new InstantCommand(() -> shooter.setFeedersRPM(500))
+            new InstantCommand(() -> led.setState(LED_STATE.BLUE)).andThen(new InstantCommand(() -> shooter.setFeedersRPM(500)))
+
                 .andThen(new WaitCommand(0.02))
                 .andThen(
                     new ConditionalCommand(
@@ -736,7 +738,8 @@ private void driverControls() {
     driveController
         .rightBumper()
         .onFalse(
-            new InstantCommand(() -> shooter.setFeedersRPM(500))
+            new InstantCommand(() -> led.setState(LED_STATE.BLUE)).andThen( new InstantCommand(() -> shooter.setFeedersRPM(500)))
+
                 .andThen(new WaitCommand(0.02))
                 .andThen(
                     new ConditionalCommand(

diff --git a/src/main/java/frc/robot/commands/AimbotStatic.java b/src/main/java/frc/robot/commands/AimbotStatic.java
@@ -0,0 +1,239 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Constants;
+import frc.robot.Constants.LED_STATE;
+import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.led.LED;
+import frc.robot.subsystems.pivot.Pivot;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.util.FieldConstants;
+import org.littletonrobotics.junction.Logger;
+
+public class AimbotStatic extends Command {
+
+  private final Drive drive;
+  private final Shooter shooter;
+  private final Pivot pivot;
+  private final LED led;
+
+  private final CommandXboxController controller;
+  private final PIDController pid;
+  private double[] gains = new double[3];
+  private DriverStation.Alliance alliance = null;
+
+  private double distanceToSpeakerMeter = 0;
+  private double pivotSetpointDeg = 0;
+  /** Creates a new Aimbot. */
+  public AimbotStatic(
+      Drive drive, CommandXboxController controller, Shooter shooter, Pivot pivot, LED led) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.drive = drive;
+    this.shooter = shooter;
+    this.pivot = pivot;
+    this.led = led;
+
+    this.controller = controller;
+
+    addRequirements(drive, shooter, pivot, led);
+
+    switch (Constants.currentMode) {
+      case REAL:
+        gains[0] = 2.5;
+        gains[1] = 0;
+        gains[2] = 0;
+        break;
+      case REPLAY:
+        gains[0] = 0;
+        gains[1] = 0;
+        gains[2] = 0;
+        break;
+      case SIM:
+        gains[0] = 13;
+        gains[1] = 0;
+        gains[2] = 01;
+        break;
+      default:
+        gains[0] = 0;
+        gains[1] = 0;
+        gains[2] = 0;
+        break;
+    }
+
+    pid = new PIDController(gains[0], gains[1], gains[2], 0.02);
+    pid.setTolerance(3);
+    pid.enableContinuousInput(-180, 180);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    led.setState(LED_STATE.FLASHING_GREEN);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    turnToSpeaker();
+    angleShooter();
+
+    Logger.recordOutput("distance from speak", Units.metersToFeet(calculateDistanceToSpeaker()));
+
+    // if (Units.metersToFeet(calculateDistanceToSpeaker()) > 12) {
+    //   led.setState(LED_STATE.FLASHING_RED);
+    // } else {
+    //   led.setState(LED_STATE.GREEN);
+    // }
+  }
+
+  public void angleShooter() {
+    if (DriverStation.getAlliance().isPresent()) this.alliance = DriverStation.getAlliance().get();
+    // Logger.recordOutput("distance to speak", Units.metersToFeet(distanceToSpeakerMeter));
+    distanceToSpeakerMeter = calculateDistanceToSpeaker();
+    if (Units.metersToFeet(distanceToSpeakerMeter) > 12) {
+      // double shootingSpeed =
+      //     MathUtil.clamp(
+      //         calculateShooterSpeed(Units.metersToFeet(distanceToSpeakerMeter)), 3250, 5400);
+      double shootingSpeed = calculateShooterSpeed(Units.metersToFeet(distanceToSpeakerMeter));
+      if (alliance == Alliance.Blue) {
+        // source side
+        if (drive.getPose().getY() < 4.5) shooter.setFlywheelRPMs(shootingSpeed, shootingSpeed + 250);
+        // amp side
+        else if (drive.getPose().getY() > 6.5) shooter.setFlywheelRPMs(shootingSpeed + 250, shootingSpeed);
+        // center
+        else shooter.setFlywheelRPMs(shootingSpeed, shootingSpeed);
+      } else {
+        // source side
+        if (drive.getPose().getY() < 4.5) shooter.setFlywheelRPMs(shootingSpeed + 250, shootingSpeed);
+        // amp side
+        else if (drive.getPose().getY() > 6.5) shooter.setFlywheelRPMs(shootingSpeed, shootingSpeed + 250);
+        // center
+        else shooter.setFlywheelRPMs(shootingSpeed, shootingSpeed);
+      }
+    } else shooter.setFlywheelRPMs(5400, 5400);
+    pivot.setPivotGoal(calculatePivotAngleDeg(distanceToSpeakerMeter));
+  }
+
+  private double calculateShooterSpeed(double distanceToSpeakerFeet) {
+    double shooterSpeed = -986.49 * distanceToSpeakerFeet + 17294.6;
+    // shooterSpeed = MathUtil.clamp(shooterSpeed, 3850, 5400);
+    shooterSpeed = MathUtil.clamp(shooterSpeed, 4000, 5400);
+    // if (distanceToSpeakerFeet >= 11) {
+    // return -430.7 * distanceToSpeakerFeet + 8815;
+    // } else return -600. * distanceToSpeakerFeet + 10406;
+    return shooterSpeed;
+  }
+
+  private double calculatePivotAngleDeg(double distanceToSpeakerMeter) {
+    // pivotSetpointDeg = (-0.272 * Math.abs(Units.metersToInches(distanceToSpeakerMeter) - 36) +
+    // 60);
+    pivotSetpointDeg = (-0.253 * Math.abs(Units.metersToInches(distanceToSpeakerMeter) - 36) + 60);
+    pivotSetpointDeg = MathUtil.clamp(pivotSetpointDeg, 34, 62);
+
+    if (Units.metersToFeet(distanceToSpeakerMeter) > 12) {
+      return 34;
+    }
+    Logger.recordOutput("pivot target auto", pivotSetpointDeg);
+    return pivotSetpointDeg;
+  }
+
+  private double calculateDistanceToSpeaker() {
+    double x = 0;
+    double y = 0;
+
+    if (alliance == DriverStation.Alliance.Red) {
+      x = FieldConstants.fieldLength - drive.getPose().getX();
+      y = FieldConstants.Speaker.speakerCenterY - drive.getPose().getY();
+    } else {
+      x = -drive.getPose().getX();
+      y = FieldConstants.Speaker.speakerCenterY - drive.getPose().getY();
+    }
+
+    return Math.hypot(x, y);
+  }
+
+  public void turnToSpeaker() {
+    double targetAngle;
+    if (DriverStation.getAlliance().isPresent()) this.alliance = DriverStation.getAlliance().get();
+
+    if (alliance == DriverStation.Alliance.Red) {
+      targetAngle =
+          new Rotation2d(
+                      FieldConstants.fieldLength - drive.getPose().getX(),
+                      FieldConstants.Speaker.speakerCenterY - drive.getPose().getY())
+                  .getDegrees()
+              + 180;
+      pid.setSetpoint(
+          new Rotation2d(
+                      FieldConstants.fieldLength - drive.getPose().getX(),
+                      FieldConstants.Speaker.speakerCenterY - drive.getPose().getY())
+                  .getDegrees()
+              + 180);
+    } else {
+      targetAngle =
+          new Rotation2d(
+                      -drive.getPose().getX(),
+                      FieldConstants.Speaker.speakerCenterY - drive.getPose().getY())
+                  .getDegrees()
+              + 180;
+      pid.setSetpoint(
+          new Rotation2d(
+                      -drive.getPose().getX(),
+                      FieldConstants.Speaker.speakerCenterY - drive.getPose().getY())
+                  .getDegrees()
+              + 180);
+    }
+
+    Logger.recordOutput("Rotation error", pid.getPositionError());
+
+    Logger.recordOutput("target angle", targetAngle);
+    double linearMagnitude =
+        MathUtil.applyDeadband(Math.hypot(-controller.getLeftY(), -controller.getLeftX()), 0.1);
+    Rotation2d linearDirection = new Rotation2d(-controller.getLeftY(), -controller.getLeftX());
+
+    // Square values
+    linearMagnitude = linearMagnitude * linearMagnitude;
+    // Calcaulate new linear velocity
+    Translation2d linearVelocity =
+        new Pose2d(new Translation2d(), linearDirection)
+            .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
+            .getTranslation();
+    double angularSpeed = pid.calculate(drive.getPose().getRotation().getDegrees());
+
+    drive.runVelocity(
+        ChassisSpeeds.fromFieldRelativeSpeeds(
+            linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
+            linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
+            Math.toRadians(angularSpeed),
+            drive.getPose().getRotation()));
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    shooter.setFeedersRPM(1000);
+    led.setState(LED_STATE.GREY);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    Logger.recordOutput("i am currently this angle", drive.getRotation().getDegrees());
+    return pid.atSetpoint() && shooter.atFlywheelSetpoints() && pivot.atGoal();
+  }
+}