Version: 2.0

MIT mode

MIT Model Description:

The following figure is the control block diagram of the actuator MIT mode:

Figure 1 MIT mode control block diagram

MIT mode is a control method developed by MIT for precise torque control. It can achieve mixed control of current, speed and position. The calculation formula is as follows:

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

In the above formula, the output values of the speed loop and the position loop are added to the feedforward current $c_{ff}$ to obtain the reference current $c_{ref}$ , where:

$p_{des}$ is the expected position of the motor side, in counts;

$\theta_m$ is the current position of the motor side, in counts; $v_{des}$ is the expected speed of the motor side, in rpm;

$\theta_m$ is the current speed of the motor side, in rpm;

In this mode, there are 5 control parameters, which need to be sent in two CAN frames. One frame is the motor current, speed, and reducer position information (which can be modified to the position of the motor end in the motor parameter setting), and the other is Kp and Kd. For the message content, please refer to the CAN2.0B communication protocol. Reasonable settings of kp, kd and current values can effectively control the stiffness, damping and torque of the actuator rotation.

Note that the position value sent by the message here defaults to the position of the reducer output side, but the driver board chip uses the position value of the motor side when calculating.

🍞

$c_{ff}$ value is usually the sum of the current required to offset the friction of the actuator and the current value calculated by the robot dynamics.

example:

When the kp and kd values are both 0, the rotational stiffness and damping are both 0, which is equivalent to the current mode, and the phase current can be directly controlled:

When the kp value is 0 and the kd value is not 0, the stiffness is 0, the damping is not 0, and the input current (feedforward torque compensation) and speed can control the rotation speed of the actuator:

When the values of kp and kd are not 0, there are many cases. Here are two of them:

(1) When the speed is 0, the input current (feedforward torque compensation) and position can achieve fixed-point control, as shown in the figure:

(2) When the desired position $p_{des}$ is a continuous differentiable function that changes with time, $v_{des}$ is the derivative of $p_{des}$ , which can realize position and speed tracking and realize the function of rotating the desired angle at the desired speed. The following figure is a test curve diagram of the target trajectory and the actual trajectory:

❗

When kp is not 0 and kd is 0, the damping of the actuator is 0. At this time, the actuator will oscillate around the target position. This is not recommended.

Example Code

python:

from motor_control import *

# port_name: Windows: "COM*"  Linux: "/dev/ttyUSB*"
# id: Executor ID
# Baudrate: The baud rate can only be 921600
port_name = "COM6"
id = [0x00]
Baudrate = 921600
motor = MotorControl(port_name, id, Baudrate)

# Feedback_cycle1(ms): Feedback period of the first status packet，
#                      Including voltage, pwm, current, speed
# Feedback_cycle2(ms)：Feedback period of the second status packet，
#                      Including the position of the motor and the position of the actuator
# Feedback_cycle3(ms)：Feedback period of the third status packet，
#                      Including MOS tube, motor temperature, chip temperature and warning information, 
#                      error information, operation mode
# cur: current(mA)
Feedback_cycle1 = 10
Feedback_cycle2 = 10
Feedback_cycle3 = 10

# MIT mode(Refer to the tutorial on how to use MIT mode)
cur = [1000]
vel = [0]
pos = [0]
kp = [0]
kd = [0]

reset(motor)
time.sleep(0.6)
mode_selection(motor, MIT_MODE, Feedback_cycle1, Feedback_cycle2, Feedback_cycle3)
time.sleep(1)

# current_pid_set(motor, {0.006}, {0.001})

# control current
write_mit(motor, cur, {0}, {0}, kp, kd)
time.sleep(2)
current = get_v_p_c_v(motor)[0][-2]
write_mit(motor, {0}, {0}, {0}, kp, kd)
time.sleep(2)

# control velocity
kp = [0]
kd = [13.1]
write_mit(motor, {500}, {600}, {0}, kp, kd)
time.sleep(2)
velocity = get_v_p_c_v(motor)[0][-1]
write_mit(motor, {0}, {0}, {0}, kp, kd)
time.sleep(2)

position = get_pos(motor)[0][-1]

# control position
kp = [0.09]
kd = [13.1]
write_mit(motor, {310}, {0}, {position + 1000}, kp, kd)
time.sleep(2)
position = get_pos(motor)[0][-1]

d = 0.002
cycle = 20
num = 1
i = 0
while 1:
    t = i * d
    ang = pi * math.sin(2 * pi / cycle * t)
    pos = ang / pi / 2 * 65536 + position

    vel = pi * math.cos(2 * pi / cycle * t)
    motor_speed = vel * 60 / 2 / pi * 101

    cur = 0
    if vel > 0: cur = 500
    elif vel < 0: cur = -500

    write_mit(motor, {cur}, {motor_speed}, {pos}, kp, kd)
    i = i + 1
    time.sleep(d)
    if t >= cycle * num:
        break

