changed gyro offsets after auto

frc3175 · Apr 7, 2022 · 4f25ec7 · 4f25ec7
1 parent 8279204
commit 4f25ec7
Show file tree

Hide file tree

Showing 14 changed files with 29 additions and 15 deletions.
diff --git a/src/main/java/com/team3175/frc2022/robot/autos/automodes/FourBallADEBlue.java b/src/main/java/com/team3175/frc2022/robot/autos/automodes/FourBallADEBlue.java
@@ -113,7 +113,8 @@ public FourBallADEBlue(Shooter shooter, Feeder feeder, Intake intake, Actuators
                     new ParallelCommandGroup(m_intakeDECommand, m_intakeDeployDE),
                     new ParallelCommandGroup(m_shootDECommand, m_intakeRetractDE, new InstantCommand(() -> m_shooter.shoot(Constants.SHOOTER_TARGET_RPM))),
                     new StopSwerve(m_drivetrain),
-                    m_shootAndFeedDE
+                    m_shootAndFeedDE,
+                    new InstantCommand(() -> m_drivetrain.setGyro(339.44))
                     );
 
     }

diff --git a/src/main/java/com/team3175/frc2022/robot/autos/automodes/FourBallADERed.java b/src/main/java/com/team3175/frc2022/robot/autos/automodes/FourBallADERed.java
@@ -113,7 +113,8 @@ public FourBallADERed(Shooter shooter, Feeder feeder, Intake intake, Actuators a
                     new ParallelCommandGroup(m_intakeDECommand, m_intakeDeployDE),
                     new ParallelCommandGroup(m_shootDECommand, m_intakeRetractDE, new InstantCommand(() -> m_shooter.shoot(Constants.SHOOTER_TARGET_RPM))),
                     new StopSwerve(m_drivetrain),
-                    m_shootAndFeedDE
+                    m_shootAndFeedDE,
+                    new InstantCommand(() -> m_drivetrain.setGyro(339.44))
                     );
 
     }

diff --git a/src/main/java/com/team3175/frc2022/robot/autos/automodes/FourBallBCDBlue.java b/src/main/java/com/team3175/frc2022/robot/autos/automodes/FourBallBCDBlue.java
@@ -113,7 +113,8 @@ public FourBallBCDBlue(Shooter shooter, Feeder feeder, Intake intake, Actuators
                     new ParallelCommandGroup(m_intakeBDCommand, m_intakeDeployBD),
                     new ParallelCommandGroup(m_shootBDCommand, m_intakeRetractBD, new InstantCommand(() -> m_shooter.shoot(Constants.SHOOTER_TARGET_RPM))),
                     new StopSwerve(m_drivetrain),
-                    m_shootAndFeedBD
+                    m_shootAndFeedBD,
+                    new InstantCommand(() -> m_drivetrain.setGyro(67.38))
                     );
 
     }

diff --git a/src/main/java/com/team3175/frc2022/robot/autos/automodes/FourBallBCDRed.java b/src/main/java/com/team3175/frc2022/robot/autos/automodes/FourBallBCDRed.java
@@ -113,7 +113,8 @@ public FourBallBCDRed(Shooter shooter, Feeder feeder, Intake intake, Actuators a
                     new ParallelCommandGroup(m_intakeBDCommand, m_intakeDeployBD),
                     new ParallelCommandGroup(m_shootBDCommand, m_intakeRetractBD, new InstantCommand(() -> m_shooter.shoot(Constants.SHOOTER_TARGET_RPM))),
                     new StopSwerve(m_drivetrain),
-                    m_shootAndFeedBD
+                    m_shootAndFeedBD,
+                    new InstantCommand(() -> m_drivetrain.setGyro(67.38))
                     );
 
     }

