Merge pull request #45 from zaneal/main

all code after 1619
fairviewrobotics · Mar 3, 2024 · 7d64eb4 · 7d64eb4
2 parents 17b84cc + bd30be8
commit 7d64eb4
Show file tree

Hide file tree

Showing 6 changed files with 82 additions and 37 deletions.
diff --git a/src/main/deploy/pathplanner/autos/Basic Auto.auto b/src/main/deploy/pathplanner/autos/Basic Auto.auto
@@ -1,6 +1,12 @@
 {
   "version": 1.0,
-  "startingPose": null,
+  "startingPose": {
+    "position": {
+      "x": 1.08,
+      "y": 1.74
+    },
+    "rotation": 0
+  },
   "command": {
     "type": "sequential",
     "data": {
@@ -23,6 +29,25 @@
               }
             ]
           }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeCommand"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoSpinForShoot"
+                }
+              }
+            ]
+          }
         }
       ]
     }

diff --git a/src/main/deploy/pathplanner/paths/Basic Path.path b/src/main/deploy/pathplanner/paths/Basic Path.path
@@ -3,41 +3,41 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 15.102013541990049,
-        "y": 1.7716141497097815
+        "x": 1.2019121769725023,
+        "y": 1.7362804447831126
       },
       "prevControl": null,
       "nextControl": {
-        "x": 14.77219270642245,
-        "y": 1.7285604756394708
+        "x": 1.4338675575499302,
+        "y": 1.6908513955916802
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 13.913910290783544,
-        "y": 1.7716141497097815
+        "x": 1.8113146642959954,
+        "y": 1.6872289485319718
       },
       "prevControl": {
-        "x": 13.96866020776865,
-        "y": 1.4903401638966032
+        "x": 1.525009348040061,
+        "y": 1.6991400521204603
       },
       "nextControl": {
-        "x": 13.84823116868456,
-        "y": 2.109036185664966
+        "x": 2.08252311874994,
+        "y": 1.675945916706649
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 14.05672971994092,
-        "y": 2.436125339843273
+        "x": 2.4986888399702725,
+        "y": 1.4527200396993387
       },
       "prevControl": {
-        "x": 14.096873559487857,
-        "y": 2.2242418790393135
+        "x": 2.271484209635086,
+        "y": 1.329438070913179
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -48,32 +48,40 @@ public class RobotContainer {
   public ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
   public IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
 
-  private boolean isRed = DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red);
+//  private boolean isRed = DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red);
 
 
 
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
-    configureButtonBindings();
+    //configureButtonBindings();
 
-    superSecretMissileTech = AutoBuilder.buildAutoChooser();
+//    indexerSubsystem = new IndexerSubsystem();
+//    shooterSubsystem = new ShooterSubsystem();
+//    intakeSubsystem = new IntakeSubsystem();
+
+//FOR ALL: tune timeouts
+
+    NamedCommands.registerCommand("AutoSpinUp", new SpinUpCommand(shooterSubsystem, false).withTimeout(3.0));
+    NamedCommands.registerCommand("AutoSpinForShoot", new SpinUpCommand(shooterSubsystem, false).withTimeout(1.5));
+
+    NamedCommands.registerCommand("IntakeCommand", new IntakeCommand(intakeSubsystem, indexerSubsystem, IntakeCommand.Targets.SPEAKER, false).withTimeout(1.5));
 
+    NamedCommands.registerCommand("AutoShoot", new AutoShoot(indexerSubsystem).withTimeout(1.5));
+
+    superSecretMissileTech = AutoBuilder.buildAutoChooser();
     SmartDashboard.putData("Auto Chooser", superSecretMissileTech);
 
-    NamedCommands.registerCommand("AutoSpinUp", new SpinUpCommand(shooterSubsystem, false));
-    NamedCommands.registerCommand("BaseCommand", new BaseCommand(indexerSubsystem));
-    NamedCommands.registerCommand("IntakeCommand", new IntakeCommand(intakeSubsystem, indexerSubsystem, IntakeCommand.Targets.SPEAKER, false));
-    NamedCommands.registerCommand("AutoRotateAndShoot", new AutoRotateToSpeakerAndShoot(swerveSubsystem, indexerSubsystem));
-    NamedCommands.registerCommand("AutoShoot", new AutoShoot(indexerSubsystem));
+    configureButtonBindings();
   }
 
   public void configureButtonBindings() {
 
     //DEFAULT COMMANDS
 
-    //indexerSubsystem.setDefaultCommand(new BaseCommand(indexerSubsystem));
+
 
     // PRIMARY CONTROLLER
 

diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java
@@ -20,7 +20,7 @@ public class ShooterConstants {
 
     public static final PIDController shooterPID = new PIDController(ShooterConstants.shooterP,ShooterConstants.shooterI, ShooterConstants.shooterD);
     public static boolean isActive = false;
-    public static final double shooterRPM = 6000;
+    public static final double shooterRPM = 1000;
     public static final double shooterWheelDiameterInches = 4.0;
     public static final double shooterWheelDiameterMeters = shooterWheelDiameterInches/39.37;
     public static final double shooterAngle = Math.toRadians(38);

diff --git a/src/main/java/frc/robot/controllers/SwerveModuleControlller.java b/src/main/java/frc/robot/controllers/SwerveModuleControlller.java
@@ -25,7 +25,7 @@ public class SwerveModuleControlller {
     public SwerveModuleControlller(int drivingPort, int turningPort, double chassisAngularOffset) {
 
         this.chassisAngularOffset = chassisAngularOffset;
-        drivingMotor = new CANSparkFlex(drivingPort, CANSparkLowLevel.MotorType.kBrushless);
+        drivingMotor = new CANSparkFlex(drivingPort, CANSparkFlex.MotorType.kBrushless);
         turningMotor = new CANSparkMax(turningPort, CANSparkLowLevel.MotorType.kBrushless);
 //        drivingEncoder = drivingMotor.getEncoder();
         drivingEncoder = drivingMotor.getEncoder();

diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
@@ -146,21 +146,25 @@ public class SwerveSubsystem extends SubsystemBase {
     public SwerveSubsystem() {
         // PathPlanner stuff
         AutoBuilder.configureHolonomic(
-                this::getPathplannerPose,
+                this::getPose,
                 this::resetOdometry,
                 this::getRobotRelativeSpeeds,
                 this::driveRobotRelative,
                 new HolonomicPathFollowerConfig(
                         new PIDConstants(1.0, 0.0, 0.0),
                         new PIDConstants(1.0, 0.0, 0.0),
-                        1.5,
-                         Units.inchesToMeters(11.875),
-                        new ReplanningConfig(true, false)
+                        1.5, //swervesubsystem.setmodulestate
+                        0.301625,//11.875 meters
+                        new ReplanningConfig()
                 ),
-//                () -> DriverStation.getAlliance().filter(value -> value == DriverStation.Alliance.Red).isPresent(),
-                () -> true,
+                () -> {
+                    var alliance = DriverStation.getAlliance();
+                    if (alliance.isPresent()) {
+                        return alliance.get() == DriverStation.Alliance.Red;
+                    }
+                    return false;
+                },
                 this
-
         );
 
 
@@ -295,11 +299,19 @@ public SwerveDrivePoseEstimator getPoseEstimator() {
      * @param chassisSpeeds {@link ChassisSpeeds} object
      */
     public void driveRobotRelative(ChassisSpeeds chassisSpeeds) {
-        double forward = -chassisSpeeds.vxMetersPerSecond;
-        double sideways = -chassisSpeeds.vyMetersPerSecond;
-        double rotation = chassisSpeeds.omegaRadiansPerSecond;
+        if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) {
+            double forward = -chassisSpeeds.vxMetersPerSecond;
+            double sideways = -chassisSpeeds.vyMetersPerSecond;
+            double rotation = chassisSpeeds.omegaRadiansPerSecond;
 
-        drive(-forward, -sideways, rotation, false, true);
+            drive(-forward, -sideways, rotation, false, true);
+        } else {
+            double forward = -chassisSpeeds.vxMetersPerSecond;
+            double sideways = -chassisSpeeds.vyMetersPerSecond;
+            double rotation = chassisSpeeds.omegaRadiansPerSecond;
+
+            drive(-forward, -sideways, rotation, false, true);
+        }
     }
 
     /**