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"));
+    }
+}

src/main/java/frc4388/utility/DesmosServer.java

@@ -7,6 +7,7 @@ import java.io.OutputStream;
 import java.net.ServerSocket;
 import java.net.Socket;
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.HashMap;
 
 import org.opencv.core.Point;
@@ -191,16 +192,25 @@ public class DesmosServer extends Thread {
     // ---------------------------------------------------------------------
 
     public static int readInteger(String name) {
+        if(!readVariables.containsKey(name))
+            return 0;
+        
         return Integer.parseInt(readVariables.get(name));
     }
 
     public static double readDouble(String name) {
+        if(!readVariables.containsKey(name))
+            return 0;
+        
         return Double.parseDouble(readVariables.get(name));
     }
 
     public static Point readPoint(String name) {
         Point point = new Point();
 
+        if(!readVariables.containsKey(name))
+            return point;
+
         String pointStr = readVariables.get(name);
         point.x = Double.parseDouble(pointStr.split(",")[0]);
         point.x = Double.parseDouble(pointStr.split(",")[1]);
@@ -209,6 +219,9 @@ public class DesmosServer extends Thread {
     }
     
     public static double[] readArray(String name) {
+        if(!readVariables.containsKey(name))
+            return new double[0];
+
     	String[] unparsed = readVariables.get(name).split(",");
     	double[] arr = new double[unparsed.length];