FIXED AUTO and anded figureEightpath

src/main/java/frc4388/robot/Robot.java

@@ -7,6 +7,7 @@
 
 package frc4388.robot;
 
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -71,7 +72,8 @@ public class Robot extends TimedRobot {
 
   @Override
   public void disabledPeriodic() {
-  }
+    m_robotContainer.resetOdometry(new Pose2d());
+    }
 
   /**
    * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
@@ -83,7 +85,6 @@ public class Robot extends TimedRobot {
 
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
     m_robotContainer.setDriveGearState(true);
-    m_robotContainer.resetOdometry(new Pose2d());
 
     //m_robotContainer.resetGyroYawRobotContainer(0);
 

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

@@ -37,6 +37,7 @@ import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.commands.auto.DriveOffLineBackward;
 import frc4388.robot.commands.auto.DriveOffLineForward;
 import frc4388.robot.commands.auto.EightBallAutoMiddle;
+import frc4388.robot.commands.auto.FigureEight;
 import frc4388.robot.commands.auto.FiveBallAutoMiddle;
 import frc4388.robot.commands.auto.SixBallAutoMiddle;
 import frc4388.robot.commands.auto.TenBallAutoMiddle;
@@ -109,6 +110,8 @@ public class RobotContainer {
 
     SixBallAutoMiddle m_sixBallAutoMiddle;
 
+    FigureEight m_figureEight;
+
     EightBallAutoMiddle m_eightBallAutoMiddle;
 
     DriveOffLineForward m_driveOffLineForward;
@@ -270,12 +273,19 @@ public class RobotContainer {
     }
 
     public void buildAutos() {
+        System.out.println("BUILDING AUTOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOS");
         String[] sixBallAutoMiddlePaths = new String[]{
             "SixBallMidComplete"
         };
 
         m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
 
+        String[] figureEight = new String[]{
+            "FigureEight"
+        };
+
+        m_figureEight = new FigureEight(m_robotDrive, buildPaths(figureEight));
+
         String[] eightBallAutoMiddlePaths = new String[]{
             "EightBallMidComplete"
         };
@@ -323,8 +333,9 @@ public class RobotContainer {
             SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);
 
             //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+            return m_figureEight.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));

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

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class FigureEight extends SequentialCommandGroup {
+  /**
+   * Creates a new FigureEight.
+   */
+  public FigureEight(Drive drive, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    addCommands(
+      paths[0]
+    );
+  }
+}

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

@@ -833,8 +833,7 @@ public class Drive extends SubsystemBase {
 
       SmartDashboard.putData("Pigeon Gyro", m_pigeonGyro);
       SmartDashboard.putData("Drive Train", m_driveTrain);
-
-      System.out.println("yooooooooooooooooooooooooooooooooooooo");
+      
       //SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get());
       //SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get());
       //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());