Moteus Code Analyze

moteus/fw at main · mjbots/moteusGitHub

Current Sampling

article

Position Sampling

article

encoder calibration

Using All-Digital Phase Locked Loop filter

Handled in motor_position.h

pll_filter_hz = 400

filter.kp = 2 * 400 * 2pi

filter.ki = (400 * 2pi) ** 2

position += dt * filter.kp * error

velocity += dt * filter.ki * error

Position Control with Vel and Acc Limit

article

User will issue position control command

d pos 0 0 0.1

which will be handled by the ISR_DoPosition() function in bldc_servo.cc

  void ISR_DoPosition(const SinCos& sin_cos, CommandData* data) MOTEUS_CCM_ATTRIBUTE {
    PID::ApplyOptions apply_options;
    apply_options.kp_scale = data->kp_scale;
    apply_options.kd_scale = data->kd_scale;

    ISR_DoPositionCommon(sin_cos, data, apply_options, data->max_torque_Nm,
                         data->feedforward_Nm, data->velocity);
  }

The service routine first will call BldcServoPosition::UpdateCommand() in bldc_servo_position.h

If the trajectory is not done, it will call UpdateTrajectory()

which then will call DoVelocityAndAccelLimits

inside the funciton,

a is the acceleration limit

v0 is the current velocity

vf is the target velocity

position_relative_raw is the target position

dx is the position error

dv is the velocity error

acceleration is calculated as:

a = accel_limit

if abs(v0) > velocity_limit:
  a = sign(-v0) * a
else:
  if v0 * dx > 0:
    decel_distance = v0**2 / 2a
    if abs(dx) >= decel_distance:
      if abs(v0) < velocity_limit:
        a = sign(dx) * a
      else:
        a = 0
    else:
      a =sign(-v0) * a
  else:
    a = sign(-v0) * a