Merge branch 'autoRevert' of https://github.com/Team4388/RiseOfRidgebotics2020 into autoRevert

src/main/java/frc4388/robot/RobotContainer.java

@@ -332,7 +332,7 @@ public class RobotContainer {
             "Bounce"
         };
 
-        m_bounce = new Bounce(m_robotDrive, buildPaths(bounce));
+        m_bounce = new Bounce(m_robotDrive, this, buildPaths(bounce));
         
         String[] barrelMany = new String[]{
             "BarrelManyWaypoints"
@@ -406,7 +406,7 @@ public class RobotContainer {
             //                                  new TankDriveVelocity(m_robotDrive, 1000, 1000, 1));
 
             //return m_barrel.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
-            return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
+            return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(1, 1));
             //return new SequentialCommandGroup(new TankDriveVelocity(m_robotDrive, 1000, 1000, 3), new TankDriveVelocity(m_robotDrive, 3000, 3000, 1));
             //return m_sequentialTest.andThen(() -> m_robotDrive.tankDriveVelocity(0,0));
 

src/main/java/frc4388/robot/commands/auto/Bounce.java

@@ -7,8 +7,12 @@
 
 package frc4388.robot.commands.auto;
 
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.RobotContainer;
 import frc4388.robot.subsystems.Drive;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
@@ -18,10 +22,14 @@ public class Bounce extends SequentialCommandGroup {
   /**
    * Creates a new Bounce.
    */
-  public Bounce(Drive drive, RamseteCommand[] paths) {
+  public Bounce(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
-    addCommands(paths[0]
+    addCommands(
+      paths[0],
+      new InstantCommand(() -> drive.switchReversed(true)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
+      paths[1]
     );
   }
 }

src/main/java/frc4388/robot/subsystems/Drive.java

@@ -46,6 +46,7 @@ public class Drive extends SubsystemBase {
   public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
   public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
   public static GyroBase m_pigeonGyro;
+  public boolean m_isReversed;
 
   /* Drive objects to manage Drive Train */
   public DifferentialDrive m_driveTrain;
@@ -332,9 +333,26 @@ public class Drive extends SubsystemBase {
     m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks);
     m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks);
 
-    m_odometry.update(Rotation2d.fromDegrees( getHeading()),
+    updateOdometry(m_isReversed);
+  }
+
+  public void updateOdometry(boolean reversed){
+    if (reversed){
+      m_odometry.update(Rotation2d.fromDegrees( getHeading() - 180),
+                                              -inchesToMeters(getDistanceInches(m_rightFrontMotor)),
+                                              inchesToMeters(getDistanceInches(m_leftFrontMotor)));
+    }
+    else
+    {
+      m_odometry.update(Rotation2d.fromDegrees( getHeading()),
                                               inchesToMeters(getDistanceInches(m_leftFrontMotor)),
                                               -inchesToMeters(getDistanceInches(m_rightFrontMotor)));
+    }
+  }
+
+  public void switchReversed(boolean reversed)
+  {
+    m_isReversed = reversed;
   }
 
   /**