Design of RoboMaster electronic control framework based on RT-Thread

Due to RT-Thread's stable and efficient kernel, rich documentation and tutorials, active community atmosphere, device driver framework, Kconfig, Scons, log system, and a large number of software packages, it is difficult not to choose RT-Thread for project development. However, it is precisely because of the wide coverage of these advantages that many beginners will find it difficult to start, but as long as you step into the door of RT-Thread, you will find its beauty. This series of documents will serve as a record and sharing of my development of the RoboMaster electronic control framework based on RT-Thread. I hope it can help more friends who are new to RT-Thread. Everyone is also welcome to communicate and share, correct deficiencies, and make progress together.

Background
The Robomaster robot competition includes multiple arms. In order to improve R&D efficiency, modularization is particularly important. Using RT-Thread helps to develop object-oriented thinking; the equipped Kconfig, Scons and other tools can realize flexible configuration of the project; the software timer can be used to monitor each motor and other modules, and RingBuffer can realize efficient processing of sensor information...

The development board used is DJI's RoboMaster-C development board, and the basic project is rt-thread>bsp>stm32f407-robomaster-c

Motor module development
The motors and ESCs used are all official products of DJI, such as 2006, 3508, 6020, etc., using CAN communication.

Constructing an object
First, we construct a general motor object based on the characteristics of the motor used

/**

@brief DJI intelligent motor typedef
/
typedef struct dji_motor_object
{
rt_device_t can_dev; // Motor CAN instance
dji_motor_measure_t measure; // Motor measurement value
uint32_t tx_id; // Send id (main sender)
uint32_t rx_id; // Receive id (main receiver)
/ Group sending settings /
uint8_t send_group; // Message grouping in the same frame
uint8_t message_num; // Position in a message frame
motor_type_e motor_type; // Motor type motor_working_type_e
stop_flag; // Start and stop flag
/ Monitoring thread related /
rt_timer_t timer; // Motor monitoring timer
/ Motor control related */
void *controller; // Motor controller
int16_t (*control)(dji_motor_measure_t measure); // Interface for controlling the motor User-defined, return value is 16-bit voltage or current value
} dji_motor_object_t;
Because we use CAN to drive these motors, which are extensions of CAN devices, we embed the rt_device_t can_dev parent class structure object.

The dji_motor_measure_t structure contains some feedback values ​​needed for motor control, including direct feedback data from the ESC and further calculated data:

/**

@brief DJI motor feedback
/
typedef struct
{
/ The following is the processed data /
float angle_single_round; // Single-turn angle
float speed_aps; // Angular velocity, unit: degree/second
float total_angle; // Total angle, pay attention to the direction
int32_t total_round; // Total number of turns, pay attention to the direction
float target; // Target value (output shaft torque/speed/angle (unit degree))
/ The following is the data directly returned by the ESC*/
uint16_t ecd; ​​// 0-8191
uint16_t last_ecd; // The encoder value read last time
int16_t speed_rpm; // Motor speed value
int16_t real_current; // Actual torque current
uint8_t temperature; // Celsius
} dji_motor_measure_t;
Register instance
Through dji_motor_object_t *dji_motor_register(motor_config_t *config, void *controller) Register the corresponding motor instance, and the user can flexibly configure the instance through motor_config_t *config:

/**
@brief Motor initialization, returns a motor instance

@param config motor configuration
@return dji_motor_object_t* motor instance pointer
*/
dji_motor_object_t *dji_motor_register(motor_config_t *config, void *controller)
{
dji_motor_object_t *object = (dji_motor_object_t )rt_malloc(sizeof(dji_motor_object_t));
rt_memset(object, 0, sizeof(dji_motor_object_t));
// Connect to user-configured motor_config
object->motor_type = config->motor_type; // 6020 or 2006 or 3508
object->rx_id = config->rx_id; // ID of the motor receiving message
object->control = controller; // Motor controller
/ Find CAN device /
object->can_dev = rt_device_find(config->can_name);
// Motor grouping, because up to 4 motors can share one CAN control message
motor_send_grouping(object, config);
// Motor offline detection timer related
object->timer = rt_timer_create("motor1",
motor_lost_callback,
object, 20,
RT_TIMER_FLAG_PERIODIC);
rt_timer_start(object->timer);
dji_motor_enable(object);
dji_motor_obj[idx++] = object;
return object;
}
/ Motor configuration structure*/
typedef struct
{
motor_type_e motor_type;
const char *can_name;
uint32_t tx_id; // Send id (main sender)
uint32_t rx_id; // Receive id (main receiver)
void *controller; // Motor controller
} motor_config_t;
void *controller in motor_config_t structure The controller set used by the motor is a structure variable with controller type as its member, as follows:

