Bounce testijg

PathWeaver/Paths/Bounce1

@@ -1,3 +1,3 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Name
-1.2190625523416865,-2.295277716936159,2.158615473813051,-0.2968869419570952,true,
+1.2190625523416865,-2.295277716936159,2.4431321265219355,-0.012370289248212707,true,
 2.295277716936159,-0.6685846807962377,-0.061851446241061314,0.5442927269213425,true,

PathWeaver/pathweaver.json

@@ -4,6 +4,6 @@
   "maxVelocity": 2.6,
   "maxAcceleration": 2.7,
   "wheelBase": 0.648,
-  "gameName": "Slalom Path",
+  "gameName": "Bounce Path",
   "outputDir": ".."
 }

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

@@ -353,7 +353,7 @@ public class RobotContainer {
             "DriveOffLineForward"
         };
 
-        m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths));
+        m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths));
 
         String[] driveOffLineBackwardPaths = new String[]{
             "DriveOffLineBackward"
@@ -399,13 +399,13 @@ public class RobotContainer {
             //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_sixBallAutoMiddle1.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
-            //return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+            return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
-            return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
+            //return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
-
+            //return m_bounce.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
         } catch (Exception e) {
             System.err.println("ERROR");
         }

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

@@ -7,8 +7,11 @@
 
 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.SequentialCommandGroup;
+import frc4388.robot.RobotContainer;
 import frc4388.robot.subsystems.Drive;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
@@ -18,11 +21,13 @@ public class DriveOffLineForward extends SequentialCommandGroup {
   /**
    * Creates a new DriveOffLineForward.
    */
-  public DriveOffLineForward(Drive drive, RamseteCommand[] paths) {
+  public DriveOffLineForward(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
 
     addCommands(
+      new InstantCommand(() -> drive.switchReversed(true)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
       paths[0]
     );
   }

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

@@ -338,7 +338,7 @@ public class Drive extends SubsystemBase {
 
   public void updateOdometry(boolean reversed){
     if (reversed){
-      m_odometry.update(Rotation2d.fromDegrees( getHeading() - 180),
+      m_odometry.update(Rotation2d.fromDegrees( getHeading()),
                                               -inchesToMeters(getDistanceInches(m_rightFrontMotor)),
                                               inchesToMeters(getDistanceInches(m_leftFrontMotor)));
     }