From e8ad3b8bd4d4ad3a87bd347a750fc8747e58eb80 Mon Sep 17 00:00:00 2001
From: Joseph Kantrowitz <josephkantrowitz18@gmail.com>
Date: Sun, 26 Jan 2025 11:24:14 -0500
Subject: [PATCH] Addes basic testbed subsystem and command

---
 src/main/java/frc/robot/Robot.java            |  1 +
 .../java/frc/robot/commands/SpinMotor.java    | 41 +++++++++++++++++++
 .../basicTestbed/TestbedSubsystem.java        | 22 ++++++++++
 3 files changed, 64 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/SpinMotor.java
 create mode 100644 src/main/java/frc/robot/subsystems/basicTestbed/TestbedSubsystem.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index be21731..51c31f3 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -51,6 +51,7 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+    
   }
 
   @Override
diff --git a/src/main/java/frc/robot/commands/SpinMotor.java b/src/main/java/frc/robot/commands/SpinMotor.java
new file mode 100644
index 0000000..b29923c
--- /dev/null
+++ b/src/main/java/frc/robot/commands/SpinMotor.java
@@ -0,0 +1,41 @@
+// 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.commands;
+
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.subsystems.basicTestbed.TestbedSubsystem;
+
+public class SpinMotor {
+  private TestbedSubsystem neoMotor;
+
+  private double startTime;
+  private double speedMotors;
+
+  public SpinMotor(TestbedSubsystem neoMotor, double speedMotors) {
+
+    this.speedMotors = speedMotors;
+    this.neoMotor = neoMotor;
+  }
+
+  // Called when the command is initially scheduled.
+  public void initialize() {
+    neoMotor.setSpeed(speedMotors);
+    startTime = Timer.getFPGATimestamp();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  public void end(boolean interrupted) {
+    neoMotor.stop();
+  }
+
+  // Returns true when the command should end.
+  public boolean isFinished() {
+
+    return (Timer.getFPGATimestamp() - startTime >= 5);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/basicTestbed/TestbedSubsystem.java b/src/main/java/frc/robot/subsystems/basicTestbed/TestbedSubsystem.java
new file mode 100644
index 0000000..cd2be1b
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/basicTestbed/TestbedSubsystem.java
@@ -0,0 +1,22 @@
+package frc.robot.subsystems.basicTestbed;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import com.revrobotics.spark.SparkLowLevel;
+import com.revrobotics.spark.SparkMax;
+
+
+public class TestbedSubsystem extends SubsystemBase {
+  private final SparkMax myMotor;
+
+  public TestbedSubsystem() {
+    this.myMotor =
+        new SparkMax(3, SparkLowLevel.MotorType.kBrushless);
+  }
+
+  public void setSpeed(double speed) {
+    myMotor.set(speed);
+  }
+
+  public void stop() {
+    myMotor.set(0);
+  }
+}