From 699dee33df2651b54c6100e99c3cc404de290c71 Mon Sep 17 00:00:00 2001
From: Kyra <101209@psdschools.org>
Date: Wed, 24 Mar 2021 17:33:16 -0600
Subject: [PATCH] Added Syntax Revered Path

Added reveresed syntax for the Bounce Path.
Need to test and build paths.
---
 .../java/frc4388/robot/RobotContainer.java    |  4 ++--
 .../frc4388/robot/commands/auto/Bounce.java   | 12 +++++++++--
 .../java/frc4388/robot/subsystems/Drive.java  | 20 ++++++++++++++++++-
 3 files changed, 31 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 2ab358d..d98ffeb 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/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));
 
diff --git a/src/main/java/frc4388/robot/commands/auto/Bounce.java b/src/main/java/frc4388/robot/commands/auto/Bounce.java
index 905e3fc..0b80db1 100644
--- a/src/main/java/frc4388/robot/commands/auto/Bounce.java
+++ b/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]
     );
   }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index fe96ca2..ba64beb 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/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;
   }
 
   /**