day 1

MillenniumFalcons · Oct 1, 2023 · f433770 · f433770
1 parent 2d6bc57
commit f433770
Show file tree

Hide file tree

Showing 6 changed files with 40 additions and 10 deletions.
diff --git a/src/main/java/team3647/frc2023/auto/AutoCommands.java b/src/main/java/team3647/frc2023/auto/AutoCommands.java
@@ -29,6 +29,7 @@ public class AutoCommands {
     public final AutonomousMode blueConeCubeBalanceBumpSideMode;
     public final AutonomousMode blueConeScoreExitBalance;
     public final AutonomousMode blueJustScore;
+    public final AutonomousMode blueJustDrive;
 
     public final AutonomousMode redConeCubeCubeBumpSideNoBump;
     public final AutonomousMode redConeCubeCubeFlatSideMode;
@@ -37,6 +38,7 @@ public class AutoCommands {
     public final AutonomousMode redConeCubeBalanceBumpSideMode;
     public final AutonomousMode redConeScoreExitBalance;
     public final AutonomousMode redJustScore;
+    public final AutonomousMode redJustDrive;
 
     public AutoCommands(
             SwerveDrive drive,
@@ -75,6 +77,9 @@ public AutoCommands(
                 new AutonomousMode(
                         coneCubeCubeMidBalanceFlatSide(Alliance.Blue),
                         Trajectories.Blue.ConeCubeCubeMidBalanceFlatSide.kFirstPathInitial);
+        blueJustDrive =
+                new AutonomousMode(
+                        justDrive(Alliance.Blue), Trajectories.Blue.justDrive.kFirstPathInitial);
 
         // red side modes
         redConeCubeCubeBumpSideNoBump =
@@ -108,6 +113,10 @@ public AutoCommands(
                         flipForPP(
                                 Trajectories.Blue.ConeCubeCubeMidBalanceFlatSide
                                         .kFirstPathInitial));
+        redJustDrive =
+                new AutonomousMode(
+                        justDrive(Alliance.Red),
+                        flipForPP(Trajectories.Blue.justDrive.kFirstPathInitial));
     }
 
     public static Pose2d getJustScore(Pose2d pose) {
@@ -309,6 +318,14 @@ public Command drive() {
         return followTrajectory(Trajectories.Blue.Test.kTrajectory);
     }
 
+    public Command justDrive(Alliance color) {
+        Command drivetrainSequence =
+                followTrajectory(
+                        PathPlannerTrajectory.transformTrajectoryForAlliance(
+                                Trajectories.Blue.justDrive.kFirstTrajectory, color));
+        return drivetrainSequence;
+    }
+
     public Command coneCubeCubeBumpSideNoBump(Alliance color) {
         Command drivetrainSequence =
                 Commands.sequence(

diff --git a/src/main/java/team3647/frc2023/auto/Trajectories.java b/src/main/java/team3647/frc2023/auto/Trajectories.java
@@ -93,6 +93,19 @@ public static final class ConeCubeCubeFlatSide {
                                     fromPose(kFourthPathFinal, Rotation2d.fromDegrees(-60))));
         }
 
+        public static final class justDrive {
+            public static final Pose2d kFirstPathInitial =
+                    new Pose2d(1.80, 0.5, FieldConstants.kZero);
+            public static final Pose2d kFirstPathFinal = new Pose2d(5.5, 0.5, FieldConstants.kZero);
+            public static final PathPlannerTrajectory kFirstTrajectory =
+                    PathPlanner.generatePath(
+                            fastConstraints,
+                            List.of(
+                                    fromPose(kFirstPathInitial, FieldConstants.kZero),
+                                    // fromPose(kFirstPathWaypoint1, FieldConstants.kZero),
+                                    fromPose(kFirstPathFinal, FieldConstants.kZero)));
+        }
+
         public static final class coneCubeCubeBumpSideNoBump {
             public static final Pose2d kFirstPathInitial =
                     new Pose2d(1.80, 0.5, FieldConstants.kZero);
@@ -309,13 +322,13 @@ public static final class ConeCubeCubeMidBalanceFlatSide {
                             fastConstraints,
                             List.of(
                                     fromPose(kSecondPathInitial, FieldConstants.kOneEighty),
-                                    fromPose(kSecondPathFinal, Rotation2d.fromDegrees(-160))));
+                                    fromPose(kSecondPathFinal, Rotation2d.fromDegrees(-150))));
 
             public static final PathPlannerTrajectory kThirdTrajectory =
                     PathPlanner.generatePath(
                             defaultConstraints,
                             List.of(
-                                    fromPose(kThirdPathInitial, Rotation2d.fromDegrees(20)),
+                                    fromPose(kThirdPathInitial, Rotation2d.fromDegrees(30)),
                                     fromPose(kThirdPathWaypoint1, Rotation2d.fromDegrees(-45.00)),
                                     fromPose(kThirdPathFinal, Rotation2d.fromDegrees(-45.00))));
 
@@ -325,7 +338,7 @@ public static final class ConeCubeCubeMidBalanceFlatSide {
                             List.of(
                                     fromPose(kFourthPathInitial, Rotation2d.fromDegrees(135)),
                                     fromPose(kFourthPathWaypoint1, Rotation2d.fromDegrees(150)),
-                                    fromPose(kFourthPathFinal, Rotation2d.fromDegrees(-160))));
+                                    fromPose(kFourthPathFinal, Rotation2d.fromDegrees(-150))));
         }
 
         public static final class ConeExitBalance {

diff --git a/src/main/java/team3647/frc2023/commands/CubeShooterCommands.java b/src/main/java/team3647/frc2023/commands/CubeShooterCommands.java
@@ -34,7 +34,7 @@ public Command cubeShooterRun(double angle, double topDemand, double bottomDeman
     }
 
     public Command intake() {
-        return cubeShooterRun(91, -0.8, -0.8);
+        return cubeShooterRun(91, -0.6, -0.6);
     }
 
     public Command scoreHybrid() {

diff --git a/src/main/java/team3647/frc2023/constants/CubeShooterConstants.java b/src/main/java/team3647/frc2023/constants/CubeShooterConstants.java
@@ -44,10 +44,10 @@ public class CubeShooterConstants {
         kTopMotor.Inverted = InvertedValue.Clockwise_Positive;
         kBottomMotor.Inverted = InvertedValue.Clockwise_Positive;
 
-        kTopCurrent.StatorCurrentLimitEnable = true;
-        kBottomCurrent.StatorCurrentLimitEnable = true;
-        kTopCurrent.StatorCurrentLimit = kMaxCurrent;
-        kBottomCurrent.StatorCurrentLimit = kMaxCurrent;
+        kTopCurrent.StatorCurrentLimitEnable = false;
+        kBottomCurrent.StatorCurrentLimitEnable = false;
+        // kTopCurrent.StatorCurrentLimit = kMaxCurrent;
+        // kBottomCurrent.StatorCurrentLimit = kMaxCurrent;
 
         kTopRollerConfigurator.apply(kTopMotor);
         kTopRollerConfigurator.apply(kTopCurrent);

diff --git a/src/main/java/team3647/frc2023/robot/RobotContainer.java b/src/main/java/team3647/frc2023/robot/RobotContainer.java
@@ -78,7 +78,7 @@ public RobotContainer() {
         extender.setEncoder(ExtenderConstants.kMinimumPositionTicks);
         wrist.setEncoder(WristConstants.kInitialDegree);
         cubeWrist.setEncoder(CubeWristConstants.kInitialDegree);
-        runningMode = autoCommands.blueConeCubeCubeMidFlatSideMode;
+        runningMode = autoCommands.redConeCubeCubeBumpSideNoBump;
         LimelightHelpers.setPipelineIndex(LimelightConstant.kLimelightCenterHost, 1);
         swerve.setRobotPose(runningMode.getPathplannerPose2d());
     }

diff --git a/src/main/java/team3647/frc2023/util/SuperstructureState.java b/src/main/java/team3647/frc2023/util/SuperstructureState.java
@@ -124,7 +124,7 @@ public SuperstructureState addWristExtend(double degs, double extend) {
             new SuperstructureState(184, 57500, 43, "ground intake long");
 
     public static final SuperstructureState doubleStationCone =
-            new SuperstructureState(126, 21575, 102, "double station");
+            new SuperstructureState(128, 24375, 102, "double station");
     public static final SuperstructureState doubleStationCube =
             new SuperstructureState(120, 16000, 117, "double station");