Add alliance flipping for teleop driving

awtybots · Feb 29, 2024 · 70db4fe · 70db4fe
1 parent 30bfa42
commit 70db4fe
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java
@@ -19,6 +19,8 @@
 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.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.drive.Drive;
@@ -70,6 +72,10 @@ public static Command joystickDrive(
           linearMagnitude = linearMagnitude * linearMagnitude;
           omega = Math.copySign(omega * omega, omega);
 
+          boolean isFlipped =
+              DriverStation.getAlliance().isPresent()
+                  && DriverStation.getAlliance().get() == Alliance.Red;
+
           // Calcaulate new linear velocity
           Translation2d linearVelocity =
               new Pose2d(new Translation2d(), linearDirection)
@@ -82,7 +88,9 @@ public static Command joystickDrive(
                   linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
                   linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
                   omega * drive.getMaxAngularSpeedRadPerSec(),
-                  drive.getRotation()));
+                  isFlipped
+                      ? drive.getRotation().plus(new Rotation2d(Math.PI))
+                      : drive.getRotation()));
         },
         drive);
   }

diff --git a/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java b/src/main/java/frc/robot/commands/ControlCommands/IntakeShooterControls.java
@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328, FRC 5829
+// Copyright 2021-2024 FRC 5829
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or
@@ -24,7 +24,6 @@
 
 public class IntakeShooterControls {
   private static final double DEADBAND = 0.3;
-  private static final double MAXINCHESPERSECOND = 3;
 
   private IntakeShooterControls() {}
 
@@ -42,8 +41,10 @@ public static Command intakeShooterDrive(
           if (rightBumperSupplier.getAsBoolean()) {
             flywheel.runVelocity(-Constants.FlywheelConstants.shootingVelocity);
 
-            double flywheelRPM = -flywheel.getVelocityRPMBottom();
-            double targetRPM = Constants.FlywheelConstants.shootingVelocity * 0.9;
+            double flywheelRPM = flywheel.getVelocityRPMBottom();
+            double targetRPM =
+                Constants.FlywheelConstants.shootingVelocity
+                    + 2000; // TODO find actual max velocity
 
             if (flywheelRPM > targetRPM) {
               intake.runPercentSpeed(1);