diff --git a/src/main/java/com/team3175/frc2022/robot/autos/automodes/OneBall.java b/src/main/java/com/team3175/frc2022/robot/autos/automodes/OneBall.java
@@ -60,7 +60,8 @@ public OneBall(Shooter shooter, Feeder feeder, SwerveDrivetrain drivetrain) {
                     m_spinUp,
                     m_shootAndFeed,
                     m_driveBackCommand,
-                    new StopSwerve(m_drivetrain));
+                    new StopSwerve(m_drivetrain),
+                    new InstantCommand(() -> m_drivetrain.setGyro(339.44)));
 
     }
 

diff --git a/src/main/java/com/team3175/frc2022/robot/autos/automodes/ThreeBallBCBlue.java b/src/main/java/com/team3175/frc2022/robot/autos/automodes/ThreeBallBCBlue.java
@@ -113,7 +113,8 @@ public ThreeBallBCBlue(Shooter shooter, Feeder feeder, Intake intake, Actuators
                     new ParallelCommandGroup(m_trajectoryCommand3, m_intakeDeploy2),
                     new ParallelCommandGroup(m_trajectoryCommand4, m_intakeRetract2, new InstantCommand(() -> m_shooter.shoot(Constants.SHOOTER_TARGET_RPM))),
                     new StopSwerve(m_drivetrain),
-                    m_shootAndFeed2
+                    m_shootAndFeed2,
+                    new InstantCommand(() -> m_drivetrain.setGyro(68.71))
                     );
 
     }

diff --git a/src/main/java/com/team3175/frc2022/robot/autos/automodes/ThreeBallBCRed.java b/src/main/java/com/team3175/frc2022/robot/autos/automodes/ThreeBallBCRed.java
@@ -113,7 +113,8 @@ public ThreeBallBCRed(Shooter shooter, Feeder feeder, Intake intake, Actuators a
                     new ParallelCommandGroup(m_trajectoryCommand3, m_intakeDeploy2),
                     new ParallelCommandGroup(m_trajectoryCommand4, m_intakeRetract2, new InstantCommand(() -> m_shooter.shoot(Constants.SHOOTER_TARGET_RPM))),
                     new StopSwerve(m_drivetrain),
-                    m_shootAndFeed2
+                    m_shootAndFeed2,
+                    new InstantCommand(() -> m_drivetrain.setGyro(68.71))
                     );
 
     }

diff --git a/src/main/java/com/team3175/frc2022/robot/autos/automodes/ThreeBallDEBlue.java b/src/main/java/com/team3175/frc2022/robot/autos/automodes/ThreeBallDEBlue.java
@@ -88,7 +88,8 @@ public ThreeBallDEBlue(Shooter shooter, Feeder feeder, Intake intake, Actuators
                     new WaitCommand(1.5),
                     new ParallelCommandGroup(m_trajectoryCommand2, m_intakeRetract, new InstantCommand(() -> m_shooter.shoot(Constants.SHOOTER_TARGET_RPM))),
                     new StopSwerve(m_drivetrain),
-                    m_shootAndFeed2
+                    m_shootAndFeed2,
+                    new InstantCommand(() -> m_drivetrain.setGyro(339.44))
                     );
 
     }

diff --git a/src/main/java/com/team3175/frc2022/robot/autos/automodes/ThreeBallDERed.java b/src/main/java/com/team3175/frc2022/robot/autos/automodes/ThreeBallDERed.java
@@ -85,10 +85,11 @@ public ThreeBallDERed(Shooter shooter, Feeder feeder, Intake intake, Actuators a
                     m_spinUp1,
                     m_shootAndFeed1,
                     new ParallelCommandGroup(m_trajectoryCommand, m_intakeDeploy),
-                    new WaitCommand(0.25),
+                    new WaitCommand(1.5),
                     new ParallelCommandGroup(m_trajectoryCommand2, m_intakeRetract, new InstantCommand(() -> m_shooter.shoot(Constants.SHOOTER_TARGET_RPM))),
                     new StopSwerve(m_drivetrain),
-                    m_shootAndFeed2
+                    m_shootAndFeed2,
+                    new InstantCommand(() -> m_drivetrain.setGyro(339.44))
                     );
 
     }

