Fixed reading + adding test motor

src/main/java/frc4388/robot/Main.java

@@ -5,6 +5,7 @@
 package frc4388.robot;
 
 import edu.wpi.first.wpilibj.RobotBase;
+import frc4388.utility.DesmosServer;
 
 /**
  * Do NOT add any static variables to this class, or any initialization at all.

src/main/java/frc4388/robot/Robot.java

@@ -7,6 +7,7 @@ package frc4388.robot;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc4388.utility.DesmosServer;
 import frc4388.utility.RobotTime;
 
 /**
@@ -22,6 +23,8 @@ public class Robot extends TimedRobot {
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
 
+  private static DesmosServer desmosServer;
+
   /**
    * This function is run when the robot is first started up and should be
    * used for any initialization code.
@@ -31,6 +34,10 @@ public class Robot extends TimedRobot {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+    
+    desmosServer = new DesmosServer(8000);
+    desmosServer.start();
+    DesmosServer.putInteger("Active", 1);
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.TestMotor;
 import frc4388.utility.DesmosServer;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.controller.IHandController;
@@ -40,6 +41,7 @@ public class RobotContainer {
   //   m_robotMap.rightBackEncoder
   // );
 
+  private final TestMotor m_testMotor = new TestMotor(m_robotMap.testMotor);
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
   /* Controllers */
@@ -57,7 +59,8 @@ public class RobotContainer {
     // m_robotSwerveDrive.setDefaultCommand(
     //     new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
     //         getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
-
+    m_testMotor.setDefaultCommand(new RunCommand(() -> m_testMotor.testDesmos(), m_testMotor));
+    
     // continually sends updates to the Blinkin LED controller to keep the lights on
     m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
   }

src/main/java/frc4388/robot/RobotMap.java

@@ -6,6 +6,8 @@ package frc4388.robot;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import frc4388.robot.Constants.LEDConstants;
@@ -92,4 +94,6 @@ public class RobotMap {
     //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
   }
 
+  public final CANSparkMax testMotor = new CANSparkMax(6, MotorType.kBrushless);
+
 }

src/main/java/frc4388/robot/subsystems/TestMotor.java

@@ -0,0 +1,23 @@
+package frc4388.robot.subsystems;
+
+import com.ctre.phoenix.sensors.CANCoder;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.utility.DesmosServer;
+
+public class TestMotor extends SubsystemBase {
+    private CANSparkMax m_testMotor;
+    private RelativeEncoder m_testEncoder;
+
+    public TestMotor(CANSparkMax testMotor) {
+        m_testMotor = testMotor;
+        m_testEncoder = m_testMotor.getEncoder();
+    }
+
+    public void testDesmos() {
+        DesmosServer.putDecimal("Position", m_testEncoder.getPosition());
+        m_testMotor.set(DesmosServer.readDouble("Speed"));
+    }
+}