The motor controller is a cascaded style position, velocity and current control loop, as per the diagram below. When the control mode is set to position control, the whole loop runs. When running in velocity control mode, the position control part is removed and the velocity command is fed directly in to the second stage input. In current control mode, only the current controller is used.
The position controller is a P loop with a single proportional gain.
pos_error = pos_setpoint - pos_feedback vel_cmd = pos_error * pos_gain + vel_feedforward
The velocity controller is a PI loop.
vel_error = vel_cmd - vel_feedback current_integral += vel_error * vel_integrator_gain current_cmd = vel_error * vel_gain + current_integral + current_feedforward
The current controller is a PI loop.
current_error = current_cmd - current_fb voltage_integral += current_error * current_integrator_gain voltage_cmd = current_error * current_gain + voltage_integral (+ voltage_feedforward when we have motor model)
For more detail refer to controller.cpp.