Refactored Autonomous Drive Code

CompetitionSoftware/src/main/java/frc/robot/Constants.java

@@ -24,7 +24,7 @@ public static class RobotConstants {
   }
 
   public static class AutoConstants {
-    public static final long kPassLineDuration_mS = 2000;
+    public static final int kPassLineDuration_mS = 2000;
     public static final double kPassLineSpeed = 0.40;
   }
 

CompetitionSoftware/src/main/java/frc/robot/RobotContainer.java

@@ -4,15 +4,14 @@
 
 package frc.robot;
 import frc.robot.Constants.*;
-import frc.robot.commands.AutonomousDrive2Sec;
-import frc.robot.commands.AutonomousDrive4Sec;
-import frc.robot.commands.AutonomousDrive6Sec;
-import frc.robot.commands.AutonomousPassLine;
+import frc.robot.commands.AutonomousDrive;
+import frc.robot.commands.AutonomousDriveTimed;
 import frc.robot.commands.ClimbCommand;
 import frc.robot.commands.NoteHandlingSpeedCommand;
 import frc.robot.commands.RotateCommand;
 import frc.robot.commands.SetGearCommand;
 import frc.robot.commands.SetModeCommand;
+import frc.robot.commands.TimedCommand;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.GooseRotationSubsystem;
 import frc.robot.subsystems.NoteHandlingSubsystem;
