Merge pull request #6 from FRC-4509-MechBulls/✨auto-align

Auto creation/tweaks
FRC-4509-MechBulls · Oct 26, 2023 · 6e49c17 · 6e49c17
2 parents 47fb704 + 23d2e8c
commit 6e49c17
Show file tree

Hide file tree

Showing 11 changed files with 448 additions and 59 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -4,10 +4,14 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.util.Units;
 
+import java.util.ArrayList;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
  * constants. This class should not be used for any other purpose. All constants should be declared
@@ -34,6 +38,7 @@ public static class OperatorConstants {
     public static final double turnExponent = 1.8;
     public static final double turnMaxSpeed = 11; //11
 
+
     public static final double maxDrivePower = 1;
 
     public static final double controllerDeadband = 0.06;
@@ -167,4 +172,34 @@ public static class EfConstants { //end effector motor ids
   }
 
 
+  public static final class FieldConstants{
+
+    public static Pose2d[] alignmentPoses = new Pose2d[18];
+
+    public static final double[] nodeYValues = new double[] {
+            Units.inchesToMeters(20.19 + 22.0 * 0),
+            Units.inchesToMeters(20.19 + 22.0 * 1),
+            Units.inchesToMeters(20.19 + 22.0 * 2),
+            Units.inchesToMeters(20.19 + 22.0 * 3),
+            Units.inchesToMeters(20.19 + 22.0 * 4),
+            Units.inchesToMeters(20.19 + 22.0 * 5),
+            Units.inchesToMeters(20.19 + 22.0 * 6),
+            Units.inchesToMeters(20.19 + 22.0 * 7),
+            Units.inchesToMeters(20.19 + 22.0 * 8)
+  };
+
+
+
+    public static final double blueAlignmentX = Units.inchesToMeters(69.0625);
+    public static final double fieldLength = Units.inchesToMeters(651.25);
+    public static final double redAlignmentX = fieldLength - blueAlignmentX;
+
+    static{
+      for(int i = 0; i<9; i++){
+        alignmentPoses[i] = new Pose2d(blueAlignmentX,nodeYValues[i], Rotation2d.fromDegrees(180));
+        alignmentPoses[i+9] = new Pose2d(redAlignmentX,nodeYValues[i], Rotation2d.fromDegrees(0));
+      }
+    }
+
+  }
 }
diff --git a/src/main/java/frc/robot/FMSGetter.java b/src/main/java/frc/robot/FMSGetter.java
@@ -0,0 +1,18 @@
+package frc.robot;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import org.apache.commons.math3.ml.neuralnet.Network;
+
+public class FMSGetter {
+
+    static NetworkTableInstance inst = NetworkTableInstance.getDefault();
+    static NetworkTable table = inst.getTable("FMSInfo");
+
+    public static boolean isRedAlliance(){
+        if(table.getValue("IsRedAlliance").isBoolean())
+            return table.getValue("IsRedAlliance").getBoolean();
+        return false;
+    }
+
+}
diff --git a/src/main/java/frc/robot/MBUtils.java b/src/main/java/frc/robot/MBUtils.java
@@ -6,6 +6,21 @@ public static double lerp(double value1, double value2, double t) { //linear int
         return (1 - t) * value1 + t * value2;
     }
 
