From cbc0c9443c8cdd5fe53e9a602741d68b6db1c15e Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Fri, 5 Feb 2021 13:54:16 -0700
Subject: [PATCH] Added Run Intake to end of Drive Forward Auto

---
 PathWeaver/Paths/FigureEight3                         |  3 +++
 PathWeaver/pathweaver.json                            |  1 -
 src/main/java/frc4388/robot/RobotContainer.java       | 11 ++++++-----
 .../robot/commands/auto/DriveOffLineForward.java      |  6 +++++-
 4 files changed, 14 insertions(+), 7 deletions(-)
 create mode 100644 PathWeaver/Paths/FigureEight3

diff --git a/PathWeaver/Paths/FigureEight3 b/PathWeaver/Paths/FigureEight3
new file mode 100644
index 0000000..610bd7c
--- /dev/null
+++ b/PathWeaver/Paths/FigureEight3
@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+0.0,0.0,10.0,0.0,true,
+10.0,-10.0,0.0,-10.0,true,
diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json
index ddd52e0..89479dc 100644
--- a/PathWeaver/pathweaver.json
+++ b/PathWeaver/pathweaver.json
@@ -1,6 +1,5 @@
 {
   "lengthUnit": "Meter",
-  "exportUnit": "Always Meters",
   "maxVelocity": 1.7,
   "maxAcceleration": 2.7,
   "wheelBase": 0.648,
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 07fa991..a67dc99 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -33,6 +33,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.commands.auto.DriveOffLineBackward;
 import frc4388.robot.commands.auto.DriveOffLineForward;
@@ -161,8 +162,8 @@ public class RobotContainer {
         // continually sends updates to the Blinkin LED controller to keep the lights on
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
         // runs the storage not
-        //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
-        m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE));
+        m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
+        //m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE));
         //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
     }
 
@@ -230,7 +231,7 @@ public class RobotContainer {
             .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5)))
             .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
 
-        // safety for climber and leveler
+         // safety for climber and leveler
         new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
             .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber))
             .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber));
@@ -332,8 +333,8 @@ public class RobotContainer {
         try {
             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_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_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
diff --git a/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java b/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java
index a5fb618..47ebfe6 100644
--- a/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java
+++ b/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java
@@ -9,7 +9,9 @@ package frc4388.robot.commands.auto;
 
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.commands.intake.RunIntake;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Intake;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
@@ -21,9 +23,11 @@ public class DriveOffLineForward extends SequentialCommandGroup {
   public DriveOffLineForward(Drive drive, RamseteCommand[] paths) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
+    Intake m_intake = new Intake();
 
     addCommands(
-      paths[0]
+      paths[0],
+      new RunIntake(m_intake)
     );
   }
 }