add-motor-controller

name	add-motor-controller
description	Add a new motor or drive controller for robot locomotion. Use when: implementing a new drive type (e.g., mecanum, Ackermann, legged), adding a new actuator controller, creating a custom motion model.

Add a New Motor/Drive Controller

Procedure for creating a new drive controller that manages motors and provides odometry for a robot locomotion system.

When to Use

• Implementing a new drive type (mecanum, Ackermann, swerve, legged)
• Adding a custom actuator controller
• Creating a new motion model for a robot platform

Architecture Overview

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

Procedure

1. Create the Drive Controller

Create Assets/Scripts/Devices/Modules/Motor/MyDriveControl/MyDrive.cs:

/*
 * 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();
		}
	}
}

2. Understand the Motor API

Each Motor wraps an ArticulationBody joint drive:

// 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

3. Understand the Location Enum

Motors are indexed by Location:

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

4. Configure PID

Each motor has its own PID controller:

motor.SetPID(pGain, iGain, dGain);

PID features:

• Integral clamping (anti-windup)
• Output clamping
• Reset on mode change

5. Integrate Odometry

Use Odometry class for dead reckoning:

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

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

Odometry features:

• Runge-Kutta 2nd order integration
• Optional IMU yaw fusion
• Rolling mean velocity filtering

6. Wire to MicomPlugin

The MicomPlugin creates the drive controller in its OnStart():

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

The plugin's FixedUpdate() calls _motorControl.Update() and the RX handler calls _motorControl.Drive().

7. Handle Multi-Axle (Optional)

For 4-wheel or multi-axle robots, use additional Location values and compute individual wheel velocities accounting for all axles:

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);
}

Checklist

• Drive controller extends MotorControl
• SetWheelInfo() configures geometry and creates Odometry
• Drive() converts twist to per-wheel velocities
• Update() runs PID on each motor, updates odometry, returns linear/angular velocity
• Reset() calls base.Reset() and resets odometry
• PID gains configured appropriately for the drive type
• Odometry integration uses appropriate model (differential, mecanum, etc.)
• Motors attached via AttachMotor() with correct Location mapping
• License header on new files