Add Motor Controller

MicomPlugin (transport bridge)
  └── MotorControl (abstract drive controller)
        └── Motor[] (per-wheel PID + drive)
              └── Articulation (ArticulationBody wrapper)

/*
 * Copyright (c) 2026 LG Electronics Inc.
 *
 * SPDX-License-Identifier: MIT
 */

using UnityEngine;

namespace SensorDevices
{
	public class MyDrive : MotorControl
	{
		private Odometry _odometry;

		public MyDrive(Transform controllerTransform)
			: base(controllerTransform)
		{
		}

		public override void SetWheelInfo(in float radius, in float separation)
		{
			// Store wheel geometry, configure motor constraints
			base.SetWheelInfo(radius, separation);

			_odometry = new Odometry(wheelRadius, wheelSeparation);
		}

		public override void Drive(in float linearVelocity, in float angularVelocity)
		{
			// Convert twist to per-wheel angular velocities
			var leftWheelVel = (linearVelocity - angularVelocity * _wheelSeparation / 2f) / _wheelRadius;
			var rightWheelVel = (linearVelocity + angularVelocity * _wheelSeparation / 2f) / _wheelRadius;

			// Apply to motors
			SetMotorVelocity(Location.LEFT, leftWheelVel);
			SetMotorVelocity(Location.RIGHT, rightWheelVel);
		}

		public override bool Update(
			out float linearVelocity, out float angularVelocity,
			in float duration, in float compensatingRatio = 1f)
		{
			// Run PID on each motor
			foreach (var motor in _motorList.Values)
			{
				motor.Run(duration);
			}

			// Compute odometry from wheel encoders
			var leftVel = GetMotorVelocity(Location.LEFT);
			var rightVel = GetMotorVelocity(Location.RIGHT);

			_odometry.Update(leftVel, rightVel, duration);

			linearVelocity = _odometry.LinearVelocity;
			angularVelocity = _odometry.AngularVelocity;

			return true;
		}

		public override void Reset()
		{
			base.Reset();
			_odometry?.Reset();
		}
	}
}

// Motor is attached per-wheel via MotorControl.AttachMotor()
motor.SetTargetVelocity(angularVelocityRadPerSec);
motor.Run(fixedDeltaTime);      // Applies PID correction
motor.GetVelocity();            // Reads back from ArticulationBody
motor.Stop();                   // Zero velocity + reset PID

public enum Location
{
	NONE = 0,
	LEFT = 1,
	RIGHT = 2,
	LEFT_REAR = 3,
	RIGHT_REAR = 4,
	// Add new locations if needed for your drive type
}

motor.SetPID(pGain, iGain, dGain);

_odometry = new Odometry(wheelRadius, wheelSeparation);
_odometry.Update(leftWheelVel, rightWheelVel, deltaTime);

// Read back:
_odometry.LinearVelocity;
_odometry.AngularVelocity;
_odometry.Pose;  // (x, y, heading)

// In MicomPlugin.OnStart() or MicomSensor setup:
_motorControl = new MyDrive(transform);
_motorControl.SetWheelInfo(wheelRadius, wheelSeparation);
_motorControl.AttachMotor(Location.LEFT, leftWheelLink);
_motorControl.AttachMotor(Location.RIGHT, rightWheelLink);

public override void Drive(in float linearVelocity, in float angularVelocity)
{
	// Front axle
	SetMotorVelocity(Location.LEFT, frontLeftVel);
	SetMotorVelocity(Location.RIGHT, frontRightVel);
	// Rear axle
	SetMotorVelocity(Location.LEFT_REAR, rearLeftVel);
	SetMotorVelocity(Location.RIGHT_REAR, rearRightVel);
}