Revert "cleaned up code, deleted comments (#13)"

This reverts commit 1130ee0.
MiamiBeachBots · Oct 19, 2023 · eab8179 · eab8179
1 parent 1130ee0
commit eab8179
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 4 deletions.
diff --git 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
@@ -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
@@ -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();
   }