diff --git a/src/__mocks__/emptyModule.js b/src/__mocks__/emptyModule.js new file mode 100644 index 000000000..f053ebf79 --- /dev/null +++ b/src/__mocks__/emptyModule.js @@ -0,0 +1 @@ +module.exports = {}; diff --git a/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Chassis.ts b/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Chassis.ts new file mode 100644 index 000000000..7579d71e7 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Chassis.ts @@ -0,0 +1,88 @@ +import * as THREE from 'three'; +import { Physics, Renderer, EntityFactory , MeshFactory } from '../../../../engine'; + +import { ChassisWrapper } from '../../../ev3/components/Chassis'; + +jest.mock('../../../../engine', () => ({ + Physics: jest.fn(), + Renderer: jest.fn(), + EntityFactory: { addCuboid: jest.fn() }, + MeshFactory: { addCuboid: jest.fn() } +})); +jest.mock('../../../../engine/Entity/EntityFactory'); + +jest.mock('three', () => { + const three = jest.requireActual('three'); + return { + ...three, + Mesh: jest.fn().mockImplementation(() => ({ + position: { copy: jest.fn() }, + quaternion: { copy: jest.fn() }, + visible: false, + })), + Color: jest.fn() + }; +}); + +const mockedMeshFactory = MeshFactory as jest.Mocked; +const mockedEntityFactory = EntityFactory as jest.Mocked; + +describe('ChassisWrapper', () => { + let physicsMock; + let rendererMock; + let chassisWrapper; + let config; + + beforeEach(() => { + physicsMock = jest.fn() as unknown as Physics; + rendererMock = {add:jest.fn()} as unknown as Renderer; + config = { + dimension: { width: 1, height: 1, depth: 1 }, + orientation: { x: 0, y: 0, z: 0, w: 1 }, + debug: true + }; + + mockedMeshFactory.addCuboid.mockReturnValue(new THREE.Mesh()); + chassisWrapper = new ChassisWrapper(physicsMock, rendererMock, config); + + }); + + it('should initialize with a debug mesh if debug is true', () => { + expect(MeshFactory.addCuboid).toHaveBeenCalledWith({ + orientation: config.orientation, + dimension: config.dimension, + color: expect.any(THREE.Color), + debug: true + }); + expect(rendererMock.add).toBeCalled(); + expect(chassisWrapper.debugMesh.visible).toBe(true); + }); + + it('should throw if getEntity is called before chassis is initialized', () => { + expect(chassisWrapper.chassis).toBe(null); + expect(() => chassisWrapper.getEntity()).toThrow('Chassis not initialized'); + }); + + it('should correctly initialize the chassis entity on start', async () => { + const mockEntity = { getTranslation: jest.fn(), getRotation: jest.fn() }; + mockedEntityFactory.addCuboid.mockReturnValue(mockEntity as any); + await chassisWrapper.start(); + + expect(chassisWrapper.chassis).toBe(mockEntity); + expect(EntityFactory.addCuboid).toHaveBeenCalledWith(physicsMock, config); + }); + + it('should update the position and orientation of the debug mesh to match the chassis entity', () => { + const mockEntity = { + getTranslation: jest.fn().mockReturnValue(new THREE.Vector3()), + getRotation: jest.fn().mockReturnValue(new THREE.Quaternion()) + }; + mockedEntityFactory.addCuboid.mockReturnValue(mockEntity as any); + chassisWrapper.chassis = mockEntity; + + chassisWrapper.update(); + + expect(chassisWrapper.debugMesh.position.copy).toHaveBeenCalledWith(mockEntity.getTranslation()); + expect(chassisWrapper.debugMesh.quaternion.copy).toHaveBeenCalledWith(mockEntity.getRotation()); + }); +}); diff --git a/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Mesh.ts b/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Mesh.ts new file mode 100644 index 000000000..082e14986 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Mesh.ts @@ -0,0 +1,105 @@ +import * as THREE from 'three'; +import { Renderer } from '../../../../engine'; +import { loadGLTF } from '../../../../engine/Render/helpers/GLTF'; +import { ChassisWrapper } from '../../../ev3/components/Chassis'; +import { Mesh } from '../../../ev3/components/Mesh'; + +jest.mock('three', () => { + const three = jest.requireActual('three'); + return { + ...three, + GLTF: jest.fn().mockImplementation(() => ({ + scene: {}, + })), + }; +}); + +jest.mock('../../../../engine/Render/helpers/GLTF', () => ({ + loadGLTF: jest.fn().mockResolvedValue({ + scene: { + position: { + copy: jest.fn(), + }, + quaternion: { + copy: jest.fn(), + }, + }, + }), +})); + +jest.mock('../../../ev3/components/Chassis', () => ({ + ChassisWrapper: jest.fn().mockImplementation(() => ({ + getEntity: jest.fn().mockReturnValue({ + getTranslation: jest.fn().mockReturnValue(new THREE.Vector3()), + getRotation: jest.fn().mockReturnValue(new THREE.Quaternion()), + }), + })), +})); + +jest.mock('../../../../engine', () => ({ + Renderer: jest.fn().mockImplementation(() => ({ + add: jest.fn(), + })), +})); + +describe('Mesh', () => { + let mesh; + let mockChassisWrapper; + let mockRenderer; + let mockConfig; + + beforeEach(() => { + mockRenderer = { add: jest.fn() } as unknown as Renderer; + mockChassisWrapper = { + getEntity: jest.fn().mockReturnValue({ + getTranslation: jest.fn().mockReturnValue(new THREE.Vector3()), + getRotation: jest.fn().mockReturnValue(new THREE.Quaternion()), + }), + config: { + orientation: { + position: { + x: 0, + y: 0.0775, + z: 0, + }, + rotation: { + x: 0, + y: 0, + z: 0, + w: 1, + }, + }, + } + } as unknown as ChassisWrapper; + mockConfig = { + url: 'path/to/mesh', + dimension: { width: 1, height: 2, depth: 3 }, + offset: { x: 0.5, y: 0.5, z: 0.5 }, + }; + + // mockLoadGLTF.mockResolvedValue({ + // scene: new THREE.GLTF().scene + // } as any); + + mesh = new Mesh(mockChassisWrapper, mockRenderer, mockConfig); + }); + + it('should initialize correctly with given configurations', () => { + expect(mesh.config.url).toBe(mockConfig.url); + expect(mesh.offset.x).toBe(0.5); + }); + + it('should load the mesh and add it to the renderer on start', async () => { + await mesh.start(); + expect(loadGLTF).toHaveBeenCalledWith(mockConfig.url, mockConfig.dimension); + expect(mockRenderer.add).toHaveBeenCalledWith(expect.any(Object)); // Checks if mesh scene is added to renderer + }); + + it('should update mesh position and orientation according to chassis', async () => { + await mesh.start(); + mesh.update({residualFactor: 0.5}); + + expect(mesh.mesh.scene.position.copy).toHaveBeenCalled(); + expect(mesh.mesh.scene.quaternion.copy).toHaveBeenCalled(); + }); +}); diff --git a/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Motor.ts b/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Motor.ts new file mode 100644 index 000000000..f50f13848 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Motor.ts @@ -0,0 +1,118 @@ +import * as THREE from 'three'; +import { Renderer, Physics } from '../../../../engine'; +import { loadGLTF } from '../../../../engine/Render/helpers/GLTF'; +import { ChassisWrapper } from '../../../ev3/components/Chassis'; +import { Motor } from '../../../ev3/components/Motor'; +import { ev3Config } from '../../../ev3/ev3/default/config'; + +jest.mock('three', () => { + const originalModule = jest.requireActual('three'); + return { + ...originalModule, + }; +}); + +jest.mock('../../../../engine/Render/helpers/GLTF', () => ({ + loadGLTF: jest.fn().mockResolvedValue({ + scene: { + position: { + copy: jest.fn(), + }, + quaternion: { + copy: jest.fn(), + }, + rotateX: jest.fn(), + rotateZ: jest.fn() + } + }), +})); + +jest.mock('../../../../engine', () => ({ + Physics: jest.fn(), + Renderer: jest.fn().mockImplementation(() => ({ + add: jest.fn(), + })), +})); + +jest.mock('../../../ev3/components/Chassis', () => ({ + ChassisWrapper: jest.fn().mockImplementation(() => ({ + getEntity: jest.fn().mockReturnValue({ + transformDirection: jest.fn().mockImplementation((v) => v), + worldVelocity: jest.fn().mockReturnValue(new THREE.Vector3()), + worldTranslation: jest.fn().mockReturnValue(new THREE.Vector3()), + applyImpulse: jest.fn(), + getMass: jest.fn().mockReturnValue(1), + getRotation: jest.fn(), + }), + })), +})); + +describe('Motor', () => { + let motor; + let mockChassisWrapper; + let mockPhysics; + let mockRenderer; + let mockConfig; + + beforeEach(() => { + mockPhysics = { + applyImpulse: jest.fn(), + } as unknown as Physics; + mockRenderer = { add: jest.fn() } as unknown as Renderer; + mockConfig = { + displacement: { x: 1, y: 0, z: 0 }, + pid: { + proportionalGain: 1, + integralGain: 0.1, + derivativeGain: 0.01, + }, + mesh: { + url: 'path/to/mesh', + dimension: { height: 1, width: 1, depth: 1 }, + }, + }; + const config = ev3Config.motors[0]; + mockChassisWrapper = new ChassisWrapper(mockPhysics, mockRenderer, config); + motor = new Motor( + mockChassisWrapper, + mockPhysics, + mockRenderer, + mockConfig + ); + }); + + it('should initialize correctly and load the mesh', async () => { + await motor.start(); + expect(loadGLTF).toHaveBeenCalledWith( + mockConfig.mesh.url, + mockConfig.mesh.dimension + ); + expect(mockRenderer.add).toHaveBeenCalled(); + }); + + it('sets motor velocity and schedules stop with distance', () => { + motor.setSpeedDistance(10, 100); + expect(motor.motorVelocity).toBe(10); + }); + + it('updates the motor velocity and applies impulse', () => { + motor.fixedUpdate({ deltaTime: 1 }); + expect(mockChassisWrapper.getEntity().applyImpulse).toHaveBeenCalled(); + }); + + it('updates mesh', async () => { + await motor.start(); + motor.update({frameDuration: 1}); + + expect(motor.mesh.scene.position.copy).toBeCalled(); + expect(motor.mesh.scene.quaternion.copy).toBeCalled(); + }); + + it('rotates the mesh if on the left side', async () => { + motor.wheelSide = 'left'; + await motor.start(); + motor.update({frameDuration: 1}); + + expect(motor.mesh.scene.rotateZ).toBeCalled(); + }); +}); diff --git a/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Wheel.ts b/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Wheel.ts new file mode 100644 index 000000000..2112bea79 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/__tests__/ev3/components/Wheel.ts @@ -0,0 +1,109 @@ +import * as THREE from 'three'; +import { Wheel } from '../../../ev3/components/Wheel'; + +jest.mock('../../../../engine/Render/debug/DebugArrow', () => ({ + DebugArrow: jest.fn().mockImplementation(() => ({ + getMesh: jest.fn().mockReturnValue({}), + update: jest.fn(), + })), +})); + +jest.mock('three', () => { + const originalModule = jest.requireActual('three'); + return { + ...originalModule, + }; +}); + +describe('Wheel', () => { + let wheel; + let mockChassisWrapper; + let mockPhysics; + let mockRenderer; + let mockConfig; + + beforeEach(() => { + mockPhysics = { + castRay: jest.fn(), + }; + mockRenderer = { + add: jest.fn(), + }; + mockChassisWrapper = { + getEntity: jest.fn().mockReturnValue({ + worldTranslation: jest.fn().mockImplementation(() => new THREE.Vector3()), + transformDirection: jest.fn().mockImplementation(() => new THREE.Vector3()), + applyImpulse: jest.fn(), + getMass: jest.fn().mockReturnValue(1), + getCollider: jest.fn().mockReturnValue({}), + }), + }, + mockConfig = { + displacement: { x: 1, y: 0, z: 0 }, + pid: { + proportionalGain: 1, + integralGain: 0.1, + derivativeGain: 0.01, + }, + gapToFloor: 0.5, + maxRayDistance: 5, + debug: true, + }; + wheel = new Wheel( + mockChassisWrapper, + mockPhysics, + mockRenderer, + mockConfig + ); + }); + + it('should initialize with a debug arrow if debug is true', () => { + expect(wheel.arrowHelper).toBeDefined(); + expect(mockRenderer.add).toHaveBeenCalled(); + }); + + it('should correctly calculate physics interactions in fixedUpdate', () => { + const timingInfo = { timestep: 16 }; // 16 ms timestep + const mockResult = { + distance: 0.3, + normal: new THREE.Vector3(0, 1, 0), + }; + mockPhysics.castRay.mockReturnValue(mockResult); + + wheel.fixedUpdate(timingInfo); + + expect(mockPhysics.castRay).toHaveBeenCalledWith( + expect.any(THREE.Vector3), + expect.any(THREE.Vector3), + mockConfig.maxRayDistance, + expect.anything() + ); + expect(mockChassisWrapper.getEntity().applyImpulse).toHaveBeenCalled(); + expect(wheel.arrowHelper.update).toHaveBeenCalled(); + }); + + it('should handle null result from castRay indicating no ground contact', () => { + const timingInfo = { timestep: 16 }; + mockPhysics.castRay.mockReturnValue(null); + + wheel.fixedUpdate(timingInfo); + + expect(mockChassisWrapper.getEntity().applyImpulse).not.toHaveBeenCalled(); + }); + + it('if wheelDistance is 0, the normal should be pointing up', () => { + const timingInfo = { timestep: 16 }; // 16 ms timestep + const mockResult = { + distance: 0, + normal: new THREE.Vector3(0, 0, 0), + }; + mockPhysics.castRay.mockReturnValue(mockResult); + + wheel.fixedUpdate(timingInfo); + + expect(mockChassisWrapper.getEntity().applyImpulse).toHaveBeenCalledWith( + expect.objectContaining({x: 0, z: 0}), // y value can be anything + expect.any(THREE.Vector3) + ); + }); +}); diff --git a/src/bundles/robot_simulation/controllers/__tests__/ev3/ev3/default/ev3.ts b/src/bundles/robot_simulation/controllers/__tests__/ev3/ev3/default/ev3.ts new file mode 100644 index 000000000..133a0dcbd --- /dev/null +++ b/src/bundles/robot_simulation/controllers/__tests__/ev3/ev3/default/ev3.ts @@ -0,0 +1,56 @@ +import { Physics, Renderer, ControllerMap } from '../../../../../engine'; +import { ChassisWrapper } from '../../../../ev3/components/Chassis'; +import { Mesh } from '../../../../ev3/components/Mesh'; +import { Motor } from '../../../../ev3/components/Motor'; +import { Wheel } from '../../../../ev3/components/Wheel'; +import { ev3Config } from '../../../../ev3/ev3/default/config'; +import { createDefaultEv3 } from '../../../../ev3/ev3/default/ev3'; +import { ColorSensor } from '../../../../ev3/sensor/ColorSensor'; +import { UltrasonicSensor } from '../../../../ev3/sensor/UltrasonicSensor'; + +jest.mock('../../../../ev3/components/Chassis', () => { + return { ChassisWrapper: jest.fn() }; +}); +jest.mock('../../../../ev3/components/Mesh', () => { + return { Mesh: jest.fn() }; +}); +jest.mock('../../../../ev3/components/Motor', () => { + return { Motor: jest.fn() }; +}); +jest.mock('../../../../ev3/components/Wheel', () => { + return { Wheel: jest.fn() }; +}); +jest.mock('../../../../ev3/sensor/ColorSensor', () => { + return { ColorSensor: jest.fn() }; +}); +jest.mock('../../../../ev3/sensor/UltrasonicSensor', () => { + return { UltrasonicSensor: jest.fn() }; +}); +jest.mock('../../../../../engine', () => { + return { + Physics: jest.fn(), + Renderer: jest.fn(), + ControllerMap: jest.fn().mockImplementation(() => { + return { add: jest.fn() }; + }) + }; +}); + +describe('createDefaultEv3', () => { + const mockPhysics = new Physics({gravity:{x:0, y:-1, z:0}, timestep: 0.01}); + const mockRenderer = jest.fn() as unknown as Renderer; + const mockConfig =ev3Config; + + it('should correctly create all components and return a controller map', () => { + createDefaultEv3(mockPhysics, mockRenderer, mockConfig); + + expect(ChassisWrapper).toHaveBeenCalledWith(mockPhysics, mockRenderer, mockConfig.chassis); + expect(Mesh).toHaveBeenCalledWith(expect.any(ChassisWrapper), mockRenderer, mockConfig.mesh); + expect(Wheel).toHaveBeenCalledTimes(4); + expect(Motor).toHaveBeenCalledTimes(2); + expect(ColorSensor).toHaveBeenCalledWith(expect.any(ChassisWrapper), mockRenderer, mockConfig.colorSensor); + expect(UltrasonicSensor).toHaveBeenCalledWith(expect.any(ChassisWrapper), mockPhysics, mockRenderer, mockConfig.ultrasonicSensor); + + expect(ControllerMap).toHaveBeenCalled(); + }); +}); diff --git a/src/bundles/robot_simulation/controllers/__tests__/ev3/feedback_control/PidController.ts b/src/bundles/robot_simulation/controllers/__tests__/ev3/feedback_control/PidController.ts new file mode 100644 index 000000000..f23f747d1 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/__tests__/ev3/feedback_control/PidController.ts @@ -0,0 +1,95 @@ +import * as THREE from 'three'; +import { NumberPidController, VectorPidController } from '../../../ev3/feedback_control/PidController'; + +jest.mock('three', () => { + const three = jest.requireActual('three'); + return { + ...three, + }; +}); + +const resetPid = (pidController:NumberPidController) => { + pidController.errorsSum = 0; + pidController.previousError = 0; +}; + +const resetVectorPid = (pidController:VectorPidController) => { + pidController.errorsSum = new THREE.Vector3(); + pidController.previousError = new THREE.Vector3(); +}; + +describe('NumberPidController', () => { + let pidController; + + beforeEach(() => { + pidController = new NumberPidController({ + proportionalGain: 0.1, + integralGain: 0.01, + derivativeGain: 0.05, + }); + }); + + it('should initialize correctly', () => { + expect(pidController.proportionalGain).toEqual(0.1); + expect(pidController.integralGain).toEqual(0.01); + expect(pidController.derivativeGain).toEqual(0.05); + }); + + it('should calculate correct PID output', () => { + const setpoint = 10; + let output; + + output = pidController.calculate(2, setpoint); + expect(output).toBeGreaterThan(0); + resetPid(pidController); + + output = pidController.calculate(9, setpoint); + expect(output).toBeGreaterThan(0); + resetPid(pidController); + + output = pidController.calculate(11, setpoint); + expect(output).toBeLessThan(0); + resetPid(pidController); + + output = pidController.calculate(10, setpoint); + expect(output).toBeCloseTo(0); + }); +}); + +describe('VectorPidController', () => { + let pidController; + + beforeEach(() => { + pidController = new VectorPidController({ + proportionalGain: 0.1, + integralGain: 0.01, + derivativeGain: 0.05, + }); + }); + + it('should initialize correctly', () => { + expect(pidController.proportionalGain).toEqual(0.1); + }); + + it('should calculate correct PID output for vector inputs', () => { + const setpoint = new THREE.Vector3(10, 10, 10); + let output; + + output = pidController.calculate(new THREE.Vector3(0, 0, 0), setpoint); + expect(output.length()).toBeGreaterThan(0); // Output should push towards setpoint + resetVectorPid(pidController); + + output = pidController.calculate(new THREE.Vector3(9, 9, 9), setpoint); + expect(output.length()).toBeGreaterThan(0); + resetVectorPid(pidController); + + output = pidController.calculate(new THREE.Vector3(11, 11, 11), setpoint); + expect(output.length()).toBeGreaterThan(0); + resetVectorPid(pidController); + + output = pidController.calculate(new THREE.Vector3(10, 10, 10), setpoint); + expect(output.x).toBeCloseTo(0); + expect(output.y).toBeCloseTo(0); + expect(output.z).toBeCloseTo(0); + }); +}); diff --git a/src/bundles/robot_simulation/controllers/__tests__/ev3/sensor/ColorSensor.ts b/src/bundles/robot_simulation/controllers/__tests__/ev3/sensor/ColorSensor.ts new file mode 100644 index 000000000..dfd45cdb9 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/__tests__/ev3/sensor/ColorSensor.ts @@ -0,0 +1,102 @@ +import * as THREE from 'three'; +import { ColorSensor } from '../../../ev3/sensor/ColorSensor'; + +jest.mock('three', () => { + const three = jest.requireActual('three'); + return { + ...three, + }; +}); + +jest.mock('../../../../engine', () => ({ + Renderer: jest.fn().mockImplementation(() => ({ + scene: jest.fn(), + render: jest.fn(), + getElement: jest.fn(() => document.createElement('canvas')), + })), +})); + +jest.mock('../../../../engine/Render/helpers/Camera', () => ({ + getCamera: jest.fn().mockImplementation(() => { + return new THREE.PerspectiveCamera(); + }), +})); + +describe('ColorSensor', () => { + let sensor; + let mockChassisWrapper; + let mockRenderer; + let mockConfig; + + beforeEach(() => { + mockChassisWrapper = { + getEntity: jest.fn(() => ({ + worldTranslation: jest.fn().mockReturnValue(new THREE.Vector3()), + })), + }; + mockRenderer = { + add: jest.fn(), + scene: jest.fn(), + render: jest.fn(), + getElement: jest.fn(() => document.createElement('canvas')), + }; + mockConfig = { + tickRateInSeconds: 0.1, + displacement: { + x: 0.04, + y: 0.2, + z: 0.01, + }, + size: { + height: 16, + width: 16, + }, + camera: { + type: 'perspective', + aspect: 1, + fov: 10, + near: 0.01, + far: 1, + }, + debug: true, + }; + + sensor = new ColorSensor(mockChassisWrapper, mockRenderer, mockConfig); + + const mockCtx = { + getImageData: jest.fn(() => ({ + data: new Uint8ClampedArray([255, 255, 255, 255]), + })), + putImageData: jest.fn(), + drawImage: jest.fn(), + fillRect: jest.fn(), + clearRect: jest.fn(), + canvas: {}, + }; + + HTMLCanvasElement.prototype.getContext = jest + .fn() + .mockImplementation((_) => { + return mockCtx; + }); + }); + + it('should initialize correctly', () => { + expect(sensor).toBeDefined(); + expect(mockRenderer.add).toHaveBeenCalled(); + }); + + it('should update color only after accumulating sufficient time', () => { + const timingInfo = { timestep: 50 }; + sensor.fixedUpdate(timingInfo); + expect(mockRenderer.render).not.toHaveBeenCalled(); + sensor.fixedUpdate(timingInfo); + }); + + it('should give correct response for sense', () => { + const colorSensed = { r: 10, g: 20, b: 30 }; + sensor.colorSensed = colorSensed; + const result = sensor.sense(); + expect(result).toEqual(colorSensed); + }); +}); diff --git a/src/bundles/robot_simulation/controllers/__tests__/ev3/sensor/UltrasonicSensor.ts b/src/bundles/robot_simulation/controllers/__tests__/ev3/sensor/UltrasonicSensor.ts new file mode 100644 index 000000000..3b2df33b2 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/__tests__/ev3/sensor/UltrasonicSensor.ts @@ -0,0 +1,73 @@ +import * as THREE from 'three'; +import { UltrasonicSensor } from '../../../ev3/sensor/UltrasonicSensor'; + +jest.mock('three', () => ({ + Vector3: jest.fn().mockImplementation(() => ({ + clone: jest.fn().mockReturnThis(), + normalize: jest.fn().mockReturnThis(), + copy: jest.fn() + })), + ArrowHelper: jest.fn().mockImplementation(() => ({ + visible: false, + position: { + copy: jest.fn() + }, + setDirection: jest.fn() + })) +})); + +describe('UltrasonicSensor', () => { + let sensor: UltrasonicSensor; + let mockChassisWrapper; + let mockPhysics; + let mockRenderer; + let mockConfig; + + beforeEach(() => { + mockChassisWrapper = { + getEntity: jest.fn(() => ({ + worldTranslation: jest.fn().mockReturnValue(new THREE.Vector3()), + transformDirection: jest.fn().mockReturnValue(new THREE.Vector3()), + getCollider: jest.fn() + })) + }; + mockPhysics = { + castRay: jest.fn().mockReturnValue({ distance: 5 }) + }; + mockRenderer = { + add: jest.fn() + }; + mockConfig = { + displacement: { x: 1, y: 1, z: 1 }, + direction: { x: 0, y: 1, z: 0 }, + debug: true + }; + + sensor = new UltrasonicSensor(mockChassisWrapper, mockPhysics, mockRenderer, mockConfig); + }); + + it('should create instances and set initial properties', () => { + expect(sensor).toBeDefined(); + expect(THREE.Vector3).toHaveBeenCalledTimes(2); // Called for displacement and direction + expect(mockRenderer.add).toHaveBeenCalledWith(sensor.debugArrow); + }); + + it('should return initial distance sensed as 0', () => { + expect(sensor.sense()).toEqual(0); + }); + + it('should calculate distance when fixedUpdate is called', () => { + sensor.fixedUpdate(); + expect(sensor.distanceSensed).toEqual(5); + expect(mockPhysics.castRay).toHaveBeenCalled(); + expect(sensor.debugArrow.visible).toBeTruthy(); + expect(sensor.debugArrow.setDirection).toHaveBeenCalled(); + }); + + it('should handle null results from castRay indicating no collision detected', () => { + mockPhysics.castRay.mockReturnValue(null); + sensor.fixedUpdate(); + expect(sensor.distanceSensed).toEqual(0); + expect(mockPhysics.castRay).toHaveBeenCalled(); + }); +}); diff --git a/src/bundles/robot_simulation/controllers/__tests__/program/Program.ts b/src/bundles/robot_simulation/controllers/__tests__/program/Program.ts new file mode 100644 index 000000000..eae138830 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/__tests__/program/Program.ts @@ -0,0 +1,81 @@ +import { CallbackHandler } from '../../../engine/Core/CallbackHandler'; +import { Program, program_controller_identifier } from '../../program/Program'; +import { runECEvaluator } from '../../program/evaluate'; + +jest.mock('../../../engine/Core/CallbackHandler'); +jest.mock('../../program/evaluate'); + +const mockedRunECEvaluator = runECEvaluator as jest.MockedFunction; +const mockedCallbackHandler = CallbackHandler as jest.MockedClass; + +describe('Program', () => { + let program: Program; + const mockCode = 'const x = 1;'; + + beforeEach(() => { + mockedCallbackHandler.mockClear(); + mockedRunECEvaluator.mockClear(); + + program = new Program(mockCode); + + jest.spyOn(console, 'error').mockImplementation(jest.fn()); + }); + + it('should initialize with default configuration if none provided', () => { + expect(program.config.stepsPerTick).toEqual(11); + expect(program.name).toEqual(program_controller_identifier); + expect(program.isPaused).toBeFalsy(); + }); + + it('should merge user configuration with default', () => { + const customProgram = new Program(mockCode, { stepsPerTick: 20 }); + expect(customProgram.config.stepsPerTick).toEqual(20); + }); + + it('should start the evaluator with correct options', () => { + const mockIterator = { next: jest.fn() } as any; + mockedRunECEvaluator.mockReturnValue(mockIterator); + + program.start(); + + expect(mockedRunECEvaluator).toHaveBeenCalledWith(mockCode, expect.anything(), expect.anything()); + expect(program.iterator).toBe(mockIterator); + }); + + it('should handle pause and resume correctly', () => { + const mockIterator = { next: jest.fn() } as any; + mockedRunECEvaluator.mockReturnValue(mockIterator); + + program.start(); + const tick = {stepCount:0,timestep: 1000} as any; + program.update(tick); + program.pause(900); + expect(program.isPaused).toBeTruthy(); + expect(CallbackHandler.prototype.addCallback).toHaveBeenCalledWith(expect.any(Function), 900); + + program.fixedUpdate(); + expect(mockIterator.next).not.toBeCalled(); + }); + + it('should process fixed number of steps per tick', () => { + const mockIterator = { next: jest.fn() } as any; + mockedRunECEvaluator.mockReturnValue(mockIterator); + + program.start(); + program.fixedUpdate(); + + expect(mockIterator.next).toHaveBeenCalledTimes(11); + }); + + it('should catch errors during fixedUpdate', () => { + expect(() => program.fixedUpdate()).toThrow('Error in program execution. Please check your code and try again.'); + }); + + it('should check callbacks on update', () => { + program.start(); + const mockTimingInfo = { deltaTime: 1 / 60 } as any; + program.update(mockTimingInfo); + + expect(mockedCallbackHandler.prototype.checkCallbacks).toHaveBeenCalledWith(mockTimingInfo); + }); +}); diff --git a/src/bundles/robot_simulation/controllers/__tests__/utils/mergeConfig.ts b/src/bundles/robot_simulation/controllers/__tests__/utils/mergeConfig.ts new file mode 100644 index 000000000..4ad579dd6 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/__tests__/utils/mergeConfig.ts @@ -0,0 +1,49 @@ +import { mergeConfig } from '../../utils/mergeConfig'; // Update the path accordingly + +describe('mergeConfig function', () => { + // Test default configuration alone + it('should return the default configuration when user configuration is undefined', () => { + const defaultConfig = { color: 'blue', size: 'large' }; + const result = mergeConfig(defaultConfig); + expect(result).toEqual(defaultConfig); + }); + + // Test overriding some properties + it('should override default properties with user properties', () => { + const defaultConfig = { color: 'blue', size: 'large', theme: 'light' }; + const userConfig = { color: 'red' }; // only overriding the color + const expectedConfig = { color: 'red', size: 'large', theme: 'light' }; + const result = mergeConfig(defaultConfig, userConfig); + expect(result).toEqual(expectedConfig); + }); + + // Test adding new properties + it('should add new properties from user configuration', () => { + type Config = { color: string, size: string, opacity?: number }; + const defaultConfig = { color: 'blue', size: 'large' }; + const userConfig = { opacity: 0.5 }; + const expectedConfig = { color: 'blue', size: 'large', opacity: 0.5 }; + const result = mergeConfig(defaultConfig, userConfig); + expect(result).toEqual(expectedConfig); + }); + + // Test nested objects + it('should correctly merge nested configurations', () => { + const defaultConfig = { window: { color: 'blue', size: 'large' }, isEnabled: true }; + const userConfig = { window: { color: 'red' }, isEnabled: false }; + const expectedConfig = { + window: { color: 'red', size: 'large' }, + isEnabled: false + }; + const result = mergeConfig(defaultConfig, userConfig); + expect(result).toEqual(expectedConfig); + }); + + // Test to ensure that defaultConfig is not mutated + it('should not mutate the default configuration object', () => { + const defaultConfig = { color: 'blue', size: 'large' }; + const userConfig = { color: 'red' }; + mergeConfig(defaultConfig, userConfig); + expect(defaultConfig).toEqual({ color: 'blue', size: 'large' }); // Check defaultConfig remains unchanged + }); +}); diff --git a/src/bundles/robot_simulation/controllers/environment/Paper.ts b/src/bundles/robot_simulation/controllers/environment/Paper.ts new file mode 100644 index 000000000..81fd70eaa --- /dev/null +++ b/src/bundles/robot_simulation/controllers/environment/Paper.ts @@ -0,0 +1,38 @@ +import * as THREE from 'three'; + +import type { Renderer } from '../../engine'; + +export type PaperConfig = { + url: string; + dimension: { + width: number; + height: number; + }; + position: {x: number, y: number}; + rotation: number, +}; + +export class Paper { + render: Renderer; + config: PaperConfig; + paper: THREE.Mesh; + + constructor(render: Renderer, config: PaperConfig) { + this.render = render; + this.config = config; + + const plane = new THREE.PlaneGeometry(this.config.dimension.width, this.config.dimension.height); // Creating a 1x1 plane for the carpet + this.paper = new THREE.Mesh(plane); + } + + async start() { + const texture = new THREE.TextureLoader() + .load(this.config.url); + const material = new THREE.MeshStandardMaterial({ map: texture }); + this.paper.position.set(this.config.position.x, 0.001, this.config.position.y); + this.paper.rotation.x = -Math.PI / 2; + this.paper.rotation.z = this.config.rotation; + this.paper.material = material; + this.render.add(this.paper); + } +} diff --git a/src/bundles/robot_simulation/controllers/ev3/components/Chassis.ts b/src/bundles/robot_simulation/controllers/ev3/components/Chassis.ts new file mode 100644 index 000000000..fc333df67 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/ev3/components/Chassis.ts @@ -0,0 +1,69 @@ +import * as THREE from 'three'; + +import { + type Physics, + type Controller, + EntityFactory, + type Entity, + MeshFactory, + type Renderer, +} from '../../../engine'; +import { type EntityCuboidOptions } from '../../../engine/Entity/EntityFactory'; + +export type ChassisWrapperConfig = EntityCuboidOptions & { + debug: boolean; +}; + +/** + * Wrapper for the chassis entity. It is needed because the chassis entity can only be initialized + * after the physics engine has been started. Therefore, the chassis entity needs to be wrapped in + * a controller. + * + * We also use this class to add an optional debug mesh to the chassis. + */ +export class ChassisWrapper implements Controller { + physics: Physics; + render: Renderer; + config: ChassisWrapperConfig; + + chassis: Entity | null = null; + debugMesh: THREE.Mesh; + + constructor( + physics: Physics, + render: Renderer, + config: ChassisWrapperConfig, + ) { + this.physics = physics; + this.render = render; + this.config = config; + + // Debug mesh. + this.debugMesh = MeshFactory.addCuboid({ + orientation: config.orientation, + dimension: config.dimension, + color: new THREE.Color(0x00ff00), + debug: true, + }); + // Set visible based on config. + this.debugMesh.visible = config.debug; + render.add(this.debugMesh); + } + + getEntity(): Entity { + if (this.chassis === null) { + throw new Error('Chassis not initialized'); + } + return this.chassis; + } + + async start(): Promise { + this.chassis = EntityFactory.addCuboid(this.physics, this.config); + } + + update(): void { + const chassisEntity = this.getEntity(); + this.debugMesh.position.copy(chassisEntity.getTranslation()); + this.debugMesh.quaternion.copy(chassisEntity.getRotation()); + } +} diff --git a/src/bundles/robot_simulation/controllers/ev3/components/Mesh.ts b/src/bundles/robot_simulation/controllers/ev3/components/Mesh.ts new file mode 100644 index 000000000..fee0ded29 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/ev3/components/Mesh.ts @@ -0,0 +1,79 @@ +import * as THREE from 'three'; + +import { type GLTF } from 'three/examples/jsm/loaders/GLTFLoader.js'; +import { type Controller, type Renderer } from '../../../engine'; +import type { Dimension, SimpleQuaternion, SimpleVector } from '../../../engine/Math/Vector'; +import type { PhysicsTimingInfo } from '../../../engine/Physics'; +import { loadGLTF } from '../../../engine/Render/helpers/GLTF'; +import { type ChassisWrapper } from './Chassis'; + +export type MeshConfig = { + url: string; + dimension: Dimension; + offset?: Partial; +}; + +/** + * This represents the mesh of the robot. In reality, the mesh could be part of the chassis, + * but for the sake of clarity it is split into its own controller. + */ +export class Mesh implements Controller { + chassisWrapper: ChassisWrapper; + render: Renderer; + config: MeshConfig; + offset: SimpleVector; + + mesh: GLTF | null = null; + + previousTranslation: SimpleVector | null= null; + previousRotation: SimpleQuaternion | null = null; + currentTranslation: SimpleVector; + currentRotation: SimpleQuaternion; + + constructor( + chassisWrapper: ChassisWrapper, + render: Renderer, + config: MeshConfig, + ) { + this.chassisWrapper = chassisWrapper; + this.render = render; + this.config = config; + this.offset = { + x: this.config?.offset?.x || 0, + y: this.config?.offset?.y || 0, + z: this.config?.offset?.z || 0, + }; + this.currentTranslation = this.chassisWrapper.config.orientation.position; + this.currentRotation = new THREE.Quaternion(0,0,0,1); + } + + async start(): Promise { + this.mesh = await loadGLTF(this.config.url, this.config.dimension); + + this.render.add(this.mesh.scene); + } + + fixedUpdate(): void { + this.previousTranslation = this.currentTranslation; + this.previousRotation = this.currentRotation; + this.currentRotation = this.chassisWrapper.getEntity().getRotation(); + this.currentTranslation = this.chassisWrapper.getEntity().getTranslation(); + } + + update(timingInfo: PhysicsTimingInfo) { + const vecCurrentTranslation = new THREE.Vector3().copy(this.currentTranslation); + const vecPreviousTranslation = new THREE.Vector3().copy(this.previousTranslation || this.currentTranslation); + const quatCurrentRotation = new THREE.Quaternion().copy(this.currentRotation); + const quatPreviousRotation = new THREE.Quaternion().copy(this.previousRotation || this.currentRotation); + + const estimatedTranslation = vecPreviousTranslation.lerp(vecCurrentTranslation, timingInfo.residualFactor); + const estimatedRotation = quatPreviousRotation.slerp(quatCurrentRotation, timingInfo.residualFactor); + + estimatedTranslation.x -= this.offset.x / 2; + estimatedTranslation.y -= this.offset.y / 2; + estimatedTranslation.z -= this.offset.z / 2; + + this.mesh?.scene.position.copy(estimatedTranslation); + this.mesh?.scene.quaternion.copy(estimatedRotation); + } +} diff --git a/src/bundles/robot_simulation/controllers/ev3/components/Motor.ts b/src/bundles/robot_simulation/controllers/ev3/components/Motor.ts new file mode 100644 index 000000000..dd2cda839 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/ev3/components/Motor.ts @@ -0,0 +1,148 @@ +import type * as THREE from 'three'; +import { type GLTF } from 'three/examples/jsm/loaders/GLTFLoader.js'; +import { type Controller, type Physics, type Renderer } from '../../../engine'; +import { CallbackHandler } from '../../../engine/Core/CallbackHandler'; +import { vec3 } from '../../../engine/Math/Convert'; +import { type Dimension, type SimpleVector } from '../../../engine/Math/Vector'; +import type { PhysicsTimingInfo } from '../../../engine/Physics'; +import { loadGLTF } from '../../../engine/Render/helpers/GLTF'; +import { VectorPidController } from '../feedback_control/PidController'; +import { type ChassisWrapper } from './Chassis'; + +type WheelSide = 'left' | 'right'; + +export type MotorConfig = { + displacement: SimpleVector; + pid: { + proportionalGain: number; + derivativeGain: number; + integralGain: number; + }; + mesh: { + url: string; + dimension: Dimension; + }; +}; +/** + * This represents the motor of the robot and is responsible for moving the robot. It is also + * responsible for the visual representation of the wheel and the friction. + */ +export class Motor implements Controller { + chassisWrapper: ChassisWrapper; + physics: Physics; + render: Renderer; + displacementVector: THREE.Vector3; + config: MotorConfig; + + motorVelocity: number; + meshRotation: number; + pid: VectorPidController; + + callbackHandler = new CallbackHandler(); + wheelSide: WheelSide; + + mesh: GLTF | null = null; + + constructor( + chassisWrapper: ChassisWrapper, + physics: Physics, + render: Renderer, + config: MotorConfig, + ) { + this.chassisWrapper = chassisWrapper; + this.physics = physics; + this.render = render; + this.displacementVector = vec3(config.displacement); + this.config = config; + + this.pid = new VectorPidController(config.pid); + this.motorVelocity = 0; + this.meshRotation = 0; + this.wheelSide = config.displacement.x > 0 ? 'right' : 'left'; + } + + setSpeedDistance(speed: number, distance: number) { + this.motorVelocity = speed; + + this.callbackHandler.addCallback(() => { + this.motorVelocity = 0; + }, (distance / speed) * 1000); + } + + async start(): Promise { + this.mesh = await loadGLTF(this.config.mesh.url, this.config.mesh.dimension); + this.render.add(this.mesh.scene); + } + + fixedUpdate(timingInfo: PhysicsTimingInfo): void { + this.callbackHandler.checkCallbacks(timingInfo); + const chassis = this.chassisWrapper.getEntity(); + + // Calculate the target motor velocity from the chassis perspective + const targetMotorVelocity = vec3({ + x: 0, + y: 0, + z: this.motorVelocity, + }); + + // Transform it to the global perspective + const targetMotorGlobalVelocity + = chassis.transformDirection(targetMotorVelocity); + + // Calculate the actual motor velocity from the global perspective + const actualMotorGlobalVelocity = chassis.worldVelocity( + this.displacementVector.clone(), + ); + + // Calculate the PID output with the PID controller + const pidOutput = this.pid.calculate( + actualMotorGlobalVelocity, + targetMotorGlobalVelocity, + ); + + // Find the global position of the motor + const motorGlobalPosition = chassis.worldTranslation( + this.displacementVector.clone(), + ); + + // Calculate the impulse to apply to the chassis + const impulse = pidOutput + .projectOnPlane( + vec3({ + x: 0, + y: 1, + z: 0, + }), + ) + .multiplyScalar(chassis.getMass()); + + // Apply the impulse to the chassis + chassis.applyImpulse(impulse, motorGlobalPosition); + } + + update(timingInfo: PhysicsTimingInfo): void { + const chassisEntity = this.chassisWrapper.getEntity(); + + // Calculate the new wheel position, adjusting the y-coordinate to half the mesh height + const wheelPosition = chassisEntity.worldTranslation(this.displacementVector.clone()); + wheelPosition.y = this.config.mesh.dimension.height / 2; // Ensure the wheel is placed correctly vertically + + // If mesh is loaded, update its position and orientation + if (this.mesh) { + this.mesh.scene.position.copy(wheelPosition); + this.mesh.scene.quaternion.copy(chassisEntity.getRotation()); + + // Calculate rotation adjustment based on motor velocity and frame duration + const radiansPerFrame = 2 * (this.motorVelocity / this.config.mesh.dimension.height) * timingInfo.frameDuration / 1000; + + // Apply rotation changes to simulate wheel turning + this.meshRotation += radiansPerFrame; + this.mesh.scene.rotateX(this.meshRotation); + + // If the wheel is on the left side, flip it to face the correct direction + if (this.wheelSide === 'left') { + this.mesh.scene.rotateZ(Math.PI); + } + } + } +} diff --git a/src/bundles/robot_simulation/controllers/ev3/components/Wheel.ts b/src/bundles/robot_simulation/controllers/ev3/components/Wheel.ts new file mode 100644 index 000000000..5074fb9c6 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/ev3/components/Wheel.ts @@ -0,0 +1,103 @@ +import type * as THREE from 'three'; +import { type Renderer, type Controller, type Physics } from '../../../engine'; +import { vec3 } from '../../../engine/Math/Convert'; +import { type SimpleVector } from '../../../engine/Math/Vector'; +import type { PhysicsTimingInfo } from '../../../engine/Physics'; +import { DebugArrow } from '../../../engine/Render/debug/DebugArrow'; +import { NumberPidController } from '../feedback_control/PidController'; +import { type ChassisWrapper } from './Chassis'; + +export type WheelConfig = { + pid: { + proportionalGain: number; + derivativeGain: number; + integralGain: number; + }; + displacement: SimpleVector; + gapToFloor: number; + maxRayDistance:number; + debug: boolean; +}; + +export class Wheel implements Controller { + chassisWrapper: ChassisWrapper; + physics: Physics; + render: Renderer; + config: WheelConfig; + + pid: NumberPidController; + displacementVector: THREE.Vector3; + downVector: THREE.Vector3; + arrowHelper: DebugArrow; + + constructor( + chassisWrapper: ChassisWrapper, + physics: Physics, + render: Renderer, + config: WheelConfig, + ) { + this.chassisWrapper = chassisWrapper; + this.physics = physics; + this.render = render; + this.displacementVector = vec3(config.displacement); + this.config = config; + + this.pid = new NumberPidController(config.pid); + this.downVector = vec3({ + x: 0, + y: -1, + z: 0, + }); + + // Debug arrow. + this.arrowHelper = new DebugArrow({ debug: config.debug }); + render.add(this.arrowHelper.getMesh()); + } + + fixedUpdate(timingInfo: PhysicsTimingInfo): void { + const chassis = this.chassisWrapper.getEntity(); + + const globalDisplacement = chassis.worldTranslation( + this.displacementVector.clone(), + ); + const globalDownDirection = chassis.transformDirection( + this.downVector.clone(), + ); + + const result = this.physics.castRay( + globalDisplacement, + globalDownDirection, + this.config.maxRayDistance, + chassis.getCollider(), + ); + + // Wheels are not touching the ground + if (result === null) { + return; + } + + const wheelDistance = result.distance; + let normal = result.normal; + + // If distance is zero, the ray originate from inside the floor/wall. + // If that is true, we assume the normal is pointing up. + if (wheelDistance === 0) { + normal = { + x: 0, + y: 1, + z: 0, + }; + } + + const error = this.pid.calculate(wheelDistance, this.config.gapToFloor); + + const force = vec3(normal) + .normalize() + .multiplyScalar((error * chassis.getMass() * timingInfo.timestep) / 1000); + + chassis.applyImpulse(force, globalDisplacement); + + // Debug arrow. + this.arrowHelper.update(globalDisplacement, force.clone(), force.length() * 1000); + } +} diff --git a/src/bundles/robot_simulation/controllers/ev3/ev3/default/config.ts b/src/bundles/robot_simulation/controllers/ev3/ev3/default/config.ts new file mode 100644 index 000000000..06a02d701 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/ev3/ev3/default/config.ts @@ -0,0 +1,155 @@ +import type { + Ev3ChassisConfig, + Ev3ColorSensorConfig, + Ev3Config, + Ev3MeshConfig, + Ev3MotorsConfig, + Ev3UltrasonicSenorConfig, + Ev3WheelsConfig, +} from './types'; + +const noRotation = { + x: 0, + y: 0, + z: 0, + w: 1, +}; + +const tinyConstant = 0.012; + +export const chassisConfig: Ev3ChassisConfig = { + orientation: { + position: { + x: 0, + y: 0.0775, + z: 0, + }, + rotation: noRotation, + }, + dimension: { + height: 0.095, + width: 0.145, + length: 0.18, + }, + mass: 0.6, + type: 'dynamic', + debug: true, +}; + +export const meshConfig: Ev3MeshConfig = { + url: 'https://keen-longma-3c1be1.netlify.app/6_remove_wheels.gltf', + dimension: chassisConfig.dimension, + offset: { + y: 0.02, + }, +}; + +export const wheelConfig: Ev3WheelsConfig = { + displacements: { + frontLeftWheel: { + x: -(chassisConfig.dimension.width / 2), + y: -(chassisConfig.dimension.height / 2), + z: chassisConfig.dimension.length / 2 - tinyConstant, + }, + + frontRightWheel: { + x: chassisConfig.dimension.width / 2, + y: -(chassisConfig.dimension.height / 2), + z: chassisConfig.dimension.length / 2 - tinyConstant, + }, + backLeftWheel: { + x: -(chassisConfig.dimension.width / 2), + y: -(chassisConfig.dimension.height / 2), + z: -(chassisConfig.dimension.length / 2 - tinyConstant), + }, + backRightWheel: { + x: chassisConfig.dimension.width / 2, + y: -(chassisConfig.dimension.height / 2), + z: -(chassisConfig.dimension.length / 2 - tinyConstant), + }, + }, + config: { + pid: { + proportionalGain: 27, + integralGain: 8, + derivativeGain: 40, + }, + gapToFloor: 0.03, + maxRayDistance: 0.05, + debug: true, + }, +}; + +export const motorConfig: Ev3MotorsConfig = { + config: { + pid: { + proportionalGain: 0.25, + derivativeGain: 0, + integralGain: 0, + }, + mesh: { + dimension: { + width: 0.028, + height: 0.0575, + length: 0.0575, + }, + url: 'https://keen-longma-3c1be1.netlify.app/6_wheel.gltf', + }, + }, + displacements: { + leftMotor: { + x: 0.058, + y: 0, + z: 0.03, + }, + rightMotor: { + x: -0.058, + y: 0, + z: 0.03, + }, + }, +}; + +export const colorSensorConfig = { + tickRateInSeconds: 0.1, + displacement: { + x: -0.06, + y: -(chassisConfig.dimension.height / 2), + z: 0.11, + }, + size: { + height: 16, + width: 16, + }, + camera: { + type: 'perspective', + aspect: 1, + fov: 10, + near: 0.01, + far: 1, + }, + debug: true, +} satisfies Ev3ColorSensorConfig; + +const ultrasonicSensorConfig: Ev3UltrasonicSenorConfig = { + displacement: { + x: 0.04, + y: 0, + z: 0.01, + }, + direction: { + x: 0, + y: 0, + z: 1, + }, + debug: true, +}; + +export const ev3Config: Ev3Config = { + chassis: chassisConfig, + motors: motorConfig, + wheels: wheelConfig, + colorSensor: colorSensorConfig, + ultrasonicSensor: ultrasonicSensorConfig, + mesh: meshConfig, +}; diff --git a/src/bundles/robot_simulation/controllers/ev3/ev3/default/ev3.ts b/src/bundles/robot_simulation/controllers/ev3/ev3/default/ev3.ts new file mode 100644 index 000000000..4b792c2cb --- /dev/null +++ b/src/bundles/robot_simulation/controllers/ev3/ev3/default/ev3.ts @@ -0,0 +1,76 @@ +import { type Physics, type Renderer, ControllerMap } from '../../../../engine'; + +import { ChassisWrapper } from '../../components/Chassis'; +import { Mesh } from '../../components/Mesh'; +import { Motor, type MotorConfig } from '../../components/Motor'; +import { Wheel, type WheelConfig } from '../../components/Wheel'; +import { ColorSensor } from '../../sensor/ColorSensor'; +import { UltrasonicSensor } from '../../sensor/UltrasonicSensor'; + +import { + wheelNames, + motorNames, + type DefaultEv3Controller, + type Ev3Config, + type WheelControllers, + type MotorControllers, +} from './types'; + +export type DefaultEv3 = ControllerMap; + +export const createDefaultEv3 = ( + physics: Physics, + render: Renderer, + config: Ev3Config, +): DefaultEv3 => { + const chassis = new ChassisWrapper(physics, render, config.chassis); + const mesh = new Mesh(chassis, render, config.mesh); + + const wheelControllers = wheelNames.reduce((acc, name) => { + const displacement = config.wheels.displacements[name]; + const wheelConfig: WheelConfig = { + ...config.wheels.config, + displacement, + }; + const wheel = new Wheel(chassis, physics, render, wheelConfig); + return { + ...acc, + [name]: wheel, + }; + }, {} as WheelControllers); + + // Motors + const motorControllers = motorNames.reduce((acc, name) => { + const displacement = config.motors.displacements[name]; + const motorConfig: MotorConfig = { + ...config.motors.config, + displacement, + }; + const motor = new Motor(chassis, physics, render, motorConfig); + return { + ...acc, + [name]: motor, + }; + }, {} as MotorControllers); + + // Sensors + const colorSensor = new ColorSensor(chassis, render, config.colorSensor); + + const ultrasonicSensor = new UltrasonicSensor( + chassis, + physics, + render, + config.ultrasonicSensor, + ); + + const ev3: DefaultEv3 = new ControllerMap({ + ...wheelControllers, + ...motorControllers, + colorSensor, + ultrasonicSensor, + mesh, + chassis, + }); + + return ev3; +}; diff --git a/src/bundles/robot_simulation/controllers/ev3/ev3/default/types.ts b/src/bundles/robot_simulation/controllers/ev3/ev3/default/types.ts new file mode 100644 index 000000000..19b14d97d --- /dev/null +++ b/src/bundles/robot_simulation/controllers/ev3/ev3/default/types.ts @@ -0,0 +1,81 @@ +import type { SimpleVector } from '../../../../engine/Math/Vector'; +import type { ChassisWrapper, ChassisWrapperConfig } from '../../components/Chassis'; +import type { Mesh, MeshConfig } from '../../components/Mesh'; +import type { Motor, MotorConfig } from '../../components/Motor'; +import type { Wheel, WheelConfig } from '../../components/Wheel'; +import type { ColorSensor, ColorSensorConfig } from '../../sensor/ColorSensor'; +import type { UltrasonicSensor, UltrasonicSensorConfig } from '../../sensor/UltrasonicSensor'; + +// ######################### Controller Types ######################### +// Wheels +export const wheelNames = [ + 'frontLeftWheel', + 'frontRightWheel', + 'backLeftWheel', + 'backRightWheel', +] as const; +export type WheelNames = (typeof wheelNames)[number]; +export type WheelControllers = Record; + +// Motors +export const motorNames = ['leftMotor', 'rightMotor'] as const; +export type MotorNames = (typeof motorNames)[number]; +export type MotorControllers = Record; + +// Chassis +export const chassisNames = ['chassis'] as const; +export type ChassisNames = (typeof chassisNames)[number]; +export type ChassisControllers = Record; + +// Mesh +export const meshNames = ['mesh'] as const; +export type MeshNames = (typeof meshNames)[number]; +export type MeshControllers = Record; + +// ColorSensor +export const colorSensorNames = ['colorSensor'] as const; +export type ColorSensorNames = (typeof colorSensorNames)[number]; +export type ColorSensorControllers = Record; + +// UltrasonicSensor +export const ultrasonicSensorNames = ['ultrasonicSensor'] as const; +export type UltrasonicSensorNames = (typeof ultrasonicSensorNames)[number]; +export type UltrasonicSensorControllers = Record< + UltrasonicSensorNames, + UltrasonicSensor +>; + +// Aggregate +export const controllerNames = [ + ...wheelNames, + ...motorNames, + ...chassisNames, + ...meshNames, + ...colorSensorNames, + ...ultrasonicSensorNames, +] as const; +export type DefaultEv3ControllerNames = (typeof controllerNames)[number]; +export type DefaultEv3Controller = ChassisControllers & ColorSensorControllers & MeshControllers & MotorControllers & UltrasonicSensorControllers & WheelControllers; + +// ######################### Config Types ######################### +export type Ev3ChassisConfig = ChassisWrapperConfig; +export type Ev3MeshConfig = MeshConfig; +export type Ev3WheelsConfig = { + displacements: Record; + config: Omit; +}; +export type Ev3MotorsConfig = { + displacements: Record; + config: Omit; +}; +export type Ev3ColorSensorConfig = ColorSensorConfig; +export type Ev3UltrasonicSenorConfig = UltrasonicSensorConfig; + +export type Ev3Config = { + chassis: Ev3ChassisConfig + mesh: Ev3MeshConfig; + wheels: Ev3WheelsConfig; + motors: Ev3MotorsConfig; + colorSensor: Ev3ColorSensorConfig; + ultrasonicSensor: Ev3UltrasonicSenorConfig; +}; diff --git a/src/bundles/robot_simulation/controllers/ev3/feedback_control/PidController.ts b/src/bundles/robot_simulation/controllers/ev3/feedback_control/PidController.ts new file mode 100644 index 000000000..accc41d3c --- /dev/null +++ b/src/bundles/robot_simulation/controllers/ev3/feedback_control/PidController.ts @@ -0,0 +1,109 @@ +import * as THREE from 'three'; + +export type PIDConfig = { + proportionalGain: number; + integralGain: number; + derivativeGain: number; +}; + +type NullaryFunction = () => T; +type BinaryFunction = (a: T, b: T) => T; +type ScaleFunction = (value: T, scale: number) => T; + +type PIDControllerOptions = { + zero: NullaryFunction; + add: BinaryFunction; + subtract: BinaryFunction; + scale: ScaleFunction; + + proportionalGain: number; + integralGain: number; + derivativeGain: number; +}; + +class PIDController { + zero: NullaryFunction; + add: BinaryFunction; + subtract: BinaryFunction; + scale: ScaleFunction; + + proportionalGain: number; + integralGain: number; + derivativeGain: number; + + errorsSum: T; + previousError: T; + + constructor({ + zero, + add, + subtract, + scale, + proportionalGain, + integralGain, + derivativeGain, + }: PIDControllerOptions) { + this.zero = zero; + this.add = add; + this.subtract = subtract; + this.scale = scale; + + this.proportionalGain = proportionalGain; + this.integralGain = integralGain; + this.derivativeGain = derivativeGain; + + this.errorsSum = this.zero(); + this.previousError = this.zero(); + } + + calculate(currentValue: T, setpoint: T): T { + const error = this.subtract(setpoint, currentValue); + this.errorsSum = this.add(this.errorsSum, error); + + const proportional = this.scale(error, this.proportionalGain); + const integral = this.scale(this.errorsSum, this.integralGain); + const derivative = this.scale(this.subtract(error, this.previousError), this.derivativeGain); + this.previousError = error; + + return this.add(this.add(proportional, integral), derivative); + } +} + +export class NumberPidController extends PIDController { + constructor({ + proportionalGain, + integralGain, + derivativeGain, + }: PIDConfig) { + super({ + zero: () => 0, + add: (a, b) => a + b, + subtract: (a, b) => a - b, + scale: (value, scale) => value * scale, + proportionalGain, + integralGain, + derivativeGain, + }); + } +} + +export class VectorPidController extends PIDController { + constructor({ + proportionalGain, + integralGain, + derivativeGain, + }: PIDConfig) { + super({ + zero: () => new THREE.Vector3(0, 0, 0), + add: (a, b) => a.clone() + .add(b), + subtract: (a, b) => a.clone() + .sub(b), + scale: (value, scale) => value.clone() + .multiplyScalar(scale), + proportionalGain, + integralGain, + derivativeGain, + }); + } +} diff --git a/src/bundles/robot_simulation/controllers/ev3/sensor/ColorSensor.ts b/src/bundles/robot_simulation/controllers/ev3/sensor/ColorSensor.ts new file mode 100644 index 000000000..8beabfc1e --- /dev/null +++ b/src/bundles/robot_simulation/controllers/ev3/sensor/ColorSensor.ts @@ -0,0 +1,159 @@ +import * as THREE from 'three'; +import { Renderer } from '../../../engine'; +import { vec3 } from '../../../engine/Math/Convert'; +import { type SimpleVector } from '../../../engine/Math/Vector'; +import type { PhysicsTimingInfo } from '../../../engine/Physics'; +import { + getCamera, + type CameraOptions, +} from '../../../engine/Render/helpers/Camera'; +import { type ChassisWrapper } from '../components/Chassis'; +import { type Sensor } from './types'; + +type Color = { r: number; g: number; b: number }; + +export type ColorSensorConfig = { + size: { + height: number; + width: number; + }; + displacement: SimpleVector; + camera: CameraOptions; + tickRateInSeconds: number; + debug: boolean; +}; + +export class ColorSensor implements Sensor { + chassisWrapper: ChassisWrapper; + displacement: THREE.Vector3; + config: ColorSensorConfig; + + camera: THREE.Camera; + spotLight: THREE.SpotLight; + renderer: Renderer; + accumulator = 0; + colorSensed: Color; + tempCanvas: HTMLCanvasElement; + + constructor( + chassisWrapper: ChassisWrapper, + render: Renderer, + config: ColorSensorConfig, + ) { + this.chassisWrapper = chassisWrapper; + this.displacement = vec3(config.displacement); + this.config = config; + + this.camera = getCamera(config.camera); + this.spotLight = new THREE.SpotLight(0xffffff, 0.2, 1, 15/180 * Math.PI); + render.add(this.spotLight); + // We create a new renderer with the same scene. But we use a different camera. + this.renderer = new Renderer(render.scene(), this.camera, { + width: this.config.size.width, + height: this.config.size.height, + control: 'none', + }); + + this.colorSensed = { + r: 0, + g: 0, + b: 0, + }; + + this.tempCanvas = document.createElement('canvas'); + this.tempCanvas.width = this.config.size.width; + this.tempCanvas.height = this.config.size.height; + + if (config.debug) { + const helper = new THREE.CameraHelper(this.camera); + render.add(helper); + } + } + + getColorSensorPosition() { + const chassis = this.chassisWrapper.getEntity(); + const colorSensorPosition = chassis.worldTranslation( + this.displacement.clone(), + ); + return colorSensorPosition; + } + + sense(): Color { + return this.colorSensed; + } + + // Even though we are rendering, we use fixedUpdate because the student's code can be affected + // by the values of sense() and could affect the determinism of the simulation. + fixedUpdate(timingInfo: PhysicsTimingInfo) { + this.accumulator += timingInfo.timestep; + + const tickRateInMilliseconds = this.config.tickRateInSeconds * 1000; + + // We check the accumulator to see if it's time update the color sensor. + // If it's not time, we return early. + if (this.accumulator < tickRateInMilliseconds) { + return; + } + this.accumulator -= tickRateInMilliseconds; + + // We move the camera to the right position + this.camera.position.copy(this.getColorSensorPosition()); + this.camera.lookAt( + this.camera.position.x, + this.camera.position.y - 1, // 1 unit below its current position + this.camera.position.z, + ); + this.spotLight.position.copy(this.camera.position); + this.spotLight.target.position.set( + this.camera.position.x, + this.camera.position.y - 1, + this.camera.position.z, + ); + this.spotLight.target.updateMatrixWorld(); + + // We render to load the color sensor data into the renderer. + this.renderer.render(); + + // We get the HTMLCanvasElement from the renderer + const rendererCanvas = this.renderer.getElement(); + + // Get the context from the temp canvas + const tempCtx = this.tempCanvas.getContext('2d', { + willReadFrequently: true, + })!; + + // Draw the renderer canvas to the temp canvas + tempCtx.drawImage(rendererCanvas, 0, 0); + + // Get the image data from the temp canvas + const imageData = tempCtx.getImageData( + 0, + 0, + this.config.size.width, + this.config.size.height, + {}, + ); + + // Calculate the average color + const averageColor = { + r: 0, + g: 0, + b: 0, + }; + + for (let i = 0; i < imageData.data.length; i += 4) { + const r = imageData.data[i]; + const g = imageData.data[i + 1]; + const b = imageData.data[i + 2]; + averageColor.r += r; + averageColor.g += g; + averageColor.b += b; + } + + averageColor.r /= imageData.data.length; + averageColor.g /= imageData.data.length; + averageColor.b /= imageData.data.length; + + this.colorSensed = averageColor; + } +} diff --git a/src/bundles/robot_simulation/controllers/ev3/sensor/UltrasonicSensor.ts b/src/bundles/robot_simulation/controllers/ev3/sensor/UltrasonicSensor.ts new file mode 100644 index 000000000..05ae2aecb --- /dev/null +++ b/src/bundles/robot_simulation/controllers/ev3/sensor/UltrasonicSensor.ts @@ -0,0 +1,78 @@ +import * as THREE from 'three'; +import { vec3 } from '../../../engine/Math/Convert'; +import { type SimpleVector } from '../../../engine/Math/Vector'; +import {type Physics } from '../../../engine/Physics'; +import { type Renderer } from '../../../engine/Render/Renderer'; +import { type ChassisWrapper } from '../components/Chassis'; +import { type Sensor } from './types'; + +export type UltrasonicSensorConfig = { + displacement: SimpleVector; + direction: SimpleVector; + debug: boolean; +}; + +export class UltrasonicSensor implements Sensor { + chassisWrapper: ChassisWrapper; + physics: Physics; + displacement: THREE.Vector3; + direction: THREE.Vector3; + distanceSensed: number = 0; + render: Renderer; + config: UltrasonicSensorConfig; + debugArrow: THREE.ArrowHelper; + + constructor( + chassis: ChassisWrapper, + physics: Physics, + render: Renderer, + config: UltrasonicSensorConfig, + ) { + this.chassisWrapper = chassis; + this.physics = physics; + this.render = render; + this.displacement = vec3(config.displacement); + this.direction = vec3(config.direction); + this.config = config; + + // Debug arrow + this.debugArrow = new THREE.ArrowHelper(); + this.debugArrow.visible = false; + this.render.add(this.debugArrow); + } + + sense(): number { + return this.distanceSensed * 100; + } + + fixedUpdate(): void { + const chassis = this.chassisWrapper.getEntity(); + const globalDisplacement = chassis.worldTranslation( + this.displacement.clone(), + ); + const globalDirection = chassis.transformDirection(this.direction.clone()); + + const result = this.physics.castRay( + globalDisplacement, + globalDirection, + 1, + this.chassisWrapper.getEntity() + .getCollider(), + ); + + if (this.config.debug) { + this.debugArrow.visible = true; + this.debugArrow.position.copy(globalDisplacement); + this.debugArrow.setDirection(globalDirection.normalize()); + } + + if (result === null) { + return; + } + + const { distance: wheelDistance } = result; + + this.distanceSensed = wheelDistance; + } + +} diff --git a/src/bundles/robot_simulation/controllers/ev3/sensor/types.ts b/src/bundles/robot_simulation/controllers/ev3/sensor/types.ts new file mode 100644 index 000000000..6cd0cc8df --- /dev/null +++ b/src/bundles/robot_simulation/controllers/ev3/sensor/types.ts @@ -0,0 +1,3 @@ +import { type Controller } from '../../../engine'; + +export type Sensor = Controller & { sense: () => T }; diff --git a/src/bundles/robot_simulation/controllers/index.ts b/src/bundles/robot_simulation/controllers/index.ts new file mode 100644 index 000000000..778141b54 --- /dev/null +++ b/src/bundles/robot_simulation/controllers/index.ts @@ -0,0 +1 @@ +export { type DefaultEv3 } from './ev3/ev3/default/ev3'; diff --git a/src/bundles/robot_simulation/controllers/program/Program.ts b/src/bundles/robot_simulation/controllers/program/Program.ts new file mode 100644 index 000000000..a6fa5d5cc --- /dev/null +++ b/src/bundles/robot_simulation/controllers/program/Program.ts @@ -0,0 +1,83 @@ +import { type IOptions } from 'js-slang'; +import context from 'js-slang/context'; +import type { DeepPartial } from '../../../../common/deepPartial'; +import { CallbackHandler } from '../../engine/Core/CallbackHandler'; +import { type Controller } from '../../engine/Core/Controller'; +import type { PhysicsTimingInfo } from '../../engine/Physics'; +import { mergeConfig } from '../utils/mergeConfig'; +import { ProgramError } from './error'; +import { runECEvaluator } from './evaluate'; + +type ProgramConfig = { + stepsPerTick: number; +}; + +const defaultProgramConfig: ProgramConfig = { + stepsPerTick: 11, +}; + +export const program_controller_identifier = 'program_controller'; + +export class Program implements Controller { + code: string; + iterator: ReturnType | null; + isPaused: boolean; + callbackHandler = new CallbackHandler(); + name: string; + config: ProgramConfig; + + constructor(code: string, config?: DeepPartial) { + this.config = mergeConfig(defaultProgramConfig, config); + this.name = program_controller_identifier; + this.code = code; + this.iterator = null; + this.isPaused = false; + } + + pause(pauseDuration: number) { + this.isPaused = true; + this.callbackHandler.addCallback(() => { + this.isPaused = false; + }, pauseDuration); + } + + start() { + const options: Partial = { + originalMaxExecTime: Infinity, + scheduler: 'preemptive', + stepLimit: Infinity, + throwInfiniteLoops: false, + useSubst: false, + }; + + context.errors = []; + + this.iterator = runECEvaluator(this.code, context, options); + } + + fixedUpdate() { + try { + if (!this.iterator) { + throw Error('Program not started'); + } + + if (this.isPaused) { + return; + } + + // steps per tick + for (let i = 0; i < this.config.stepsPerTick; i++) { + const result = this.iterator.next(); + } + } catch (e) { + console.error(e); + throw new ProgramError( + 'Error in program execution. Please check your code and try again.', + ); + } + } + + update(frameTiming: PhysicsTimingInfo): void { + this.callbackHandler.checkCallbacks(frameTiming); + } +} diff --git a/src/bundles/robot_simulation/controllers/program/evaluate.ts b/src/bundles/robot_simulation/controllers/program/evaluate.ts new file mode 100644 index 000000000..4107595ed --- /dev/null +++ b/src/bundles/robot_simulation/controllers/program/evaluate.ts @@ -0,0 +1,58 @@ +import { + Control, + Stash, + generateCSEMachineStateStream, +} from 'js-slang/dist/cse-machine/interpreter'; +import { parse } from 'js-slang/dist/parser/parser'; +import { Variant, type Context } from 'js-slang/dist/types'; +import * as _ from 'lodash'; + +export const DEFAULT_SOURCE_OPTIONS = { + scheduler: 'async', + steps: 1000, + stepLimit: -1, + executionMethod: 'auto', + variant: Variant.DEFAULT, + originalMaxExecTime: 1000, + useSubst: false, + isPrelude: false, + throwInfiniteLoops: true, + envSteps: -1, + importOptions: { + wrapSourceModules: true, + checkImports: true, + loadTabs: true, + }, +}; + +export function* runECEvaluator( + code: string, + context: Context, + options: any +): Generator<{ steps: number }, void, undefined> { + const theOptions = _.merge({ ...DEFAULT_SOURCE_OPTIONS }, options); + const program = parse(code, context); + + if (!program) { + return; + } + + try { + context.runtime.isRunning = true; + context.runtime.control = new Control(program); + context.runtime.stash = new Stash(); + yield* generateCSEMachineStateStream( + context, + context.runtime.control, + context.runtime.stash, + theOptions.envSteps, + theOptions.stepLimit, + theOptions.isPrelude + ); + // eslint-disable-next-line no-useless-catch + } catch (error) { + throw error; + } finally { + context.runtime.isRunning = false; + } +} diff --git a/src/bundles/robot_simulation/engine/Physics.ts b/src/bundles/robot_simulation/engine/Physics.ts index 098edbea1..0ed38a07d 100644 --- a/src/bundles/robot_simulation/engine/Physics.ts +++ b/src/bundles/robot_simulation/engine/Physics.ts @@ -9,6 +9,7 @@ import { type SimpleVector } from './Math/Vector'; export type PhysicsTimingInfo = FrameTimingInfo & { stepCount: number; timestep: number; + residualFactor: number; }; export class TimeStampedEvent extends Event { @@ -67,7 +68,7 @@ export class Physics extends TypedEventTarget { this.internals = { initialized: true, world, - accumulator: 0, + accumulator: world.timestep, stepCount: 0, }; } @@ -134,15 +135,16 @@ export class Physics extends TypedEventTarget { const maxFrameTime = 0.05; const frameDuration = timing.frameDuration / 1000; - this.internals.accumulator -= Math.min(frameDuration, maxFrameTime); + this.internals.accumulator += Math.min(frameDuration, maxFrameTime); const currentPhysicsTimingInfo = { ...timing, stepCount: this.internals.stepCount, timestep: this.configuration.timestep * 1000, + residualFactor: this.internals.accumulator / this.configuration.timestep, }; - while (this.internals.accumulator <= 0) { + while (this.internals.accumulator >= this.configuration.timestep) { this.dispatchEvent( 'beforePhysicsUpdate', new TimeStampedEvent('beforePhysicsUpdate', currentPhysicsTimingInfo), @@ -158,7 +160,8 @@ export class Physics extends TypedEventTarget { new TimeStampedEvent('afterPhysicsUpdate', currentPhysicsTimingInfo), ); - this.internals.accumulator += this.configuration.timestep; + this.internals.accumulator -= this.configuration.timestep; + currentPhysicsTimingInfo.residualFactor = this.internals.accumulator / this.configuration.timestep; } return currentPhysicsTimingInfo; diff --git a/src/bundles/robot_simulation/engine/Render/Renderer.ts b/src/bundles/robot_simulation/engine/Render/Renderer.ts index fdee56c98..223487612 100644 --- a/src/bundles/robot_simulation/engine/Render/Renderer.ts +++ b/src/bundles/robot_simulation/engine/Render/Renderer.ts @@ -1,7 +1,5 @@ -/* eslint-disable import/extensions */ import * as THREE from 'three'; import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js'; - import { type GLTF, GLTFLoader, diff --git a/src/bundles/robot_simulation/engine/Render/helpers/GLTF.ts b/src/bundles/robot_simulation/engine/Render/helpers/GLTF.ts index 10b86918e..773068a3d 100644 --- a/src/bundles/robot_simulation/engine/Render/helpers/GLTF.ts +++ b/src/bundles/robot_simulation/engine/Render/helpers/GLTF.ts @@ -1,5 +1,4 @@ import * as THREE from 'three'; -// eslint-disable-next-line import/extensions import { type GLTF, GLTFLoader } from 'three/examples/jsm/loaders/GLTFLoader.js'; import type { Dimension } from '../../Math/Vector'; diff --git a/src/bundles/robot_simulation/engine/__tests__/Physics.ts b/src/bundles/robot_simulation/engine/__tests__/Physics.ts index 32d201e99..8530350c2 100644 --- a/src/bundles/robot_simulation/engine/__tests__/Physics.ts +++ b/src/bundles/robot_simulation/engine/__tests__/Physics.ts @@ -37,7 +37,7 @@ describe('Physics', () => { expect(rapier.init).toHaveBeenCalled(); expect(physics.internals).toHaveProperty('initialized', true); expect(physics.internals).toHaveProperty('world'); - expect(physics.internals).toHaveProperty('accumulator', 0); + expect(physics.internals).toHaveProperty('accumulator', physics.configuration.timestep); }); test('createRigidBody throws if not initialized', () => { diff --git a/src/bundles/robot_simulation/ev3_functions.ts b/src/bundles/robot_simulation/ev3_functions.ts new file mode 100644 index 000000000..60601975e --- /dev/null +++ b/src/bundles/robot_simulation/ev3_functions.ts @@ -0,0 +1,242 @@ +/* eslint-disable @typescript-eslint/naming-convention */ + +import { type Motor } from './controllers/ev3/components/Motor'; +import { motorConfig } from './controllers/ev3/ev3/default/config'; +import { type ColorSensor } from './controllers/ev3/sensor/ColorSensor'; +import { type UltrasonicSensor } from './controllers/ev3/sensor/UltrasonicSensor'; +import { + program_controller_identifier, + type Program, +} from './controllers/program/Program'; + +import { getEv3FromContext, getWorldFromContext } from './helper_functions'; + +type MotorFunctionReturnType = Motor | null; + +/** + * @categoryDescription EV3 + * These functions are mocking the the normal EV3 functions found + * at https://docs.sourceacademy.org/EV3/global.html + * @module robot_simulation + */ + +/** + * Pauses for a period of time. + * + * @param duration The time to wait, in milliseconds. + * + * @category EV3 + */ +export function ev3_pause(duration: number): void { + const world = getWorldFromContext(); + const program = world.controllers.controllers.find( + (controller) => controller.name === program_controller_identifier + ) as Program; + program.pause(duration); +} + +/** + * Gets the motor connected to port A. + * + * @returns The motor connected to port A + * + * @category EV3 + */ +export function ev3_motorA(): MotorFunctionReturnType { + const ev3 = getEv3FromContext(); + return ev3.get('leftMotor'); +} + +/** + * Gets the motor connected to port B. + * + * @returns The motor connected to port B + * + * @category EV3 + */ +export function ev3_motorB(): MotorFunctionReturnType { + const ev3 = getEv3FromContext(); + return ev3.get('rightMotor'); +} + +/** + * Gets the motor connected to port C. + * + * @returns The motor connected to port C + * + * @category EV3 + */ +export function ev3_motorC(): MotorFunctionReturnType { + return null; +} + +/** + * Gets the motor connected to port D. + * + * @returns The motor connected to port D + * + * @category EV3 + */ +export function ev3_motorD(): MotorFunctionReturnType { + return null; +} + +/** + * Causes the motor to rotate until the position reaches ev3_motorGetPosition() + position with the given speed. + * Note: this works by sending instructions to the motors. + * This will return almost immediately, without waiting for the motor to reach the given absolute position. + * If you wish to wait, use ev3_pause. + * + * @param motor The motor + * @param position The amount to turn + * @param speed The speed to run at, in tacho counts per second + * + * @category EV3 + */ +export function ev3_runToRelativePosition( + motor: MotorFunctionReturnType, + position: number, + speed: number +): void { + if (motor === null) { + return; + } + + const wheelDiameter = motorConfig.config.mesh.dimension.height; + const speedInMetersPerSecond = (speed / 360) * Math.PI * wheelDiameter; + const distanceInMetersPerSecond = (position / 360) * Math.PI * wheelDiameter; + + motor.setSpeedDistance(speedInMetersPerSecond, distanceInMetersPerSecond); +} + +/** + * Causes the motor to rotate for a specified duration at the specified speed. + * + * Note: this works by sending instructions to the motors. This will return almost immediately, + * without waiting for the motor to actually run for the specified duration. + * If you wish to wait, use ev3_pause. + * + * @param motor + * @param time + * @param speed + * @returns void + * + * @category EV3 + */ +export function ev3_runForTime( + motor: MotorFunctionReturnType, + time: number, + speed: number +) { + if (motor === null) { + return; + } + const wheelDiameter = motorConfig.config.mesh.dimension.height; + const speedInMetersPerSecond = (speed / 360) * Math.PI * wheelDiameter; + const distanceInMetersPerSecond = speedInMetersPerSecond * time; + + motor.setSpeedDistance(speedInMetersPerSecond, distanceInMetersPerSecond); +} + +/** + * Gets the motor's current speed, in tacho counts per second. + * + * Returns 0 if the motor is not connected. + * + * @param motor + * @returns number + * + * @category EV3 + */ +export function ev3_motorGetSpeed(motor: MotorFunctionReturnType): number { + if (motor === null) { + return 0; + } + + return motor.motorVelocity; +} + +/** + * Gets the colour sensor connected any of ports 1, 2, 3 or 4. + * + * @returns The colour sensor + * + * @category EV3 + */ +export function ev3_colorSensor() { + const ev3 = getEv3FromContext(); + return ev3.get('colorSensor'); +} + +/** + * Gets the amount of red seen by the colour sensor. + * + * @param colorSensor The color sensor + * @returns The amount of blue, in sensor-specific units. + * + * @category EV3 + */ +export function ev3_colorSensorRed(colorSensor: ColorSensor) { + return colorSensor.sense().r; +} + +/** + * Gets the amount of green seen by the colour sensor. + * + * @param colorSensor The color sensor + * @returns The amount of green, in sensor-specific units. + * + * @category EV3 + */ +export function ev3_colorSensorGreen(colorSensor: ColorSensor) { + return colorSensor.sense().g; +} + +/** + * Gets the amount of blue seen by the colour sensor. + * + * @param colorSensor The color sensor + * @returns The amount of blue, in sensor-specific units. + * + * @category EV3 + */ +export function ev3_colorSensorBlue(colorSensor: ColorSensor) { + return colorSensor.sense().b; +} + +/** + * Gets the ultrasonic sensor connected any of ports 1, 2, 3 or 4. + * + * @returns The ultrasonic sensor + * + * @category EV3 + */ +export function ev3_ultrasonicSensor() { + const ev3 = getEv3FromContext(); + return ev3.get('ultrasonicSensor'); +} +/** + * Gets the distance read by the ultrasonic sensor in centimeters. + * + * @param ultraSonicSensor The ultrasonic sensor + * @returns The distance, in centimeters. + * + * @category EV3 + */ +export function ev3_ultrasonicSensorDistance( + ultraSonicSensor: UltrasonicSensor +): number { + return ultraSonicSensor.sense(); +} + +/** + * Checks if the peripheral is connected. + * + * @param obj The peripheral to check. + * @returns boolean + * + * @category EV3 + */ +export function ev3_connected(obj: any) { + return obj !== null; +} diff --git a/src/bundles/robot_simulation/helper_functions.ts b/src/bundles/robot_simulation/helper_functions.ts index f898da155..d2f2aa213 100644 --- a/src/bundles/robot_simulation/helper_functions.ts +++ b/src/bundles/robot_simulation/helper_functions.ts @@ -2,18 +2,37 @@ import context from 'js-slang/context'; import { interrupt } from '../../common/specialErrors'; import { sceneConfig } from './config'; import { Cuboid, type CuboidConfig } from './controllers/environment/Cuboid'; +import { Paper, type PaperConfig } from './controllers/environment/Paper'; +import { ev3Config } from './controllers/ev3/ev3/default/config'; +import { + createDefaultEv3, + type DefaultEv3, +} from './controllers/ev3/ev3/default/ev3'; +import { Program } from './controllers/program/Program'; import { type Controller, Physics, Renderer, Timer, World } from './engine'; import { RobotConsole } from './engine/Core/RobotConsole'; -import { isRigidBodyType, type RigidBodyType } from './engine/Entity/EntityFactory'; +import { + isRigidBodyType, + type RigidBodyType, +} from './engine/Entity/EntityFactory'; import type { PhysicsConfig } from './engine/Physics'; import type { RenderConfig } from './engine/Render/Renderer'; import { getCamera, type CameraOptions } from './engine/Render/helpers/Camera'; import { createScene } from './engine/Render/helpers/Scene'; -const storedWorld = context.moduleContexts.robot_simulation.state?.world; +/** + * @categoryDescription Configuration + * These functions are use to configure the simulation world. + * @module + */ -// Helper functions to get objects from context +/** + * A helper function that retrieves the world from the context + * + * @private + * @category helper + */ export function getWorldFromContext(): World { const world = context.moduleContexts.robot_simulation.state?.world; if (world === undefined) { @@ -22,10 +41,37 @@ export function getWorldFromContext(): World { return world as World; } -// Physics +/** + * A helper function that retrieves the EV3 from context + * + * @private + * @category helper + */ +export function getEv3FromContext(): DefaultEv3 { + const ev3 = context.moduleContexts.robot_simulation.state?.ev3; + if (ev3 === undefined) { + throw new Error('ev3 not initialized'); + } + return ev3 as DefaultEv3; +} + +/** + * Create a physics engine with the provided gravity and timestep. A physics engine + * with default gravity and timestep can be created using {@link createPhysics}. + * + * The returned Physics object is designed to be passed into {@link createWorld}. + * + * **This is a configuration function and should be called within {@link init_simulation}.** + * + * @param gravity The gravity of the world + * @param timestep The timestep of the world + * @returns Physics + * + * @category Configuration + */ export function createCustomPhysics( gravity: number, - timestep: number, + timestep: number ): Physics { const physicsConfig: PhysicsConfig = { gravity: { @@ -39,11 +85,33 @@ export function createCustomPhysics( return physics; } +/** + * Create a physics engine with default gravity and timestep. Default gravity is -9.81 and timestep is 1/20. + * A custom physics engine can be created using {@link createCustomPhysics}. + * + * The returned Physics object is designed to be passed into {@link createWorld}. + * + * **This is a configuration function and should be called within {@link init_simulation}.** + * + * @returns Physics + * + * @category Configuration + */ export function createPhysics(): Physics { return createCustomPhysics(-9.81, 1 / 20); } -// Renderer +/** + * Creates a renderer for the simulation. + * + * The returned Renderer object is designed to be passed into {@link createWorld}. + * + * **This is a configuration function and should be called within {@link init_simulation}.** + * + * @returns Renderer + * + * @category Configuration + */ export function createRenderer(): Renderer { const sceneCameraOptions: CameraOptions = { type: 'perspective', @@ -65,30 +133,126 @@ export function createRenderer(): Renderer { return renderer; } -// Timer +/** + * Creates a Timer for the simulation. + * + * The returned Timer object is designed to be passed into {@link createWorld}. + * + * **This is a configuration function and should be called within {@link init_simulation}.** + * + * @returns Timer + * + * @category Configuration + */ export function createTimer(): Timer { const timer = new Timer(); return timer; } -// Robot console -export function createRobotConsole() { +/** + * Creates a RobotConsole for the simulation. + * + * The RobotConsole is used to display messages and errors to the user. The console + * messages can be seen in the console tab of the simulator. + * + * The returned RobotConsole object is designed to be passed into {@link createWorld}. + * + * **This is a configuration function and should be called within {@link init_simulation}.** + * + * @returns RobotConsole + * + * @category Configuration + */ +export function createRobotConsole(): RobotConsole { const robot_console = new RobotConsole(); return robot_console; } -// Create world +/** + * Creates a custom world with the provided {@link createPhysics | physics}, {@link createRenderer | renderer}, {@link createTimer | timer} and {@link createRobotConsole | console} . + * + * A world is responsible for managing the physics, rendering, timing and console of the simulation. + * It also manages the controllers that are added to the world, ensuring that the appropriate functions + * are called at the correct time. + * + * The returned World object is designed to be returned by the {@link init_simulation} callback. + * + * You can add controllers to the world using {@link addControllerToWorld}. + * + * **This is a configuration function and should be called within {@link init_simulation}.** + * + * @example + * An empty simulation + * ``` + * init_simulation(() => { + * const physics = createPhysics(); + * const renderer = createRenderer(); + * const timer = createTimer(); + * const robot_console = createRobotConsole(); + * const world = createWorld(physics, renderer, timer, robot_console); + * + * return world; + * }); + * ``` + * + * @param physics The physics engine of the world. See {@link createPhysics} + * @param renderer The renderer engine of the world. See {@link createRenderer} + * @param timer The timer of the world. See {@link createTimer} + * @param robotConsole The console of the world. See {@link createRobotConsole} + * @returns World + * + * @category Configuration + */ export function createWorld( physics: Physics, renderer: Renderer, timer: Timer, - robotConsole: RobotConsole, -) { + robotConsole: RobotConsole +): World { const world = new World(physics, renderer, timer, robotConsole); return world; } -// Environment +/** + * Creates a cuboid. joel-todo: The dynamic version wont work + * + * This function is used to create the {@link createFloor | floor} and {@link createWall | wall} controllers. + * + * The returned Cuboid object is designed to be added to the world using {@link addControllerToWorld}. + * + * **This is a Controller function and should be called within {@link init_simulation}.** + * + * @param physics The physics engine passed to the world + * @param renderer The renderer engine of the world. See {@link createRenderer} + * @param position_x The x position of the cuboid + * @param position_y The y position of the cuboid + * @param position_z The z position of the cuboid + * @param width The width of the cuboid in meters + * @param length The length of the cuboid in meters + * @param height The height of the cuboid in meters + * @param mass The mass of the cuboid in kg + * @param color The color of the cuboid. Can be a hex code or a string. {@see https://threejs.org/docs/#api/en/math/Color} + * @param bodyType "rigid" or "dynamic". Determines if the cuboid is fixed or can move. + * @returns Cuboid + * + * @example + * ``` + * init_simulation(() => { + * const physics = createPhysics(); + * const renderer = createRenderer(); + * const timer = createTimer(); + * const robot_console = createRobotConsole(); + * const world = createWorld(physics, renderer, timer, robot_console); + * + * const cuboid = createCuboid(...); + * addControllerToWorld(cuboid, world); + * + * return world; + * }); + * ``` + * + * @category Controller + */ export function createCuboid( physics: Physics, renderer: Renderer, @@ -100,7 +264,7 @@ export function createCuboid( height: number, mass: number, color: number | string, - bodyType: string, + bodyType: string ) { if (isRigidBodyType(bodyType) === false) { throw new Error('Invalid body type'); @@ -128,6 +292,19 @@ export function createCuboid( return cuboid; } +/** + * Create a floor. This function is a wrapper around {@link createCuboid}. + * + * The returned Cuboid object is designed to be added to the world using {@link addControllerToWorld}. + * + * **This is a Controller function and should be called within {@link init_simulation}.** + * + * @param physics The physics engine of the world. See {@link createPhysics} + * @param renderer The renderer engine of the world. See {@link createRenderer} + * @returns Cuboid + * + * @category Controller + */ export function createFloor(physics: Physics, renderer: Renderer) { const floor = createCuboid( physics, @@ -140,32 +317,148 @@ export function createFloor(physics: Physics, renderer: Renderer) { 1, // height 1, // mass 'white', // color - 'fixed', // bodyType + 'fixed' // bodyType ); return floor; } -export function createWall(physics: Physics, renderer: Renderer) { +/** + * Creates a wall. This function is a wrapper around {@link createCuboid}. + * + * The returned Cuboid object is designed to be added to the world using {@link addControllerToWorld}. + * + * **This is a Controller function and should be called within {@link init_simulation}.** + * + * @param physics The physics engine of the world. See {@link createPhysics} + * @param renderer The renderer engine of the world. See {@link createRenderer} + * @param x The x position of the wall + * @param y The y position of the wall + * @param width The width of the wall in meters + * @param length The length of the wall in meters + * @param height The height of the wall in meters + * @returns Cuboid + * + * @category Controller + */ +export function createWall( + physics: Physics, + renderer: Renderer, + x: number, + y: number, + width: number, + length: number, + height: number +) { const wall = createCuboid( physics, renderer, - 0, // position_x - 1, // position_y - 1, // position_z - 1, // width - 0.1, // length - 2, // height + x, // position_x + height / 2, + y, // position_y + width, // width + length, // length + height, // height 1, // mass 'yellow', // color - 'fixed', // bodyType + 'fixed' // bodyType ); return wall; } +/** + * Creates a paper on the floor. + * + * The returned Paper object is designed to be added to the world using {@link addControllerToWorld}. + * + * **This is a Controller function and should be called within {@link init_simulation}.** + * + * @param render The renderer engine of the world. See {@link createRenderer} + * @param url The url of the image to be displayed on the paper. + * @param width The width of the paper in meters. + * @param height The height of the paper in meters. + * @param x The x position of the paper. + * @param y The y position of the paper. + * @param rotation The rotation of the paper in degrees. + * + * @returns Paper + * + * @category Controller + */ +export function createPaper( + render: Renderer, + url: string, + width: number, + height: number, + x: number, + y: number, + rotation: number +) { + const paperConfig: PaperConfig = { + url, + dimension: { + width, + height, + }, + position: { x, y }, + rotation: (rotation * Math.PI) / 180, + }; + const paper = new Paper(render, paperConfig); + return paper; +} + +/** + * Creates a CSE machine as a Program Object. The CSE machine is used to evaluate the code written + * by the user. The execution of the code will be automatically synchronized with the simulation + * to ensure that the code is executed at the correct time. + * + * The returned Program object is designed to be added to the world using {@link addControllerToWorld}. + * + * **This is a Controller function and should be called within {@link init_simulation}.** + * + * @returns Program + * + * @category Controller + * + */ +export function createCSE() { + const code = context.unTypecheckedCode[0]; + const program = new Program(code); + return program; +} + +/** + * Add a controller to the world. + * + * The controller is a unit of computation modelled after Unity's MonoBehaviour. It is used to + * encapsulate the logic of the simulation. Controllers can be used to create robots, sensors, + * actuators, and other objects in the simulation. + * + * The controller should be added to the world using this function in order for the simulation to + * access the controller's logic. + * + * **This is a Utility function and should be called within {@link init_simulation}.* + * + * @param controller + * @param world + * + * @category Utility + */ export function addControllerToWorld(controller: Controller, world: World) { world.addController(controller); } +/** + * Save a value to the context. + * + * There are 2 important values to be saved. The world and the ev3. + * The world needs to be saved in order for the simulation to access the physics, renderer, timer and console. + * The ev3 needs to be saved in order for the "ev3_" functions to access the EV3 + * + * @param key The key to save the value as + * @param value The value to save + * + * @returns void + */ export function saveToContext(key: string, value: any) { if (!context.moduleContexts.robot_simulation.state) { context.moduleContexts.robot_simulation.state = {}; @@ -173,8 +466,47 @@ export function saveToContext(key: string, value: any) { context.moduleContexts.robot_simulation.state[key] = value; } -// Initialization +/** + * Create an EV3. + * + * The resulting EV3 should be saved to the context using {@link saveToContext}. + * + * The returned EV3 object is designed to be added to the world using {@link addControllerToWorld}. + * + * **This is a Controller function and should be called within {@link init_simulation}.** + * + * @example + * ``` + * init_simulation(() => { + * ... + * const ev3 = createEv3(physics, renderer); + * saveToContext('ev3', ev3); + * }) + * ``` + * + * @param physics The physics engine of the world. See {@link createPhysics} + * @param renderer The renderer engine of the world. See {@link createRenderer} + * @returns EV3 + */ +export function createEv3(physics: Physics, renderer: Renderer): DefaultEv3 { + const ev3 = createDefaultEv3(physics, renderer, ev3Config); + return ev3; +} + +/** + * Initialize the simulation world. This function is to be called before the robot code. + * This function is used to describe the simulation environment and the controllers. + * + * The callback function takes in no parameters and returns a world created by {@link createWorld}. + * The world should be configured with the physics, renderer, timer and console. + * The controllers should be added to the world using {@link addControllerToWorld}. + * The world should be saved to the context using {@link saveToContext}. + * + * @param worldFactory A callback function that returns the world object. Type signature: () => World + * @returns void + */ export function init_simulation(worldFactory: () => World) { + const storedWorld = context.moduleContexts.robot_simulation.state?.world; if (storedWorld !== undefined) { return; } diff --git a/src/bundles/robot_simulation/index.ts b/src/bundles/robot_simulation/index.ts index 862f33518..d74463c62 100644 --- a/src/bundles/robot_simulation/index.ts +++ b/src/bundles/robot_simulation/index.ts @@ -5,6 +5,21 @@ * @author Joel Chan */ +export { + ev3_motorA, + ev3_motorB, + ev3_motorC, + ev3_motorD, + ev3_runToRelativePosition, + ev3_colorSensorRed, + ev3_colorSensorGreen, + ev3_pause, + ev3_colorSensor, + ev3_colorSensorBlue, + ev3_ultrasonicSensor, + ev3_ultrasonicSensorDistance, +} from './ev3_functions'; + export { createCustomPhysics, createPhysics, @@ -14,7 +29,10 @@ export { createTimer, createWorld, createWall, + createEv3, + createPaper, createFloor, + createCSE, addControllerToWorld, createRobotConsole, saveToContext, diff --git a/src/jest.config.js b/src/jest.config.js index c490f46d0..c67898054 100644 --- a/src/jest.config.js +++ b/src/jest.config.js @@ -20,7 +20,8 @@ export default { moduleNameMapper: { // Module Name settings required to make chalk work with jest '#(.*)': '/node_modules/$1', - '^js-slang/context': '/__mocks__/context.ts' + '^js-slang/context': '/__mocks__/context.ts', + '^three.+.js': '/__mocks__/emptyModule.js', // 'lowdb': '/node_modules/lowdb/lib', // 'steno': '/node_modules/steno/lib', }, diff --git a/src/tabs/RobotSimulation/components/Simulation/index.tsx b/src/tabs/RobotSimulation/components/Simulation/index.tsx index 77b5bb579..4726003fc 100644 --- a/src/tabs/RobotSimulation/components/Simulation/index.tsx +++ b/src/tabs/RobotSimulation/components/Simulation/index.tsx @@ -1,10 +1,17 @@ -import { Tabs } from '@blueprintjs/core'; +import { Tab, Tabs } from '@blueprintjs/core'; import { useRef, type CSSProperties, useEffect, useState } from 'react'; +import { type DefaultEv3 } from '../../../../bundles/robot_simulation/controllers'; import { type World } from '../../../../bundles/robot_simulation/engine'; import { type WorldState } from '../../../../bundles/robot_simulation/engine/World'; import type { DebuggerContext } from '../../../../typings/type_helpers'; +import { ColorSensorPanel } from '../TabPanels/ColorSensorPanel'; +import { ConsolePanel } from '../TabPanels/ConsolePanel'; +import { MotorPidPanel } from '../TabPanels/MotorPidPanel'; +import { UltrasonicSensorPanel } from '../TabPanels/UltrasonicSensorPanel'; +import { WheelPidPanel } from '../TabPanels/WheelPidPanel'; + const WrapperStyle: CSSProperties = { display: 'flex', flexDirection: 'column', @@ -38,10 +45,17 @@ export const SimulationCanvas: React.FC = ({ isOpen, }) => { const ref = useRef(null); - const [currentState, setCurrentState] = useState('unintialized'); + const sensorRef = useRef(null); + const [currentState, setCurrentState] + = useState('unintialized'); const world = context.context.moduleContexts.robot_simulation.state .world as World; + const ev3 = context.context.moduleContexts.robot_simulation.state + .ev3 as DefaultEv3; + + const robotConsole = world.robotConsole; + useEffect(() => { const startThreeAndRapierEngines = async () => { setCurrentState(world.state); @@ -51,6 +65,10 @@ export const SimulationCanvas: React.FC = ({ if (ref.current) { ref.current.replaceChildren(world.render.getElement()); } + + if (sensorRef.current) { + sensorRef.current.replaceChildren(ev3.get('colorSensor').renderer.getElement()); + } }; if (currentState === 'unintialized') { @@ -81,7 +99,13 @@ export const SimulationCanvas: React.FC = ({
{currentState}
- {/* This will be added in part 2 */} + + } /> + } /> + }/> + }/> + } /> +
); diff --git a/src/tabs/RobotSimulation/components/TabPanels/ColorSensorPanel.tsx b/src/tabs/RobotSimulation/components/TabPanels/ColorSensorPanel.tsx new file mode 100644 index 000000000..644cc42ab --- /dev/null +++ b/src/tabs/RobotSimulation/components/TabPanels/ColorSensorPanel.tsx @@ -0,0 +1,43 @@ +import React, { useEffect, useRef } from 'react'; +import { type DefaultEv3 } from '../../../../bundles/robot_simulation/controllers/ev3/ev3/default/ev3'; +import { useFetchFromSimulation } from '../../hooks/fetchFromSimulation'; +import { LastUpdated } from './tabComponents/LastUpdated'; +import { TabWrapper } from './tabComponents/Wrapper'; + +export const ColorSensorPanel: React.FC<{ ev3: DefaultEv3 }> = ({ ev3 }) => { + const colorSensor = ev3.get('colorSensor'); + const sensorVisionRef = useRef(null); + + const [timing, color] = useFetchFromSimulation(() => { + if (ev3.get('colorSensor') === undefined) { + return null; + } + return colorSensor.sense(); + }, 1000); + + useEffect(() => { + if (sensorVisionRef.current) { + sensorVisionRef.current.replaceChildren( + colorSensor.renderer.getElement() + ); + } + }, [timing]); + + if (timing === null) { + return Loading color sensor; + } + + if (color === null) { + return Color sensor not found; + } + + return ( + + +
+

Red: {color.r}

+

Green: {color.g}

+

Blue: {color.b}

+ + ); +}; diff --git a/src/tabs/RobotSimulation/components/TabPanels/ConsolePanel.tsx b/src/tabs/RobotSimulation/components/TabPanels/ConsolePanel.tsx new file mode 100644 index 000000000..8ff7e73c4 --- /dev/null +++ b/src/tabs/RobotSimulation/components/TabPanels/ConsolePanel.tsx @@ -0,0 +1,60 @@ +import { + type LogEntry, + type RobotConsole, +} from '../../../../bundles/robot_simulation/engine/Core/RobotConsole'; +import { useFetchFromSimulation } from '../../hooks/fetchFromSimulation'; +import { LastUpdated, getTimeString } from './tabComponents/LastUpdated'; +import { TabWrapper } from './tabComponents/Wrapper'; + +const getLogString = (log: LogEntry) => { + const logLevelText: Record = { + source: 'Runtime Source Error', + error: 'Error', + }; + + const timeString = getTimeString(new Date(log.timestamp)); + return `[${timeString}] ${logLevelText[log.level]}: ${log.message}`; +}; + +export const ConsolePanel: React.FC<{ + robot_console?: RobotConsole; +}> = ({ robot_console }) => { + const [timing, logs] = useFetchFromSimulation(() => { + if (robot_console === undefined) { + return null; + } + return robot_console.getLogs(); + }, 1000); + + if (timing === null) { + return Not fetched yet; + } + + if (logs === null) { + return ( + + Console not found. Ensure that the world is initialized properly. + + ); + } + + if (logs.length === 0) { + return ( + + +

There is currently no logs

+ + ); + } + + return ( + + +
    + {logs.map((log, i) => ( +
  • {getLogString(log)}
    • + ))} +
+ + ); +}; diff --git a/src/tabs/RobotSimulation/components/TabPanels/MotorPidPanel.tsx b/src/tabs/RobotSimulation/components/TabPanels/MotorPidPanel.tsx new file mode 100644 index 000000000..ec9a88f43 --- /dev/null +++ b/src/tabs/RobotSimulation/components/TabPanels/MotorPidPanel.tsx @@ -0,0 +1,57 @@ +import { NumericInput } from '@blueprintjs/core'; +import { type CSSProperties } from 'react'; +import { type DefaultEv3 } from '../../../../bundles/robot_simulation/controllers'; +import { TabWrapper } from './tabComponents/Wrapper'; + +const RowStyle: CSSProperties = { + display: 'flex', + flexDirection: 'row', + gap: '0.6rem', +}; + +export const MotorPidPanel: React.FC<{ ev3: DefaultEv3 }> = ({ ev3 }) => { + const onChangeProportional = (value: number) => { + ev3.get('leftMotor').pid.proportionalGain = value; + ev3.get('rightMotor').pid.proportionalGain = value; + }; + const onChangeIntegral = (value: number) => { + ev3.get('leftMotor').pid.integralGain = value; + ev3.get('rightMotor').pid.integralGain = value; + }; + const onChangeDerivative = (value: number) => { + ev3.get('leftMotor').pid.derivativeGain = value; + ev3.get('rightMotor').pid.derivativeGain = value; + }; + + return ( + +
+ Proportional Gain: + +
+
+ Integral Gain: + +
+
+ Derivative Gain: + +
+ + ); +}; diff --git a/src/tabs/RobotSimulation/components/TabPanels/UltrasonicSensorPanel.tsx b/src/tabs/RobotSimulation/components/TabPanels/UltrasonicSensorPanel.tsx new file mode 100644 index 000000000..dfa64a3fe --- /dev/null +++ b/src/tabs/RobotSimulation/components/TabPanels/UltrasonicSensorPanel.tsx @@ -0,0 +1,35 @@ +import React from 'react'; +import { type DefaultEv3 } from '../../../../bundles/robot_simulation/controllers/ev3/ev3/default/ev3'; +import { useFetchFromSimulation } from '../../hooks/fetchFromSimulation'; +import { LastUpdated } from './tabComponents/LastUpdated'; +import { TabWrapper } from './tabComponents/Wrapper'; + +export const UltrasonicSensorPanel: React.FC<{ ev3: DefaultEv3 }> = ({ + ev3, +}) => { + const ultrasonicSensor = ev3.get('ultrasonicSensor'); + + const [timing, distanceSensed] = useFetchFromSimulation(() => { + if (ultrasonicSensor === undefined) { + return null; + } + return ultrasonicSensor.sense(); + }, 1000); + + if (timing === null) { + return Loading ultrasonic sensor; + } + + if (distanceSensed === null) { + return Ultrasonic sensor not found; + } + + return ( + + +
+

Distance: {distanceSensed}

+
+ + ); +}; diff --git a/src/tabs/RobotSimulation/components/TabPanels/WheelPidPanel.tsx b/src/tabs/RobotSimulation/components/TabPanels/WheelPidPanel.tsx new file mode 100644 index 000000000..b4f5fcd30 --- /dev/null +++ b/src/tabs/RobotSimulation/components/TabPanels/WheelPidPanel.tsx @@ -0,0 +1,63 @@ +import { NumericInput } from '@blueprintjs/core'; +import { type CSSProperties } from 'react'; +import { type DefaultEv3 } from '../../../../bundles/robot_simulation/controllers'; +import { TabWrapper } from './tabComponents/Wrapper'; + +const RowStyle: CSSProperties = { + display: 'flex', + flexDirection: 'row', + gap: '0.6rem', +}; + +export const WheelPidPanel: React.FC<{ ev3: DefaultEv3 }> = ({ ev3 }) => { + const onChangeProportional = (value: number) => { + ev3.get('backLeftWheel').pid.proportionalGain = value; + ev3.get('backRightWheel').pid.proportionalGain = value; + ev3.get('frontLeftWheel').pid.proportionalGain = value; + ev3.get('frontRightWheel').pid.proportionalGain = value; + }; + const onChangeIntegral = (value: number) => { + ev3.get('backLeftWheel').pid.integralGain = value; + ev3.get('backRightWheel').pid.integralGain = value; + ev3.get('frontLeftWheel').pid.integralGain = value; + ev3.get('frontRightWheel').pid.integralGain = value; + }; + const onChangeDerivative = (value: number) => { + ev3.get('backLeftWheel').pid.derivativeGain = value; + ev3.get('backRightWheel').pid.derivativeGain = value; + ev3.get('frontLeftWheel').pid.derivativeGain = value; + ev3.get('frontRightWheel').pid.derivativeGain = value; + }; + + return ( + +
+ Proportional Gain: + +
+
+ Integral Gain: + +
+
+ Derivative Gain: + +
+ + ); +}; diff --git a/src/tabs/RobotSimulation/components/TabPanels/tabComponents/LastUpdated.tsx b/src/tabs/RobotSimulation/components/TabPanels/tabComponents/LastUpdated.tsx new file mode 100644 index 000000000..116de13ca --- /dev/null +++ b/src/tabs/RobotSimulation/components/TabPanels/tabComponents/LastUpdated.tsx @@ -0,0 +1,21 @@ +import type React from 'react'; + +export const getTimeString = (date: Date) => { + const options: Intl.DateTimeFormatOptions = { + hour: '2-digit', + minute: '2-digit', + second: '2-digit', + hour12: false, // Use 24-hour format. Set to true for 12-hour format if preferred. + }; + return date.toLocaleTimeString([], options); +}; + +export const LastUpdated: React.FC<{ time: Date }> = ({ + time, +}: { + time: Date; +}) => { + const timeString = getTimeString(time); + + return Last updated: {timeString}; +}; diff --git a/src/tabs/RobotSimulation/components/TabPanels/tabComponents/Wrapper.tsx b/src/tabs/RobotSimulation/components/TabPanels/tabComponents/Wrapper.tsx new file mode 100644 index 000000000..d7d5aa806 --- /dev/null +++ b/src/tabs/RobotSimulation/components/TabPanels/tabComponents/Wrapper.tsx @@ -0,0 +1,10 @@ +import type { CSSProperties } from 'react'; + +const panelWrapperStyle: CSSProperties = { + 'padding': '10px' +}; +export const TabWrapper:React.FC<{ + children?: React.ReactNode; +}> = ({children}) => { + return
{children}
; +}; diff --git a/src/tabs/RobotSimulation/components/TabUi.tsx b/src/tabs/RobotSimulation/components/TabUi.tsx index 23de6702d..00b9375c6 100644 --- a/src/tabs/RobotSimulation/components/TabUi.tsx +++ b/src/tabs/RobotSimulation/components/TabUi.tsx @@ -1,4 +1,4 @@ -import type React from 'react'; +import React from 'react'; type TabUiProps = { onOpenCanvas: () => void;