MIT模式的使用方法

MIT模式工作说明：

下图是执行器MIT模式的控制框图：

图1 MIT模式控制框图

MIT模式是一种由麻省理工大学开发的用于精确扭矩控制的控制方法，可以实现电流、速度、位置的混合控制，计算公式如下

$c_{ref} = Kp \ast (p_{des} - \theta_m) + Kd \ast (v_{des} - d\theta_m) + c_{ff}$

在上式中，速度环和位置环的输出值与前馈电流$c_{ff}$相加得到参考电流$c_{ref}$，其中：

$p_{des}$为执行器输出轴的期望位置，单位为count；

$\theta_m$为执行器输出轴的当前位置，单位为count； $v_{des}$为减速机输入端的期望速度，单位为rpm；

$\theta_m$为减速机输入端的当前速度，单位为rpm；

在这个模式下共有5个控制参数，需要分为两条CAN帧发送，一条帧为电机的电流，速度，和减速器的位置信息（可在电机参数设置中修改为电机端的位置），另一条为Kp和Kd，报文内容请见通讯协议介绍。合理的设置kp、kd值和电流值可以有效地控制执行器旋转的刚度、阻尼和力矩。

例子：

1. 当kp和kd值都为0时，旋转刚度、阻尼都为0，此时相当于电流模式，可以直接控制相电流：

1. 当kp值为0，kd值不为0时，刚度为0，阻尼非0，输入电流（前馈力矩补偿）与速度量可以控制执行器的旋转速度：

1. 当kp和kd的值都不为0时有多种情况，这里介绍其中两种：

(1) 当速度量为0时，输入电流（前馈力矩补偿）、位置量可以实现定点控制，如图：

（2）当期望位置$p_{des}$是随时间变化的连续可导函数时，$v_{des}$是$p_{des}$的导数，可以实现位置和速度跟踪，实现按照期望速度旋转期望角度的功能。下图是目标轨迹与实际轨迹的测试曲线图：

❗

当kp不为0，kd等于0时，执行器的阻尼为0，此时执行器将会以目标位置为中心发生震荡，不建议这样使用。

SDK：

C++

• 单执行器

motor.mode_selection(Mode::MIT_MODE, xx, xx, xx)
motor.write_mit(Target current, Target velocity, Target position, KP, KD)

demo 固定电流

❗

目标电流不为0，其他都为0

#include <motor_control.h>
string port_name = "COM27"         // Windows: "COM*"  Linux: "/dev/ttyUSB*"
MotorControl motor(port_name, 921600, 0x01);             // Params: serial port path (string), baud rate (uint32_t), actuator id (0x05060100)

int main()
{
    motor.mode_selection(Mode::MIT_MODE, 10, 10, 10);    // Params: Mode::operation mode, feedback cycle 1, 2, 3
    Sleep(10);                                           // Windows: Sleep(ms)  Linux: sleep(s)
    motor.write_mit(500, 0, 0, 0.0, 0.0);                // Params: target current, target velocity, target position, kp, kd
}

固定速度（电流的大小应视情况而定）

❗

目标位置和KP为0，其他不为0

#include <motor_control.h>
string port_name = "COM27";         // Windows: "COM*"  Linux: "/dev/ttyUSB*"
MotorControl motor(port_name, 921600, 0x01);             // Params: serial port path (string), baud rate (uint32_t), actuator id (0x05060100)

int main()
{
    motor.mode_selection(Mode::MIT_MODE, 10, 10, 10);    // Params: Mode::operation mode, feedback cycle 1, 2, 3
    Sleep(10);                                           // Windows: Sleep(ms)  Linux: sleep(s)
    motor.write_mit(300, 500, 0, 0.0, 10.0);             // Params: target current, target velocity, target position, kp, kd
}

点到点（电流的大小应视情况而定）

❗

目标速度为0，其他不为0

#include <motor_control.h>
string port_name = "COM27";         // Windows: "COM*"  Linux: "/dev/ttyUSB*"
MotorControl motor(port_name, 921600, 0x01);             // Params: serial port path (string), baud rate (uint32_t), actuator id (0x05060100)

int main()
{
    motor.mode_selection(Mode::MIT_MODE, 10, 10, 10);    // Params: Mode::operation mode, feedback cycle 1, 2, 3
    Sleep(10);                                           // Windows: Sleep(ms)  Linux: sleep(s)
    motor.write_mit(300, 0, 65535, 13.1, 10.0);          // Params: target current, target velocity, target position, kp, kd
}

• 多执行器

motors.mode_selection(Mode::MIT_MODE, xx, xx, xx)
motors.write_mit(vector<int16_t>target current, vector<int16_t>target velocity, vector<int16_t>target position, vector<float>KP, vector<float>KD)

demo: 固定电流

❗

目标电流不为0，其他都为0

#include <multi_motor_control.h>
vector<int16_t>cur = { 500, 500 };
vector<int16_t>vel = { 0, 0 };
vector<int32_t>pos = { 0, 0 };
vector<float>kp = { 0.0, 0.0 };
//vector<float>kd = { 0.0, 0.0 };
string port_name = "COM27";         // Windows: "COM*"  Linux: "/dev/ttyUSB*"
MultiMotorControl motors(port_name, 921600, ids);        // Params: serial port path (string), baud rate (uint32_t), actuator ids (0x05060100, 0x05061700)