diff --git a/src/main/java/com/team3175/frc2022/robot/autos/automodes/TwoBallA.java b/src/main/java/com/team3175/frc2022/robot/autos/automodes/TwoBallA.java
@@ -88,7 +88,8 @@ public TwoBallA(Shooter shooter, Feeder feeder, Intake intake, Actuators actuato
                     new WaitCommand(0.25),
                     new ParallelCommandGroup(m_trajectoryCommand2, m_intakeRetract, new InstantCommand(() -> m_shooter.shoot(Constants.SHOOTER_TARGET_RPM))),
                     new StopSwerve(m_drivetrain),
-                    m_shootAndFeed2
+                    m_shootAndFeed2,
+                    new InstantCommand(() -> m_drivetrain.setGyro(339.44))
                     );
 
     }

diff --git a/src/main/java/com/team3175/frc2022/robot/autos/automodes/TwoBallADefenseBlue.java b/src/main/java/com/team3175/frc2022/robot/autos/automodes/TwoBallADefenseBlue.java
@@ -117,7 +117,8 @@ public TwoBallADefenseBlue(Shooter shooter, Feeder feeder, Intake intake, Actuat
                     new ParallelCommandGroup(m_intakeXCommand, m_intakeDeployX),
                     new ParallelCommandGroup(m_shootXCommand, m_intakeRetractX, m_spinUpX),
                     new StopSwerve(m_drivetrain),
-                    m_shootAndFeedX
+                    m_shootAndFeedX,
+                    new InstantCommand(() -> m_drivetrain.setGyro(180.00))
                     );
 
     }

diff --git a/src/main/java/com/team3175/frc2022/robot/autos/automodes/TwoBallADefenseRed.java b/src/main/java/com/team3175/frc2022/robot/autos/automodes/TwoBallADefenseRed.java
@@ -117,7 +117,8 @@ public TwoBallADefenseRed(Shooter shooter, Feeder feeder, Intake intake, Actuato
                     new ParallelCommandGroup(m_intakeXCommand, m_intakeDeployX),
                     new ParallelCommandGroup(m_shootXCommand, m_intakeRetractX, m_spinUpX),
                     new StopSwerve(m_drivetrain),
-                    m_shootAndFeedX
+                    m_shootAndFeedX,
+                    new InstantCommand(() -> m_drivetrain.setGyro(180.00))
                     );
 
     }

diff --git a/src/main/java/com/team3175/frc2022/robot/autos/automodes/TwoBallB.java b/src/main/java/com/team3175/frc2022/robot/autos/automodes/TwoBallB.java
@@ -86,7 +86,8 @@ public TwoBallB(Shooter shooter, Feeder feeder, Intake intake, Actuators actuato
                     new ParallelCommandGroup(m_trajectoryCommand, m_intakeDeploy),
                     new ParallelCommandGroup(m_trajectoryCommand2, m_intakeRetract, new InstantCommand(() -> m_shooter.shoot(Constants.SHOOTER_TARGET_RPM))),
                     new StopSwerve(m_drivetrain),
-                    m_shootAndFeed2
+                    m_shootAndFeed2,
+                    new InstantCommand(() -> m_drivetrain.setGyro(68.71))
                     );
 
     }

diff --git a/src/main/java/com/team3175/frc2022/robot/autos/automodes/TwoBallC.java b/src/main/java/com/team3175/frc2022/robot/autos/automodes/TwoBallC.java
@@ -86,7 +86,8 @@ public TwoBallC(Shooter shooter, Feeder feeder, Intake intake, Actuators actuato
                     new ParallelCommandGroup(m_trajectoryCommand, m_intakeDeploy),
                     new ParallelCommandGroup(m_trajectoryCommand2, m_intakeRetract, new InstantCommand(() -> m_shooter.shoot(Constants.SHOOTER_TARGET_RPM))),
                     new StopSwerve(m_drivetrain),
-                    m_shootAndFeed2
+                    m_shootAndFeed2,
+                    new InstantCommand(() -> m_drivetrain.setGyro(68.71))
                     );
 
     }