From eab8179f3d64533258182ce38512d7a9ecd8d95a Mon Sep 17 00:00:00 2001
From: Jack Nelson <jack@jacknelson.xyz>
Date: Thu, 19 Oct 2023 16:11:15 -0400
Subject: [PATCH] Revert "cleaned up code, deleted comments (#13)"

This reverts commit 1130ee0a14e064e69c7a254327d3fcaed2f44f19.
---
 src/main/java/frc/robot/Robot.java               | 16 ++++++++++++++--
 src/main/java/frc/robot/RobotContainer.java      |  7 ++++++-
 .../robot/subsystems/UltrasonicSubsystem.java    |  2 +-
 3 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index a7e23ad..bdd6c6c 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -107,7 +107,10 @@ public void autonomousInit() {
 
   /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {}
+  public void autonomousPeriodic() {
+    // DO ultrasonid
+    // SmartDashboard.putNumber("Auto Sensor", getRangeInches(ultrasonic1));
+  }
 
   @Override
   public void teleopInit() {
@@ -136,7 +139,16 @@ public void teleopInit() {
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+    // System.out.println(
+    // "Ultrasonic Distance In Inches: " + m_robotContainer.getUltrasonic1().DistanceIn());
+    // TODO: add dashboard stuff
+    // SmartDashboard.putNumber("Ultrasonic Sensor Distance", getRangeInches(ultrasonic1));
+    // SmartDashboard.putNumber("Ultrasonic Top Sensor Distance", getRangeInches(ultrasonic2));
+    // SmartDashboard.putNumber("Throttle", m_robotContainer.getStick().getThrottle());
+    // SmartDashboard.putNumber("Gyro Rate", m_robotContainer.getGyro().getRate());
+    // SmartDashboard.putNumber("Gyro angle", m_robotContainer.getGyro().getAngle());
+  }
 
   @Override
   public void testInit() {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index dc2a4f6..5e3b70e 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,7 +4,6 @@
 
 package frc.robot;
 
-import com.pathplanner.lib.PathPlanner;
 import com.pathplanner.lib.PathPlannerTrajectory;
 import com.pathplanner.lib.auto.PIDConstants;
 import com.pathplanner.lib.auto.RamseteAutoBuilder;
@@ -14,6 +13,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -198,10 +198,15 @@ public Joystick getFlightStick() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
+    /*
     pathGroup =
         PathPlanner.loadPathGroup(
             autoDashboardChooser.getSelected(), DriveConstants.autoPathConstraints);
     // Generate the auto command from the auto builder using the routine selected in the dashboard.
     return autoBuilder.fullAuto(pathGroup);
+    */
+    // drive forward for 3 seconds.
+    return new SequentialCommandGroup(
+        new DefaultDrive(m_driveSubsystem, () -> 0.33, () -> 0.33).withTimeout(3));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java b/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java
index e27792c..42b028f 100644
--- a/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java
@@ -31,7 +31,7 @@ public double DistanceCM() {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    // SmartDashboard.putNumber("Ultrasonic Sensor Distance", getRangeInches(ultrasonic1));
+
     // Calculate what percentage of 5 Volts we are actually at
     voltageScaleFactor = 5 / RobotController.getVoltage5V();
   }