needs tuning

Author: davidikeda

src/main/java/frc/robot/RobotContainer.java

@@ -28,23 +28,26 @@ public class RobotContainer {
   private CommandXboxController controller = new CommandXboxController(1);
   private SwerveDriveState driveState = new SwerveDriveState();
   public static boolean isExtended = false;
+  public static boolean shootOnTheMove = false;
 
   private Command shootGroup =
       Commands.parallel(
               new KickerCommand(kicker, KickerCommand.Position.INTAKE),
               Commands.runEnd(
-                  () -> hopper.slowMove(HopperConfig.HOPPER_RETRACT_ROTATION),
-                  () -> hopper.stop(),
-                  hopper).withDeadline(Commands.waitSeconds(2)),
+                      () -> hopper.slowMove(HopperConfig.HOPPER_RETRACT_ROTATION),
+                      () -> hopper.stop(),
+                      hopper)
+                  .withDeadline(Commands.waitSeconds(2)),
               new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE))
           .finallyDo(
               interrupt ->
                   CommandScheduler.getInstance()
                       .schedule(
                           Commands.runEnd(
-                              () -> hopper.move(HopperConfig.HOPPER_EXTEND_ROTATION),
-                              () -> hopper.stop(),
-                              hopper).withDeadline(Commands.waitSeconds(2))));
+                                  () -> hopper.move(HopperConfig.HOPPER_EXTEND_ROTATION),
+                                  () -> hopper.stop(),
+                                  hopper)
+                              .withDeadline(Commands.waitSeconds(2))));
 
   public RobotContainer() {
     configureBindings();
@@ -74,9 +77,9 @@ private void configureBindings() {
         .toggleOnTrue(new IntakeCommand(intake, IntakeCommand.Position.INTAKE));
 
     controller
-            .povRight()
-            .and(() -> !shootGroup.isScheduled())
-            .toggleOnTrue(new IntakeCommand(intake, IntakeCommand.Position.OUTTAKE));
+        .povRight()
+        .and(() -> !shootGroup.isScheduled())
+        .toggleOnTrue(new IntakeCommand(intake, IntakeCommand.Position.OUTTAKE));
     // Right Stick Down = Extend/Retract Hopper
     controller
         .rightStick()
@@ -112,6 +115,19 @@ private void configureBindings() {
         .toggleOnTrue(
             new FlywheelCommand(flywheel, FlywheelCommand.Position.CALCULATED_SHOT, drivetrain));
     controller.povDown().onTrue(Commands.runOnce(() -> hopper.zero(), hopper));
+    // B = Shoot on the move toggle - placeholder button
+    controller
+        .b()
+        .whileTrue(
+            Commands.parallel(
+                new DrivetrainCommand(
+                    drivetrain,
+                    DrivetrainCommand.Position.SOTM,
+                    controller::getLeftX,
+                    controller::getLeftY,
+                    controller::getRightX),
+                new FlywheelCommand(
+                    flywheel, FlywheelCommand.Position.CALCULATED_SHOT, drivetrain)));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/DrivetrainCommand.java

@@ -6,10 +6,13 @@
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.config.TunerConstants;
 import frc.robot.helpers.ApplyModuleStates;
+import frc.robot.helpers.ShotCalculator;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import java.util.function.DoubleSupplier;
 
@@ -40,6 +43,8 @@ public static enum Position {
               SwerveModule.DriveRequestType
                   .OpenLoopVoltage); // Use open-loop control for drive motors
 
+  private static final double NOTE_EXIT_VELOCITY = 10.0; // This needs to be tuned
+
   SwerveModuleState[] states = {
     new SwerveModuleState(0, Rotation2d.fromDegrees(45)),
     new SwerveModuleState(0, Rotation2d.fromDegrees(135)),
@@ -88,12 +93,35 @@ public void execute() {
         System.out.println("Drivetrain: Still Shot Configuration");
         break;
 
-      // (Incomplete) Mode for moving shots
+      // (Incomplete) Mode for moving shots; pretty much finished but needs tuning and testing, and
+      // may need to be changed to use a different method of calculating the shot
       case SOTM:
-        // shoot on the move, reference Mechanical Advantage build log
-        System.out.println("Drivetrain: SOTM Drive");
-        break;
+        double vx = -leftY.getAsDouble() * MaxSpeed;
+        double vy = -leftX.getAsDouble() * MaxSpeed;
+        Translation2d robotPosition = subsystem.getState().Pose.getTranslation();
+        ChassisSpeeds speeds = subsystem.getState().Speeds;
+        Translation2d robotVelocity =
+            new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
+        Translation2d goal = ShotCalculator.calculateGoalPosition();
+        double distance = robotPosition.getDistance(goal);
+        double flightTime = distance / NOTE_EXIT_VELOCITY;
+        Translation2d toGoal = goal.minus(robotPosition);
+        Translation2d toGoalDir =
+            toGoal.getNorm() > 1e-6 ? toGoal.div(toGoal.getNorm()) : new Translation2d();
+        Translation2d lateralVelocity =
+            robotVelocity.minus(toGoalDir.times(robotVelocity.dot(toGoalDir)));
+        Translation2d virtualGoal = goal.minus(lateralVelocity.times(flightTime));
+        Rotation2d targetAngle = virtualGoal.minus(robotPosition).getAngle();
+        double currentHeading = subsystem.getState().Pose.getRotation().getRadians();
+        double targetHeading = targetAngle.getRadians();
+        double error = targetHeading - currentHeading;
+        error = Math.atan2(Math.sin(error), Math.cos(error));
+        double kP = 4.0; // tune
+        double omega = error * kP;
+        omega = Math.max(-MaxAngularRate, Math.min(MaxAngularRate, omega));
+        subsystem.setControl(drive.withVelocityX(vx).withVelocityY(vy).withRotationalRate(omega));
 
+        break;
       default:
         break;
     }