+    public static double slerp(double a, double b, double t) { //spherical interpolation (yum!)
+        // Normalize angles
+        double delta = ((b - a) + Math.PI) % (2 * Math.PI) - Math.PI;
+
+        // Make sure we interpolate in the short direction
+        if (delta > Math.PI) {
+            delta -= 2 * Math.PI;
+        } else if (delta < -Math.PI) {
+            delta += 2 * Math.PI;
+        }
+
+        return a + delta * t;
+    }
+
+
     public static double clamp(double input, double absMax){
         absMax = Math.abs(absMax);
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -4,12 +4,17 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.commands.Autos;
+import frc.robot.commands.TravelToPose;
 import frc.robot.subsystems.EndEffectorSub;
 import frc.robot.subsystems.StateControllerSub;
 import frc.robot.subsystems.arm.Arm;
@@ -31,6 +36,7 @@ public class RobotContainer {
   VisionSubsystem visionSub = new VisionSubsystem();
   SwerveSubsystem swerveSubsystem = new SwerveSubsystem(visionSub);
   RunCommand drive = new RunCommand(()->swerveSubsystem.joystickDrive(driver.getLeftX(),driver.getLeftY(),driver.getRightX()),swerveSubsystem);
+  //RunCommand overcookedDrive = new RunCommand(()->swerveSubsystem.overcookedDrive(driver.getLeftX(),driver.getLeftY(),driver.getRightX(), driver.getRightY()),swerveSubsystem);
 
 
 
@@ -45,18 +51,22 @@ public class RobotContainer {
   Command setArmToHolding = stageOneToHolding.andThen(stageTwoToHolding);
 
 
-
+  SendableChooser<Command> autoChooser = new SendableChooser<>();
 
 
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
 
+
+
   //  driver.rightTrigger(0.5).onTrue(ArmCommands.placeCubeL2orL3(arm));
 //    driver.leftTrigger(0.5).onTrue(ArmCommands.retractCubeFromL2orL3(arm));
 
     swerveSubsystem.setDefaultCommand(drive);
 
+    driver.a().whileTrue(new RunCommand(swerveSubsystem::alignWithClosestNode,swerveSubsystem));
+    driver.b().whileTrue(new RunCommand(swerveSubsystem::autoBalanceForward,swerveSubsystem));
 
     driver.start().onTrue(new InstantCommand(swerveSubsystem::resetOdometry));
 
@@ -79,9 +89,15 @@ public RobotContainer() {
     operator.rightBumper().onTrue(new InstantCommand(stateController::setItemCube));
     operator.rightTrigger(0.5).onTrue(new InstantCommand(stateController::setItemCube));
 
+    driver.leftTrigger(0.5).onTrue(new InstantCommand(stateController::setArmModeToPostPlacing));
+  //  driver.b().onTrue(Autos.skibidiAutonomous(swerveSubsystem,arm,stateController,false));
+   // driver.b().onTrue(Autos.placeLeaveBalanceAuto(swerveSubsystem,stateController,false));
+
+ //   driver.y().onTrue(new TravelToPose(swerveSubsystem, new Pose2d(swerveSubsystem.getOdometry().getEstimatedPosition().getX(),swerveSubsystem.getOdometry().getEstimatedPosition().getY(), Rotation2d.fromDegrees(0)),1,1));
 
     // Configure the trigger bindings
     configureBindings();
+    createAutos();
   }
 
   /**
@@ -93,6 +109,19 @@ public RobotContainer() {
    * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
    * joysticks}.
    */
+
+  private void createAutos(){
+    SmartDashboard.putData(autoChooser);
+    autoChooser.addOption("b_placeBalance", Autos.placeLeaveBalanceAuto(swerveSubsystem,stateController,false));
+    autoChooser.addOption("r_placeBalance", Autos.placeLeaveBalanceAuto(swerveSubsystem,stateController,true));
+    autoChooser.addOption("b_twoCones", Autos.skibidiAutonomous(swerveSubsystem,stateController,false));
+    autoChooser.addOption("r_twoCones", Autos.skibidiAutonomous(swerveSubsystem,stateController,true));
+    autoChooser.setDefaultOption("no auto :'( ", null);
+
+
+
+  }
+
   private void configureBindings() {
    // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
   }
@@ -104,6 +133,6 @@ private void configureBindings() {
    */
   public Command getAutonomousCommand() {
     // An example command will be run in autonomous
-      return null;
+      return autoChooser.getSelected();
   }
 }
diff --git a/src/main/java/frc/robot/commands/ArmCommands.java b/src/main/java/frc/robot/commands/ArmCommands.java
@@ -1,5 +1,6 @@
 package frc.robot.commands;
 
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.commands.Arm.ArmFollowSplineTimed;
 import frc.robot.commands.Arm.ArmToPointLinearTimed;
@@ -11,7 +12,7 @@ public class ArmCommands {
 
     public static Command placeConeL3(Arm arm){
         double[] x = { 0.19, 0.64, 1.19}; // x values
-        double[] y = { 0.03, 0.91, 0.79 }; // y values
+        double[] y = { 0.03, 0.91, 0.79 + Units.inchesToMeters(2) }; // y values
         //   double[] x = { 0.19, 0.67, 1.19}; // x values
         //   double[] y = { 0.03, 0.75, 0.79 }; // y values
 
@@ -31,7 +32,7 @@ public static Command placeConeL3(Arm arm){
 
     public static Command placeConeL2(Arm arm){
         double[] x = { 0.19, 0.69, .86}; // x values
-        double[] y = { 0.03, 0.48, 0.48 }; // y values
+        double[] y = { 0.03, 0.48, 0.48 + Units.inchesToMeters(1.5)}; // y values
         //   double[] x = { 0.19, 0.67, 1.19}; // x values
         //   double[] y = { 0.03, 0.75, 0.79 }; // y values
 
@@ -77,7 +78,7 @@ public static Command retractFromConeL3(Arm arm){
   //      double[] y = { 0.03, 1, 0.79 }; // y values
 
         double[] x = { 0.19, 0.64, 1.19}; // x values
-        double[] y = { 0.03, 0.91, 0.79 }; // y values
+        double[] y = { 0.03, 0.91, 0.79 + Units.inchesToMeters(2) }; // y values
 
         SplineInterpolator interpolator = new SplineInterpolator();
         PolynomialSplineFunction spline = interpolator.interpolate(x, y);
@@ -114,7 +115,7 @@ public static Command intakeConeUpright(Arm arm){
     public static Command intakeConeFallen(Arm arm){
 
 
-        ArmToPointLinearTimed step2 = new ArmToPointLinearTimed(arm,0.804,-0.317,1); //0.505,-0.14
+        ArmToPointLinearTimed step2 = new ArmToPointLinearTimed(arm,0.804,-0.317 - Units.inchesToMeters(1.5),1); //0.505,-0.14
 
         return step2;
     }
@@ -128,8 +129,8 @@ public static Command retractFromConeFallen(Arm arm){
 
 
     public static Command placeCubeL2orL3(Arm arm){
-        double[] x = { 0.19, 0.604, 0.935}; // x values
-        double[] y = { 0.03, 0.421, 0.410 }; // y values
+        double[] x = { 0.19, 0.604, 0.935 + Units.inchesToMeters(2)}; // x values
+        double[] y = { 0.03, 0.421, 0.410 + Units.inchesToMeters(4)}; // y values
 
         SplineInterpolator interpolator = new SplineInterpolator();
         PolynomialSplineFunction spline = interpolator.interpolate(x, y);
@@ -145,8 +146,10 @@ public static Command placeCubeL1(Arm arm) {
     }
 
     public static Command retractCubeFromL2orL3(Arm arm){
-        double[] x = { 0.19, 0.604, 0.935}; // x values
-        double[] y = { 0.03, 0.421, 0.404 }; // y values
+       // double[] x = { 0.19, 0.604, 0.935}; // x values
+      //  double[] y = { 0.03, 0.421, 0.404 }; // y values
+        double[] x = { 0.19, 0.604, 0.935 + Units.inchesToMeters(2)}; // x values
+        double[] y = { 0.03, 0.421, 0.410 + Units.inchesToMeters(4)}; // y values
 
         SplineInterpolator interpolator = new SplineInterpolator();
         PolynomialSplineFunction spline = interpolator.interpolate(x, y);