update position and index in drive command

src/server/command/move-command.ts

@@ -1,6 +1,7 @@
 import { RobotCommand, Reversible } from "./command";
 import { Position } from "../robot/position";
 import { robotManager } from "../api/managers";
+import { GridIndices } from "../robot/grid-indices";
 
 /**
  * Represents a rotation.
@@ -102,6 +103,17 @@ export class DriveCommand
 
     public async execute(): Promise<void> {
         const robot = robotManager.getRobot(this.robotId);
+        const currentPosition = robot.position;
+        const newPositionX = this.tileDistance * Math.cos(robot.headingRadians);
+        const newPositionY = this.tileDistance * Math.sin(robot.headingRadians);
+        robot.position = new Position(
+            newPositionX + currentPosition.x,
+            newPositionY + currentPosition.y,
+        );
+        robotManager.updateRobot(
+            this.robotId,
+            new GridIndices(Math.floor(newPositionX), Math.floor(newPositionY)),
+        );
         return robot.sendDrivePacket(this.tileDistance);
     }
 

src/server/robot/robot.ts

@@ -29,15 +29,15 @@ export class Robot {
         return this._position;
     }
 
-    private set position(coords: Position) {
+    public set position(coords: Position) {
         this._position = coords;
     }
 
     public get headingRadians(): number {
         return this._headingRadians;
     }
 
-    private set headingRadians(headingRadians: number) {
+    public set headingRadians(headingRadians: number) {
         this._headingRadians = headingRadians;
     }