c++：

#include <motor_control.h>
#include <multi_motor_control.h>
#include <iostream>
#include <thread>
#include <chrono>

#ifdef _WIN32
    #include <windows.h>
    #include <mmsystem.h>
    #pragma comment(lib, "winmm.lib")
#endif

#define delay(s) std::this_thread::sleep_for(std::chrono::milliseconds((int32_t)(s*1000)))

int main()
{
#ifdef _WIN32
    timeBeginPeriod(1);
#endif

    #define status_packet1 0xb1
    #define status_packet2 0xb2
    #define status_packet3 0xb3
    
    // port_name: Windows: "COM*"  Linux: "/dev/ttyUSB*"
    // id: Executor ID
    // Baudrate: The baud rate can only be 921600
    string port_name = "COM6";
    uint32_t Baudrate = 921600;
    uint8_t id = 0x00;
    MotorControl motor(port_name, Baudrate, id);
    
    // Feedback_cycle1(ms): Feedback period of the first status packet，
    //                      Including voltage, pwm, current, speed
    // Feedback_cycle2(ms)：Feedback period of the second status packet，
    //                      Including the position of the motor and the position of the actuator
    // Feedback_cycle3(ms)：Feedback period of the third status packet，
    //                      Including MOS tube, motor, chip temperature and warning information, 
    //                      error information, current operation mode
    // cur: current(mA)
    uint8_t Feedback_cycle1 = 10;
    uint8_t Feedback_cycle2 = 10;
    uint8_t Feedback_cycle3 = 10;
    
    // MIT_MODE(Refer to the tutorial on "how to use MIT mode")
    int16_t cur = 1000;
    int16_t vel = 0;
    int32_t pos = 0;
    float kp = 0;
    float kd = 0;
    motor.reset();
    motor.mode_selection(Mode::MIT_MODE, Feedback_cycle1, Feedback_cycle2, Feedback_cycle3);
    
    // motor.write_current_pid(0.006, 0.001);              // Params: KP, KI
    
    // control current
    motor.write_mit(cur, 0, 0, kp, kd);
    
    cout << endl << "MIT mode" << endl;
    printf("target_current: %d\n", cur);
    delay(2);
    MotorStatus status = motor.get_motor_status({status_packet1,status_packet2,status_packet3});
    printf("current: %d\n", status.current_value);
    
    motor.write_mit(0, 0, 0, kp, kd);
    delay(2);
    
    // control velocity
    kp = 0;
    kd = 13.1;
    motor.write_mit(500, 1000, 0, kp, kd);
    printf("target_velocity: %d\n", 1000);
    delay(2);
    status = motor.get_motor_status({status_packet1,status_packet2,status_packet3});
    printf("velocity: %d\n", status.velocity_value);
    motor.write_mit(0, 0, 0, kp, kd);
    delay(2);
    
    // control position
    kp = 0.09;
    kd = 13.1;
    status = motor.get_motor_status({ status_packet1,status_packet2,status_packet3 });
    motor.write_mit(300, 0, status.actuator_position_value + 1000, kp, kd);
    printf("target_position: %d\n", status.actuator_position_value + 1000);
    delay(5);
    status = motor.get_motor_status({status_packet1,status_packet2,status_packet3});
    printf("position: %d\n", status.actuator_position_value);
    
    // ---------Running track points---------
    d = 0.002;     // unit time
    cycle = 10;     // Track cycle
    num = 1;       // Execution times
    
    printf("control Trajectory\r\n");
    while (1)
    {
        static size_t i = 0;
    
        double t = i * d;
    
        // Guidable position
        double ang = pi * sinf(2 * pi / cycle * t);
        int pos = ang / pi / 2 * 65536 + status.actuator_position_value;
    
        // Derivative of position
        double vel = pi * cosf(2 * pi / cycle * t);
        int motor_speed = vel * 60 / 2 / pi * 101;
    
        // Compensated current
        double cur = 0;
        if (vel > 0) cur = 500;
        else if (vel < 0) cur = -500;
    
        // Controlling position and velocity
        motor.write_mit(cur, motor_speed, pos, 0.09, 13.1);
    
        i++;
        delay(d);
    
        if (t >= cycle * num)
        {
            break;
        }
    }
    
#ifdef _WIN32
    timeEndPeriod(1);
#endif
}

MIT Model Description:

example:​

Example Code

example: