ftc codesim hardware improvements

Author: RoushilS

simulation/SyntheSimFTC/src/main/java/com/autodesk/synthesis/ftc/FTCWsBridge.java

@@ -27,6 +27,7 @@ public interface ConnectionListener {
     private final Gamepad gamepad1 = new Gamepad();
     private final Gamepad gamepad2 = new Gamepad();
     private final Map<String, SynthesisDcMotor> dcMotors = new ConcurrentHashMap<>();
+    private final Map<String, SynthesisServo> servos = new ConcurrentHashMap<>();
     private volatile ConnectionListener listener;
 
     private volatile boolean dsEnabled;
@@ -70,6 +71,19 @@ public void sendMotorPower(String deviceName, double power) {
         send("CANMotor", deviceName, data);
     }
 
+    public void registerServo(String deviceName, SynthesisServo servo) {
+        servos.put(deviceName, servo);
+        Map<String, Object> init = new HashMap<>();
+        init.put("<init", true);
+        send("PWM", deviceName, init);
+    }
+
+    public void sendServoPosition(String deviceName, double position) {
+        Map<String, Object> data = new HashMap<>();
+        data.put("<position", position);
+        send("PWM", deviceName, data);
+    }
+
     //Announces the driver station deice
     private void registerDriverStation() {
         Map<String, Object> init = new HashMap<>();
@@ -107,6 +121,7 @@ public void onOpen(WebSocket conn, ClientHandshake handshake) {
     public void onClose(WebSocket conn, int code, String reason, boolean remote) {
         System.out.println("[FTCWsBridge] Fission disconnected: " + reason);
         dcMotors.clear();
+        servos.clear();
         ConnectionListener l = listener;
         if (l != null) {
             l.onFissionDisconnected();

simulation/SyntheSimFTC/src/main/java/com/autodesk/synthesis/ftc/OpModeRunner.java

@@ -146,10 +146,12 @@ private void stopCurrent() {
         }
 
         private com.qualcomm.robotcore.hardware.HardwareDevice createDevice(Class<?> requestedType, String deviceName) {
-            // Widen this as the shim grows past DcMotorSimple (Servo, CRServo, IMU, ...).
             if (requestedType.isAssignableFrom(SynthesisDcMotor.class)) {
                 return new SynthesisDcMotor(deviceName, bridge);
             }
+            if (requestedType.isAssignableFrom(SynthesisServo.class)) {
+                return new SynthesisServo(deviceName, bridge);
+            }
 
             return null;
         }

simulation/SyntheSimFTC/src/main/java/com/autodesk/synthesis/ftc/SynthesisServo.java

@@ -0,0 +1,73 @@
+package com.autodesk.synthesis.ftc;
+
+import com.qualcomm.robotcore.hardware.HardwareDevice;
+import com.qualcomm.robotcore.hardware.Servo;
+
+/**
+ * Minimal synthetic FTC servo backed by the PWM contract used by Fission.
+ */
+public class SynthesisServo implements Servo {
+    private final String deviceName;
+    private final FTCWsBridge bridge;
+
+    private volatile Direction direction = Direction.FORWARD;
+    private volatile double position = 0.0;
+
+    public SynthesisServo(String deviceName, FTCWsBridge bridge) {
+        this.deviceName = deviceName;
+        this.bridge = bridge;
+        bridge.registerServo(deviceName, this);
+    }
+
+    @Override
+    public void setDirection(Direction direction) {
+        this.direction = direction;
+    }
+
+    @Override
+    public Direction getDirection() {
+        return direction;
+    }
+
+    @Override
+    public void setPosition(double position) {
+        this.position = Math.max(0.0, Math.min(1.0, position));
+        double effectivePosition = direction == Direction.REVERSE ? 1.0 - this.position : this.position;
+        bridge.sendServoPosition(deviceName, effectivePosition);
+    }
+
+    @Override
+    public double getPosition() {
+        return position;
+    }
+
+    @Override
+    public HardwareDevice.Manufacturer getManufacturer() {
+        return HardwareDevice.Manufacturer.Synthesis;
+    }
+
+    @Override
+    public String getDeviceName() {
+        return "Synthesis Servo";
+    }
+
+    @Override
+    public String getConnectionInfo() {
+        return "Synthesis simulation: " + deviceName;
+    }
+
+    @Override
+    public int getVersion() {
+        return 1;
+    }
+
+    @Override
+    public void resetDeviceConfigurationForOpMode() {
+        direction = Direction.FORWARD;
+        position = 0.0;
+    }
+
+    @Override
+    public void close() {
+    }
+}

simulation/SyntheSimFTC/src/main/java/com/qualcomm/robotcore/hardware/Servo.java

@@ -0,0 +1,19 @@
+package com.qualcomm.robotcore.hardware;
+
+/**
+ * Clean-room shim of the real FTC SDK interface.
+ */
+public interface Servo extends HardwareDevice {
+    enum Direction {
+        FORWARD,
+        REVERSE,
+    }
+
+    void setDirection(Direction direction);
+
+    Direction getDirection();
+
+    void setPosition(double position);
+
+    double getPosition();
+}