static struct chassis_controller_t { pid_object_t
*speed_pid ;
}chassis_controller;
static struct gimbal_controller_t{
pid_object_t *speed_pid;
pid_object_t *angle_pid;
}gimbal_controlelr;
The void *controller passed in when calling dji_motor_object_t *dji_motor_register is the specific implementation of the controller corresponding to the motor. For example, pid calculation, filtering, etc. will be assigned to the function pointer corresponding to the motor object and executed when the motor control calculation is performed, as follows:

rt_int16_t chassis_control(dji_motor_measure_t measure){
static rt_int16_t set = 0;
set = pid_calculate(chassis_controller.speed_pid, measure.speed_rpm, 1000);
return set;
}
Data processing
The motor object is inseparable from the stable and fast data transmission and analysis and calculation. Next, we will discuss the idea of ​​using RT-Thread's CAN device driver to send and receive data.

First is the reception of data. stm32f4 has two CAN peripherals. All motors and devices using the CAN bus are mounted on these two buses, but each CAN bus of RT-Thread can only register a corresponding receiving callback function through rt_device_set_rx_indicate(can_dev, can_rx_call);. However, the data parsing and processing of different types of motors and different CAN devices are different. My solution here is: first create a usr_callback file to uniformly manage the user receiving swap functions that may be used by CAN, serial ports and other devices; register a large device type callback function to the corresponding CAN device, which further subdivides the data parsing of each mounted device, and implements it as follows:

#ifdef BSP_USING_CAN
rt_err_t can_rx_call(rt_device_t dev, rt_size_t size)
{
struct rt_can_msg rxmsg = {0};
uint8_t rxbuff = rxmsg.data;
/ Read a frame of data from CAN /
rt_device_read(dev, 0, &rxmsg, sizeof(rxmsg));
/ CAN generates an interrupt after receiving the data, calls this callback function, and then sends the receive semaphore /
#ifdef BSP_USING_DJI_MOTOR
dji_motot_rx_callback(rxmsg.id, rxbuff);
#endif / BSP_USING_DJI_MOTOR /
return RT_EOK;
}
#endif / BSP_USING_CAN */
But there is also a problem here, rt_err_t can_rx_call(rt_device_t dev, rt_size_t size) The parameter passed in cannot determine the specific CAN device source, so all the used CAN peripheral data processing functions will be called, but this is not a big problem at present, because the same ID device will not be mounted on the same bus, which is also an error that should be avoided from the beginning.

The next step is to send the CAN message. Just call rt_device_write to send the filled CAN message frame.

Offline detection
Here, the RT-Thread software timer is used to perform offline detection on the motor. If the corresponding motor feedback message is not received within the specified time, the timeout callback is entered and a warning log is output:

/**

@brief Motor timer timeout callback function
@param motor_ptr
*/
static void motor_lost_callback(void *motor_ptr)
{
dji_motor_object_t *motor = (dji_motor_object_t *)motor_ptr;
// dji_motor_stop(motor);
LOG_W("[dji_motor] Motor lost, can bus [%s] , id 0x[%x]", motor->can_dev->parent.name, motor->rx_id);
}
Usage example
The encapsulated motor module usage example is as follows:

static struct chassis_controller_t{
pid_object_t *speed_pid;
}chassis_controller;
static struct gimbal_controller_t{
pid_object_t *speed_pid;
pid_object_t *angle_pid;
}gimbal_controlelr;
static dji_motor_object_t *chassis_motor;
static dji_motor_object_t *gimbal_motor;
rt_int16_t chassis_control(dji_motor_measure_t measure){
static rt_int16_t set = 0;
set = pid_calculate(chassis_controller.speed_pid, measure.speed_rpm, 1000);
return set;
}
rt_int16_t gimbal_control(dji_motor_measure_t measure){
static rt_int16_t set = 0;
set = pid_calculate(gimbal_controlelr.speed_pid, measure.speed_rpm, 0);
return set;
}
static void example_init()
{
pid_config_t chassis_speed_config = {
.Kp = 10, // 4.5
.Ki = 0, // 0
.Kd = 0, // 0
.IntegralLimit = 3000,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.MaxOut = 12000,
};
pid_config_t gimbal_speed_config = {
.Kp = 50, // 50
.Ki = 200, // 200
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 3000,
.MaxOut = 20000,
};
chassis_controller.speed_pid = pid_register(&chassis_speed_config);
gimbal_controlelr.speed_pid = pid_register(&gimbal_speed_config);
motor_config_t chassis_motor_config = {
.motor_type = M3508,
.can_name = CAN_CHASSIS,
.rx_id = 0x201,
.controller = &chassis_controller,
};
motor_config_t gimbal_motor_config = {
.motor_type = GM6020,
.can_name = CAN_GIMBAL,
.rx_id = 0x206,
.controller = &gimbal_controlelr,
};
chassis_motor = dji_motor_register(&chassis_motor_config, chassis_control);
gimbal_motor = dji_motor_register(&gimbal_motor_config, gimbal_control);
}
到此就可以方便且灵活的配置和使用电机模块啦。