@@ -122,10 +121,14 @@ public void reportStatus() {
   private void configureDriverStationControls()
   {
     // Drop-down chooser for auto program.
-    m_autoChooser.setDefaultOption("Pass Auto Line", new AutonomousPassLine(m_driveSubsystem));
-    m_autoChooser.addOption("Drive forward 2 Seconds", new AutonomousDrive2Sec(m_driveSubsystem));
-    m_autoChooser.addOption("Drive forward 4 Seconds", new AutonomousDrive4Sec(m_driveSubsystem));
-    m_autoChooser.addOption("Drive forward 6 Seconds", new AutonomousDrive6Sec(m_driveSubsystem));
+    m_autoChooser.setDefaultOption("Pass Auto Line", new AutonomousDriveTimed(m_driveSubsystem, AutoConstants.kPassLineDuration_mS));
+    m_autoChooser.addOption("Drive forward 2 Seconds", new AutonomousDriveTimed(m_driveSubsystem, 2000));
+    m_autoChooser.addOption("Drive forward 4 Seconds", new AutonomousDriveTimed(m_driveSubsystem, 4000));
+    m_autoChooser.addOption("Drive forward 6 Seconds", new AutonomousDriveTimed(m_driveSubsystem, 6000));
+
+    AutonomousDrive autoDrive = new AutonomousDrive(m_driveSubsystem);
+    m_autoChooser.addOption("Drive Forward 2 Seconds !! NOT TESTED !!", new TimedCommand(autoDrive, 2000));
+
     SmartDashboard.putData(m_autoChooser);
   }
 

CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive2Sec.java → CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive.java

@@ -7,18 +7,17 @@
 import frc.robot.subsystems.DriveSubsystem;
 
 /** An example command that uses an example subsystem. */
-public class AutonomousDrive2Sec extends Command {
+public class AutonomousDrive extends Command {
   private final DriveSubsystem m_subsystem;
   private double m_leftSpeed = 0.0;
   private double m_rightSpeed = 0.0;
-  private long m_startTime;
 
   /**
    * Creates a new AutonomousDrive2Sec.
    *
    * @param subsystem The subsystem used by this command.
    */
-  public AutonomousDrive2Sec(DriveSubsystem subsystem) 
+  public AutonomousDrive(DriveSubsystem subsystem) 
   {
     m_subsystem = subsystem;
 
@@ -29,7 +28,6 @@ public AutonomousDrive2Sec(DriveSubsystem subsystem)
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_startTime = System.currentTimeMillis();
     m_subsystem.setTankSpeeds(0.0, 0.0);
 
     m_leftSpeed = AutoConstants.kPassLineSpeed;
@@ -51,6 +49,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return (System.currentTimeMillis() - m_startTime) >= 2000;
+    return false; // <-- VERY SKETCHY !!
   }
 }

CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDrive4Sec.java → CompetitionSoftware/src/main/java/frc/robot/commands/AutonomousDriveTimed.java

@@ -3,25 +3,33 @@
 package frc.robot.commands;
 
 import frc.robot.Constants.*;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.DriveSubsystem;
 
 /** An example command that uses an example subsystem. */
-public class AutonomousDrive4Sec extends Command {
+public class AutonomousDriveTimed extends Command {
   private final DriveSubsystem m_subsystem;
   private double m_leftSpeed = 0.0;
   private double m_rightSpeed = 0.0;
   private long m_startTime;
+  private int m_executeTime;
 
   /**
-   * Creates a new AutonomousDrive4Sec.
+   * Creates a new AutonomousDrive2Sec.
    *
    * @param subsystem The subsystem used by this command.
+   * @param executeTime wants time in positive miliseconds
    */
-  public AutonomousDrive4Sec(DriveSubsystem subsystem) 
+  public AutonomousDriveTimed(DriveSubsystem subsystem, int executeTime) 
   {
     m_subsystem = subsystem;
 
+    if (executeTime < 0) {
+      DriverStation.reportError("executeTime in AutonomusDrive Constructor is negative", null);
+    } 
+    m_executeTime = executeTime;
+
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(m_subsystem);
   }
@@ -51,6 +59,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return (System.currentTimeMillis() - m_startTime) >= 4000;
+    return (System.currentTimeMillis() - m_startTime) >= m_executeTime;
   }
 }

CompetitionSoftware/src/main/java/frc/robot/commands/TimedCommand.java

@@ -0,0 +1,62 @@
+// This should end up in common when that is set up - Owen G.
+
+package frc.robot.commands;
+import java.util.Set;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+
+/** An example command that uses an example subsystem. */
+public class TimedCommand extends Command {
+  private final Command m_command;
+  private long m_startTime;
+  private int m_executeTime;
+
+  /**
+   * Runs a command for a timed ammount.
+   *
+   * @param command The command being executed.
+   * @param executeTime executeTime wants the run time in positive miliseconds.
+   */
+  public TimedCommand(Command command, int executeTime) 
+  {
+    m_command = command;
+
+    if (executeTime < 0) {
+      DriverStation.reportError("executeTime in TimedCommand Constructor is negative", null);
+    } 
+    m_executeTime = executeTime;
+
+    // Gets the required subsystem dependencies and then adds them as a requirement.
+    Set<Subsystem> requirements = m_command.getRequirements();
+    for (Subsystem requirement : requirements) {
+      addRequirements(requirement);
+    }
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_startTime = System.currentTimeMillis();
+    m_command.initialize();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_command.execute();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_command.end(interrupted);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    boolean timedOut = (System.currentTimeMillis() - m_startTime) >= m_executeTime;
+    return timedOut || m_command.isFinished();
+  }
+}

Prototype_Projects/BasicDriveSingleTalon/networktables.json

@@ -0,0 +1 @@
+[]

Prototype_Projects/BasicDriveSingleTalon/src/main/java/frc/robot/Constants.java

@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+  public static class RobotConstants {
+    public static final int periodicTicksPerSecond = 50;
+    public static final int intakeReportingFreq = 1;
+  }
+
+  public static class DrivetrainConstants {
+    public static final int kLeftMotorCANID  = 10;
+    public static final int kRightMotorCANID = 20;
+  }
+
+  public static class DriverConstants {
+    public static final int kDriveJoystick = 0;
+    public static final double kTurnDivider = 2.0;
+
+    // Buttons
+    public static final int kIntake = 1; // Joystick Trigger
+    public static final double kThrottleMultiplier = -1.0;
+  }
+
+  public static class OperatorConstants {
+    public static final int kOperatorController = 1;
+
+    public static final int kRunForwardButton = 4; // Button Y
+    public static final int kRunBackwardButton = 3;  // Button X
+  }
+
+  public static class WheeledIntakeConstants {
+    public static final int kLeftMotorCANID = 30;
+    public static final int kRightMotorCANID = 40;
+
+    public static final int kLeftMotorCurrentLimit = 3;
+    public static final int kRightMotorCurrentLimit = 3;
+
+    public static final double kRollerSpeed = -0.6; // "random" value
+  }
+
+  public static class ClawdiaConstants {
+    public static final int kMotorCANID = 50;
+
+    public static final int kMotorCurrentLimit = 3;
+
+    public static final double kClawSpeed = 0.1; // low value for testing
+  }
+}