int main()
{
    motors.mode_selection(Mode::MIT_MODE, 10, 10, 10);   // Params: Mode::operation mode, feedback cycle 1, 2, 3
    Sleep(10);                                           // Windows: Sleep(ms)  Linux: sleep(s)
    motors.write_mit(cur, vel, pos, kp, kd);             // Params: vector of target current, vector of target velocity, vector of target position,
                                                         // vector of kp, vector of kd
}

固定速度（电流的大小应视情况而定）

❗

目标位置和KP为0，其他不为0

#include <multi_motor_control.h>
vector<int16_t>cur = { 300, 300 };
vector<int16_t>vel = { 500, 500 };
vector<int32_t>pos = { 0, 0 };
vector<float>kp = { 0.0, 0.0 };
vector<float>kd = { 10.0, 10.0 };
vector<uint8_t>ids = { 0x01, 0x17 };
string port_name = "COM27";         // Windows: "COM*"  Linux: "/dev/ttyUSB*"
MultiMotorControl motors(port_name, 921600, ids);        // Params: serial port path (string), baud rate (uint32_t), actuator ids (0x05060100, 0x05061700)

int main()
{
    motors.mode_selection(Mode::MIT_MODE, 10, 10, 10);   // Params: Mode::operation mode, feedback cycle 1, 2, 3
    Sleep(10);                                           // Windows: Sleep(ms)  Linux: sleep(s)
    motors.write_mit(cur, vel, pos, kp, kd);             // Params: vector of target current, vector of target velocity, vector of target position,
                                                         // vector of kp, vector of kd
}

点到点（电流的大小应视情况而定）

❗

目标速度为0，其他不为0

#include <multi_motor_control.h>
vector<int16_t>cur = { 300, 300 };
vector<int16_t>vel = { 0, 0 };
vector<int32_t>pos = { 65535, 65535 };
vector<float>kp = { 13.1, 13.1 };
vector<float>kd = { 10.0, 10.0 };
vector<uint8_t>ids = { 0x01, 0x17 };
string port_name = "COM27";         // Windows: "COM*"  Linux: "/dev/ttyUSB*"
MultiMotorControl motors(port_name, 921600, ids);            // Params: serial port path (string), baud rate (uint32_t), actuator ids (0x05060100, 0x05061700)

int main()
{
    motors.mode_selection(Mode::MIT_MODE, 10, 10, 10);   // Params: Mode::operation mode, feedback cycle 1, 2, 3
    Sleep(10);                                           // Windows: Sleep(ms)  Linux: sleep(s)
    motors.write_mit(cur, vel, pos, kp, kd);             // Params: vector of target current, vector of target velocity, vector of target position,
                                                         // vector of kp, vector of kd
}

Python

mode_selection(MotorControl object, MIT_MODE, xx, xx, xx)
write_mit(MotorControl object, [Target current], [Target velocity], [Target position], [KP], [KD])

demo: 点到点

❗

目标速度为0，其他不为0

from motor_control import *
id = [0x01, 0x17]           # list of int
cur = [300, 300]            # list of int
vel = [0, 0]                # list of int
pos = [65535, 65535]        # list of int
kp = [13.1, 13.1]           # list of float
kd = [10.0, 10.0]           # list of float
port_name = "COM27"         # Windows: "COM*"  Linux: "/dev/ttyUSB*"
motor = MotorControl(port_name, id, 921600) # Params: serial port (str), list of actuator ID (0x05060100, 0x05061700)

mode_selection(motor, MIT_MODE, 10, 10, 10) # Params: MotorControl object, operation mode, feedback cycle 1, 2, 3
time.sleep(0.01)
write_mit(motor, cur, vel, pos, kp, kd)     # Params: MotorControl object, list of target current, list of target velocity, list of target position, list of KP, list of KD

固定速度（电流的大小应视情况而定）

❗

目标位置和KP为0，其他不为0

from motor_control import *
id = [0x01, 0x17]           # list of int
cur = [300, 300]            # list of int
vel = [500, 500]            # list of int
pos = [0, 0]                # list of int
kp = [0.0, 0.0]             # list of float
kd = [10.0, 10.0]           # list of float
port_name = "COM27"         # Windows: "COM*"  Linux: "/dev/ttyUSB*"
motor = MotorControl(port_name, id, 921600) # Params: serial port (str), list of actuator ID (0x05060100, 0x05061700)

mode_selection(motor, MIT_MODE, 10, 10, 10) # Params: MotorControl object, operation mode, feedback cycle 1, 2, 3
time.sleep(0.01)
write_mit(motor, cur, vel, pos, kp, kd)     # Params: MotorControl object, list of target current, list of target velocity, list of target position, list of KP, list of KD

固定扭矩（电流的大小应视情况而定）

❗

目标电流不为0，其他都为0

from motor_control import *
id = [0x01, 0x17]           # list of int
cur = [500, 500]            # list of int
vel = [0, 0]                # list of int
pos = [0, 0]                # list of int
kp = [0.0, 0.0]             # list of float
kd = [0.0, 0.0]             # list of float
port_name = "COM27"         # Windows: "COM*"  Linux: "/dev/ttyUSB*"
motor = MotorControl(port_name, id, 921600) # Params: serial port (str), list of actuator ID (0x05060100, 0x05061700)

mode_selection(motor, MIT_MODE, 10, 10, 10) # Params: MotorControl object, operation mode, feedback cycle 1, 2, 3
time.sleep(0.01)
write_mit(motor, cur, vel, pos, kp, kd)     # Params: MotorControl object, list of target current, list of target velocity, list of target position, list of KP, list of KD