Send encoder and gyro data

public/js/xrp.js

@@ -2,6 +2,13 @@ const ROBOT_SIZE = 40; // 40px
 const ARENA_WIDTH = 800;
 const ARENA_HEIGHT = 500;
 
+// Robot is 10 physical inches, 
+// thus if we've moved 40px, we've moved 10 inches
+
+const INCH_PER_PIXEL = 0.5 / 40;
+const DISTANCE_PER_WHEEL_REV_IN = 2.3622 * Math.PI;
+const ENCODER_COUNT_PER_PIXEL = DISTANCE_PER_WHEEL_REV_IN * INCH_PER_PIXEL * 585;
+
 class Robot {
     constructor() {
         this._position = {
@@ -14,6 +21,9 @@ class Robot {
         this._leftSpeed = 0;
         this._rightSpeed = 0;
         this._bearing = 0;
+
+        this._leftEncoder = 0;
+        this._rightEncoder = 0;
     }
 
     get robotDivPosition() {
@@ -82,9 +92,22 @@ class Robot {
         this._rightSpeed = val;
     }
 
+    get leftEncoderValue() {
+        return this._leftEncoder;
+    }
+
+    get rightEncoderValue() {
+        return this._rightEncoder;
+    }
+
     resetPosition() {
+        this._leftEncoder = 0;
+        this._rightEncoder = 0;
+        this._leftSpeed = 0;
+        this._rightSpeed = 0;
         this.position = { x: ARENA_WIDTH / 2, y: ARENA_HEIGHT / 2};
         this.bearing = 0;
+
     }
 
     update() {
@@ -98,6 +121,13 @@ class Robot {
     _processTick(timeDelta) {
         const timeInSec = timeDelta / 1000;
 
+        // The speed values are in "pixels per second"
+        const leftDelta = this._leftSpeed * timeInSec;
+        const rightDelta = this._rightSpeed * timeInSec;
+
+        this._leftEncoder += leftDelta * ENCODER_COUNT_PER_PIXEL;
+        this._rightEncoder += rightDelta * ENCODER_COUNT_PER_PIXEL;
+
         const newPos = _calculateNewPosition(this._position, this._bearing, 
                             this._leftSpeed, this._rightSpeed, timeInSec);
 
@@ -199,4 +229,17 @@ setInterval(() => {
     _lastTime = currTime;
 }, 10);
 
+setInterval(() => {
+    if (shouldSendSensorData) {
+        ws.send(JSON.stringify({
+            type: "SensorData",
+            payload: {
+                heading: robot.bearing,
+                leftEncoder: robot.leftEncoderValue,
+                rightEncoder: robot.rightEncoderValue
+            }
+        }))
+    }
+}, 50);
+
 };

src/index.ts

@@ -73,6 +73,19 @@ wsServer.on("connection", (ws, request) => {
 
     ws.on("message", (data) => {
         // Sensor data TODO
+        try {
+            const messageJson = JSON.parse(data.toString());
+
+            if (messageJson.type === "SensorData") {
+                const payload = messageJson.payload;
+                xrp.setHeading(payload.heading);
+                xrp.setLeftEncoderValue(payload.leftEncoder);
+                xrp.setRightEncoderValue(payload.rightEncoder);
+
+            }
+        } catch (err) {
+
+        }
     });
 });
 

src/xrp-sim.ts

@@ -120,6 +120,7 @@ export default class XRPSim {
     }
 
     public setHeading(hdg: number) {
+        console.log("Heading: ", hdg);
         this._xrpState.setHeading(hdg);
     }
 
@@ -133,6 +134,7 @@ export default class XRPSim {
 
     private _sendToWPILib() {
         if (this._remoteSocketInfo !== null) {
+            // console.log("sendToWPILib ", this._xrpState.getLeftEncoder(), this._xrpState.getRightEncoder());
             const header = Buffer.alloc(3);
             header.writeUInt16BE(this._outboundSeq, 0);
 

src/xrp-state.ts

@@ -12,10 +12,18 @@ export default class XRPState {
         this._encoderInputs.set(0, val);
     }
 
+    public getLeftEncoder(): number {
+        return this._encoderInputs.get(0) || 0;
+    }
+
     public setRightEncoder(val: number) {
         this._encoderInputs.set(1, val);
     }
 
+    public getRightEncoder(): number {
+        return this._encoderInputs.get(1) || 0;
+    }
+
     public setHeading(val: number) {
         this._heading = val;
     }