机械臂运动控制，通讯的解包-＞运动控制-＞数据封包上报过程

本文链接：https://blog.csdn.net/dmjkun/article/details/133633927

一、协议
在这里插入图片描述
数据格式为小端模式，浮点数格式为IEEE754，需与上位机的PC端一致，如window系统，其它系统需要自行测试，用于传输16位、32位、float数据格式，避免只传输字节数据带来转换的繁琐及精度丢失。
二、下位机数据接收线程

/** 
*  通讯主线程. 
* . 
* @param[in]   parameter：线程参数.
* @param[out]  无.  
* @retval  无.
* @par 标识符 
*      保留 
* @par 其它 
*      无 
* @par 修改日志 
*      kun于2022-07-25创建 
*/
void comm_thread_entry(void *parameter)
{
    uint8_t ch;
    
    serial = rt_device_find("uart2");
    if (!serial)
    {
        LOG_E("find comm uart1 failed!\n");
        return;
    }
    else{
        struct serial_configure config = RT_SERIAL_CONFIG_DEFAULT;  /* 初始化配置参数 */
        /* step2：修改串口配置参数 */
        config.baud_rate = BAUD_RATE_115200;        //修改波特率为 9600
        config.data_bits = DATA_BITS_8;           //数据位 8
        config.stop_bits = STOP_BITS_1;           //停止位 1
        config.bufsz     = RECV_BUF_LEN;            //修改缓冲区 buff size 为 128
        config.parity    = PARITY_NONE;           //无奇偶校验位

        /* step3：控制串口设备。通过控制接口传入命令控制字，与控制参数 */
        rt_device_control(serial, RT_DEVICE_CTRL_CONFIG, &config);
        
        rt_device_open(serial, RT_DEVICE_FLAG_INT_RX);
    }
        
    while(1){
        
        rt_thread_delay(1);
        
//        rt_device_write(serial, 0, "hello world", 11);
                
        if( rt_device_read(serial, -1, &ch, 1) > 0 ){
            
            comm_state.recv_time = 100;
                        
            switch( comm_state.recv_state )
            {
                case RECV_START:
                    if( ch == COMM_HEADER ){
                        comm_state.recv_buf[comm_state.recv_index++] = ch;
                        comm_state.recv_state++;
                    }
                    break;
                case RECV_ID:
                    if( ch == ROBOT_ID ){
                        comm_state.recv_buf[comm_state.recv_index++] = ch;
                        comm_state.recv_state++;
                        comm_state.recv_sub_len = 2;
                    }
                    else{
                        comm_state.recv_state = 0;
                        comm_state.recv_index = 0;
                    }
                    break;
                case RECV_LEN:
                    comm_state.recv_buf[comm_state.recv_index++] = ch;
                    comm_state.recv_sub_len --;
                    if( comm_state.recv_sub_len==0 ){
                        comm_header_t *header = (comm_header_t *)comm_state.recv_buf;
                        comm_state.recv_data_len = header->len;
                        comm_state.recv_state++;
                    }
                    break;
                case RECV_FUN_DATA:
                    comm_state.recv_buf[comm_state.recv_index++] = ch;
                    comm_state.recv_data_len --;
                    if( comm_state.recv_data_len == 0 ){
                        comm_state.recv_state++;
                        comm_state.recv_sub_len = 2;
                    }
                    break;
                case RECV_CRC:
                    comm_state.recv_buf[comm_state.recv_index++] = ch;
                    comm_state.recv_sub_len --;
                    if( comm_state.recv_sub_len == 0 ){
                        comm_state.recv_state++;
                    }
                    break;
                case RECV_END:
                    if( ch == COMM_ENDER ){
                        comm_state.recv_buf[comm_state.recv_index++] = ch;
                        comm_state.recv_len = comm_state.recv_index;
                        comm_state.recv_state = 0;
                        comm_state.recv_index = 0;
                        comm_state.recv_complete = 1;
                    }
                    else{
                        comm_state.recv_state = 0;
                        comm_state.recv_index = 0;
                    }
                    break;
                default:
                    break;
            }
        }
        
        //字节流接收超时
        if( comm_state.recv_time > 0 ){
            comm_state.recv_time --;
            if( comm_state.recv_time==0 ){
                if( comm_state.recv_state != RECV_START )
                    LOG_I("recv byte step %d timeout",comm_state.recv_state);
                comm_state.recv_state = RECV_START;
                comm_state.recv_len = 0;
                comm_state.recv_index = 0;
            }
        }
        
        if( comm_state.recv_complete ){
            
            comm_state.recv_complete = 0;
            
            LOG_HEX("RECV", 16, comm_state.recv_buf, comm_state.recv_len);
            
            comm_header_t *header_ptr = (comm_header_t *)comm_state.recv_buf;
            uint16_t *crc_ptr = (uint16_t *)&comm_state.recv_buf[comm_state.recv_len-3];
            if( comm_crc16( comm_state.recv_buf, comm_state.recv_len-3 ) == *crc_ptr ){

                switch( header_ptr->fun_code )
                {
                    case HAND_SETP_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"HAND_SETP_CMD");
                        comm_hand_set_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case DEPOT_POS_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"DEPOT_POS_CMD");
                        comm_depot_pos_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case AVOID_HIGHT_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"AVOID_HIGHT_CMD");
                        avoid_hight_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case MOTOR_PMT_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"MOTOR_RST_CMD");
                        motor_pmt_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case STATION_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"STATION_CMD");
                        station_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case DEPOT_OFFSET_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"DEPOT_OFFSET_CMD");
                        depot_offset_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case INIT_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"INIT_CMD");
                        comm_init_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case STATION_MOVE_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"STATION_MOVE_CMD");
                        comm_station_move_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case STATION_CLAMP_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"STATION_CLAMP_CMD");
                        comm_station_clamp_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case DEPOT_IN_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"DEPOT_IN_CMD");
                        comm_depot_in_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case DEPOT_A_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"DEPOT_A_CMD");
                        comm_depot_a_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case DEPOT_B_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"DEPOT_B_CMD");
                        comm_depot_b_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case DEPOT_OCCUPY_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"DEPOT_OCCUPY_CMD");
                        comm_occupy_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case CLAMP_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"CLAMP_CMD");
                        comm_clamp_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case STATUS_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"STATUS_CMD");
                        comm_status_cmd(&header_ptr->data, header_ptr->len-1);                        
                        break;
                    case RBT_MOVE_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"RBT_MOVE_CMD");
                        comm_rbt_move_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case DEPOT_MOVE_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"DEPOT_MOVE_CMD");
                        comm_depot_move_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case MOTOR_INIT_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"DEPOT_MOVE_CMD");
                        comm_motor_init_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case SWITCH_CTRL_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"SWITCH_CTRL_CMD");
                        comm_switch_ctrl_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case LIGHT_CTRL_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"LIGHT_CTRL_CMD");
                        comm_light_ctrl_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    case OTA_CMD:
                        LOG_I("recv cmd %02X %s",header_ptr->fun_code,"OTA_CMD");
                        comm_ota_cmd(&header_ptr->data, header_ptr->len-1);
                        break;
                    default:
                        LOG_E("illegal cmd %02X",header_ptr->fun_code);
                        break;
                }
            }
            else{
                LOG_E("recv cmd %02X crc error",header_ptr->fun_code);
            }
        }
    }
}

comm_state.recv_complete 接收一帧完整数据包标记，然后进行校验，判断功能码进行命令处理流转

/** 
*  设置工位座标. 
* . 
* @param[in]   data：数据指针，len：长度.
* @param[out]  无.  
* @retval  无.
* @par 标识符 
*      保留 
* @par 其它 
*      无 
* @par 修改日志 
*      kun于2022-07-25创建 
*/
static void comm_depot_pos_cmd(void *data, uint16_t len)
{
    #pragma pack(push)
    #pragma pack(1)
    typedef struct _depot_pos_recv_t{
        uint8_t         mode;
        station_no_t    no;
        depot_pmt_t     pmt;
    }depot_pos_recv_t;
    #pragma pack(pop)
    
    #pragma pack(push)
    #pragma pack(1)
    typedef struct _depot_pos_send_t{
        uint8_t         mode;
        station_no_t    no;
        depot_pmt_t     pmt[DEPOT_MAX];
    }depot_pos_send_t;
    #pragma pack(pop)
    
    depot_pos_recv_t *depot_pos_recv_ptr = (depot_pos_recv_t *)data;
    
    if( depot_pos_recv_ptr->mode==0x00 ){
    
        if( depot_pos_recv_ptr->no <= DEPOT_MAX ){
            config_get_ptr()->depot_pmt[depot_pos_recv_ptr->no-1].speed = depot_pos_recv_ptr->pmt.speed;
            config_get_ptr()->depot_pmt[depot_pos_recv_ptr->no-1].door = depot_pos_recv_ptr->pmt.door;
            config_get_ptr()->depot_pmt[depot_pos_recv_ptr->no-1].clamp = depot_pos_recv_ptr->pmt.clamp;
            config_get_ptr()->depot_pmt[depot_pos_recv_ptr->no-1].chip = depot_pos_recv_ptr->pmt.chip;
            config_get_ptr()->depot_pmt[depot_pos_recv_ptr->no-1].scaner = depot_pos_recv_ptr->pmt.scaner;
            
            config_save();
            
            comm_reply(EXE_OK, 0, DEPOT_POS_CMD, RT_NULL, 0);
        }
    }
    else if( depot_pos_recv_ptr->mode==0x01 ){
        
        depot_pos_send_t depot_pos_send;
        
        depot_pos_send.mode = depot_pos_recv_ptr->mode;
        depot_pos_send.no = 0;
        
        rt_memcpy(depot_pos_send.pmt,config_get_ptr()->depot_pmt,sizeof(((config_t*)0)->depot_pmt));
        
        comm_reply(EXE_OK, 0, DEPOT_POS_CMD, &depot_pos_send, sizeof(depot_pos_send_t));
    }
}

typedef struct _depot_pmt_t{
    float       speed;
    float       door;
    float       clamp;
    float       chip;
    float       scaner;
}depot_pmt_t;

这个函数传输各个工位的座标数据到下位机进行保存，传输格式为浮点，单位是mm
定义提取结构体
typedef struct _depot_pmt_t{
float speed;
float door;
float clamp;
float chip;
float scaner;
}depot_pmt_t;
typedef struct _depot_pos_recv_t{
uint8_t mode;
station_no_t no;
depot_pmt_t pmt;
}depot_pos_recv_t;
定义结构体指针指向接收的数据帧的数据区
depot_pos_recv_t *depot_pos_recv_ptr = (depot_pos_recv_t *)data;
通过结构体的指针提取需要的数据放到存储单元中，各个数据为浮点数，没有数据转换带来精度损失及转换繁琐问题
config_get_ptr()->depot_pmt[depot_pos_recv_ptr->no-1].speed = depot_pos_recv_ptr->pmt.speed;
config_get_ptr()->depot_pmt[depot_pos_recv_ptr->no-1].door = depot_pos_recv_ptr->pmt.door;
config_get_ptr()->depot_pmt[depot_pos_recv_ptr->no-1].clamp = depot_pos_recv_ptr->pmt.clamp;
config_get_ptr()->depot_pmt[depot_pos_recv_ptr->no-1].chip = depot_pos_recv_ptr->pmt.chip;
config_get_ptr()->depot_pmt[depot_pos_recv_ptr->no-1].scaner = depot_pos_recv_ptr->pmt.scaner;

三、下位机数据上报

/** 
*  通讯状态返回. 
* . 
* @param[in]   status：状态，error_code：错误码，fun_code：功能码，data：数据指针，len：长度.
* @param[out]  无.  
* @retval  无.
* @par 标识符 
*      保留 
* @par 其它 
*      无 
* @par 修改日志 
*      kun于2022-07-25创建 
*/
void comm_reply(comm_cmd_retsult_t status, uint16_t error_code, uint8_t fun_code, void *data, uint16_t len)
{
    #pragma pack(push)
    #pragma pack(1)
    typedef struct {
        comm_cmd_retsult_t  status;
        uint16_t            error_code;
    }reply_t;
    #pragma pack(pop)
    
    uint16_t fix_data_len = sizeof(reply_t)+len;
    //comm_header_t包含一个数据字节所以需要减1，3为校验码+结束符
    uint8_t *send_ptr = rt_malloc(sizeof(comm_header_t) -1 + 3 + fix_data_len);
    
    if( send_ptr ){
        comm_header_t *comm_header;
        
        comm_header = (comm_header_t *)send_ptr;
        
        comm_header->header = COMM_HEADER;
        comm_header->id = ROBOT_ID;
        //1为命令码
        comm_header->len = fix_data_len+1;
        comm_header->fun_code = fun_code;
        
        reply_t *reply_ptr = (reply_t*)&comm_header->data;
        
        reply_ptr->status = status;
        reply_ptr->error_code = error_code;
        
        uint8_t *target_data = (uint8_t*)(&comm_header->data+sizeof(reply_t));
        uint8_t *source_data = (uint8_t*)data;
        for( uint16_t i=0; i<len; i++ ){
            *target_data++ = *source_data++;
        }
        uint16_t *crc_ptr = (uint16_t *)target_data;
        //comm_header_t包含一个数据字节所以需要减1
        *crc_ptr = comm_crc16( send_ptr, sizeof(comm_header_t) -1 + fix_data_len);
        
        target_data+=2;
        *target_data = COMM_ENDER;
        
        if( status == EXE_OK )
            LOG_D("reply EXE OK:%02X",fun_code);
        else if( status == EXE_FAIL )
            LOG_D("reply EXE FAIL:%02X",fun_code);
        else if( status == RECV_OK )
            LOG_D("reply RECV OK:%02X",fun_code);
//        LOG_HEX("REPLY", 16, send_ptr, sizeof(comm_header_t) -1 + 3 + fix_data_len);
        
        rt_device_write(serial, 0, send_ptr, sizeof(comm_header_t) -1 + 3 + fix_data_len);
        
        rt_free(send_ptr);
    }
    else{
        LOG_E("can not malloc memory:%02X",fun_code);
    }
}

所有命令的上报为统一格式，填入状态，错误码，功能码，及命令的私有数据，调用comm_reply函数将对上报数据进行打包发7送

四、运动执行

/** 
*  移动运动轴. 
* . 
* @param[in]   data：数据指针，len：长度.
* @param[out]  无.  
* @retval  无.
* @par 标识符 
*      保留 
* @par 其它 
*      无 
* @par 修改日志 
*      kun于2022-07-25创建 
*/
static void comm_rbt_move_cmd(void *data, uint16_t len)
{
    #pragma pack(push)
    #pragma pack(1)
    typedef struct _move_cmd_t{
        uint8_t     mode;
        uint8_t     axle;
        float       speed;
        float       distance;
    }move_cmd_t;
    #pragma pack(pop)
    
    static const uint8_t rbt_axis[]={ROBOT_Y_AXIS,ROBOT_Z_AXIS};
    
    move_cmd_t *move_cmd_ptr = (move_cmd_t *)data;
    
    robot_msg_t msg;
    
    msg.type.move_msg.axle = rbt_axis[move_cmd_ptr->axle-1];
    msg.robot_cmd = ROBOT_CMD_MOVE;
    msg.type.move_msg.action = move_cmd_ptr->mode;
    msg.type.move_msg.abs_res = 0;
    msg.type.move_msg.position = move_cmd_ptr->distance;
    msg.type.move_msg.speed = move_cmd_ptr->speed;
    
    comm_reply(RECV_OK, 0, RBT_MOVE_CMD, RT_NULL, 0);
    
    robot_write_queue(&msg);
}

解释移动命令，提取运行座标及速度数据，建立一个消息，将数据放入消息中，由运动线程接收消息将进行电机的控制

五、运动控制

/** 
*  机械臂动作执行线程. 
* . 
* @param[in]   parameter：建立线程传入参数.
* @param[out]  无.  
* @retval  无.
* @par 标识符 
*      保留 
* @par 其它 
*      无 
* @par 修改日志 
*      kun于2022-07-29创建 
*/
void robot_thread_entry(void *parameter)
{
    robot_msg_t robot_msg;
    uint8_t i;
    float speed;
    float dist = 0;
    station_xyz_t   station_target;

    while(1)
    {
        //读取命令队列
        if( robot_read_queue(&robot_msg, RT_WAITING_FOREVER) == RT_EOK ){
            
            const char *rbt_cmd_str[]={ "ROBOT_CMD_MOVE",
                                        "ROBOT_CMD_ROUTER",
                                        "ROBOT_CMD_CLAMP",
                                        "ROBOT_CMD_DEPOT",
                                        "ROBOT_CMD_INIT",
                                    };
            LOG_D("get robot cmd %s\n",rbt_cmd_str[robot_msg.robot_cmd]);
     
            //移动调试命令
            if( robot_msg.robot_cmd == ROBOT_CMD_MOVE ){
                
                if( robot_msg.type.move_msg.action == ACT_MOVE ){
                    motor_move(robot_msg.type.move_msg.axle, robot_msg.type.move_msg.position*ROBOT_AXIS_MM, robot_msg.type.move_msg.speed*ROBOT_AXIS_MM);
                    motor_wait_stop(robot_msg.type.move_msg.axle,MOTOR_TIMEOUT);
                }
                else if( robot_msg.type.move_msg.action == ACT_STOP ){
                    motor_stop(robot_msg.type.move_msg.axle);
                }
                
                //更新各个电机的距离
                if( robot_msg.type.move_msg.axle == ROBOT_Z_AXIS ){
                    robot_state.axis_mm_z = (float)motor_curr_step((motor_no_t)ROBOT_Z_AXIS)/(float)ROBOT_AXIS_MM;
                    comm_reply(EXE_OK,RBT_NO_ERR,RBT_MOVE_CMD,RT_NULL,0);
                }
                else if( robot_msg.type.move_msg.axle == ROBOT_Y_AXIS ){
                    robot_state.axis_mm_y = (float)motor_curr_step((motor_no_t)ROBOT_Y_AXIS)/(float)ROBOT_AXIS_MM;
                    comm_reply(EXE_OK,RBT_NO_ERR,RBT_MOVE_CMD,RT_NULL,0);
                }
                else if( robot_msg.type.move_msg.axle == ROBOT_IN_AXIS ){
                    robot_state.axis_mm_in = (float)motor_curr_step((motor_no_t)ROBOT_IN_AXIS)/(float)ROBOT_AXIS_MM;
                    comm_reply(EXE_OK,RBT_NO_ERR,DEPOT_MOVE_CMD,RT_NULL,0);
                }
                else if( robot_msg.type.move_msg.axle == ROBOT_A_AXIS ){
                    robot_state.axis_mm_a = (float)motor_curr_step((motor_no_t)ROBOT_A_AXIS)/(float)ROBOT_AXIS_MM;
                    comm_reply(EXE_OK,RBT_NO_ERR,DEPOT_MOVE_CMD,RT_NULL,0);
                }
                else if( robot_msg.type.move_msg.axle == ROBOT_B_AXIS ){
                    robot_state.axis_mm_b = (float)motor_curr_step((motor_no_t)ROBOT_B_AXIS)/(float)ROBOT_AXIS_MM;
                    comm_reply(EXE_OK,RBT_NO_ERR,DEPOT_MOVE_CMD,RT_NULL,0);
                }
            }
            //路径路由命令
            else if( robot_msg.robot_cmd == ROBOT_CMD_ROUTER ){
                
                //已经处于目标位置，直接返回
                if( robot_msg.type.router_msg.station == robot_state.curr_station ){
                    comm_reply(EXE_OK,RBT_NO_ERR,STATION_MOVE_CMD,RT_NULL,0);
                }
                else{
                    //如果当前已经处于制备、扩增、检测位，直接运动到各个待机位
                    //当前位置不在待机位先运动到待机位
                    if( robot_state.curr_station != DJ_STA ){
                        
                        if( robot_state.curr_station == JC_STA || robot_state.curr_station == KZ_STA ||\
                            robot_state.curr_station == ZB_STA ){
                        
                            dist = robot_state.axis_mm_z + config_get_ptr()->avoid_hight;
                                
                            dist>ROBOT_Z_UP_MAX?dist=ROBOT_Z_UP_MAX:0;
                        
                            //抬升避让高度
                            LOG_D("No1. Z-axis move to %s %d.%03d mm speed %d.%03d mm",\
                                station_name[robot_state.curr_station], \
                                (int)dist,\
                                float_to_int(dist),\
                                (int)(config_get_ptr()->station_pmt.speed.z),\
                                float_to_int(config_get_ptr()->station_pmt.speed.z));
                            robot_move_distance(ROBOT_Z_AXIS, dist, config_get_ptr()->station_pmt.speed.z);
                            if( motor_wait_stop((motor_no_t)ROBOT_Z_AXIS,MOTOR_TIMEOUT) == 3){
                                continue;
                            }
                        }
                        //运行到待机位Y轴
                        dist = config_get_ptr()->station_pmt.depot_in.y;
                        LOG_D("No2. Y-axis move to AXES %d.%03d mm speed %d.%03d mm",\
                            (int)dist,\
                            float_to_int(dist),\
                            config_get_ptr()->station_pmt.speed.y,\
                            float_to_int(config_get_ptr()->station_pmt.speed.y));
                        robot_move_distance(ROBOT_Y_AXIS, dist, config_get_ptr()->station_pmt.speed.y);
                        if( motor_wait_stop((motor_no_t)ROBOT_Y_AXIS,MOTOR_TIMEOUT) == 3){
                            continue;
                        }
                    }
                    
                    switch( robot_msg.type.router_msg.station )
                    {
                        case DJ_STA:               //待机位
                            station_target.y = config_get_ptr()->station_pmt.standby.y;
                            station_target.z = config_get_ptr()->station_pmt.standby.z;
                            break;
                        case JIN_STA:                //进料仓
                            station_target.y = config_get_ptr()->station_pmt.depot_in.y;
                            station_target.z = config_get_ptr()->station_pmt.depot_in.z;
                            break;
                        case FL1_STA:                //废料位1
                            station_target.y = config_get_ptr()->station_pmt.depot_a.y;
                            station_target.z = config_get_ptr()->station_pmt.depot_a.z;
                            break;
                        case FL2_STA:                //废料位2
                            station_target.y = config_get_ptr()->station_pmt.depot_b.y;
                            station_target.z = config_get_ptr()->station_pmt.depot_b.z;
                            break;
                        case ZB_STA:                 //制备位
                            station_target.y = config_get_ptr()->station_pmt.zb_pos.y;
                            station_target.z = config_get_ptr()->station_pmt.zb_pos.z;
                            break;
                        case JC_STA:                 //检测位
                            station_target.y = config_get_ptr()->station_pmt.jc_pos[robot_msg.type.router_msg.index].y;
                            station_target.z = config_get_ptr()->station_pmt.jc_pos[robot_msg.type.router_msg.index].z;
                            break;
                        case KZ_STA:                 //扩增位
                            station_target.y = config_get_ptr()->station_pmt.kz_pos[robot_msg.type.router_msg.index].y;
                            station_target.z = config_get_ptr()->station_pmt.kz_pos[robot_msg.type.router_msg.index].z;
                            break;
                        default:
                            break;
                    }

                    //当前位置已经是待机位，直接运行Y轴坐标
                    //运行到目标位置Z轴-20MM座标
                    if( robot_msg.type.router_msg.station != ZB_STA ){
                        dist = config_get_ptr()->station_pmt.depot_in.y;
                        LOG_D("No5. Y-axis move to %s-H %d.%03d mm speed %d.%03d mm",\
                            station_name[robot_msg.type.router_msg.station],\
                            (int)(dist),\
                            float_to_int(dist),\
                            (int)config_get_ptr()->station_pmt.speed.y,\
                            float_to_int(config_get_ptr()->station_pmt.speed.y));
                        
                        robot_move_distance(ROBOT_Y_AXIS, dist, config_get_ptr()->station_pmt.speed.y);
                        if( motor_wait_stop((motor_no_t)ROBOT_Y_AXIS,MOTOR_TIMEOUT) == 3){
                            continue;
                        }
                    }
                    
                    if( robot_msg.type.router_msg.station == KZ_STA || robot_msg.type.router_msg.station == JC_STA ||\
                        robot_msg.type.router_msg.station == ZB_STA){
                        dist  = station_target.z + config_get_ptr()->avoid_hight;
                        dist>ROBOT_Z_UP_MAX?dist=ROBOT_Z_UP_MAX:0;
                    }
                    else{
                        dist  = station_target.z;
                    }
                    //移动Y轴到目标位置坐标
                    LOG_D("No6. Z-axis move to %s %d.%03d mm speed %d.%03d mm",\
                        station_name[robot_msg.type.router_msg.station],\
                        (int)dist,\
                        float_to_int(dist),\
                        (int)config_get_ptr()->station_pmt.speed.z,\
                        float_to_int(config_get_ptr()->station_pmt.speed.z));
                    
                    robot_move_distance(ROBOT_Z_AXIS, dist, config_get_ptr()->station_pmt.speed.z);
                    if( motor_wait_stop((motor_no_t)ROBOT_Z_AXIS,MOTOR_TIMEOUT) == 3){
                        continue;
                    }
                    
                    //移动到Y轴目标坐标上方
                    dist  = station_target.y;
                    speed = config_get_ptr()->station_pmt.speed.y;
                    LOG_D("No7. Y-axis move to %s %d.%03d mm speed %d.%03d mm",\
                        station_name[robot_msg.type.router_msg.station],\
                        (int)dist,\
                        float_to_int(dist),\
                        (int)(speed),\
                        float_to_int(speed));
                    
                    robot_move_distance(ROBOT_Y_AXIS, dist, speed);
                    if( motor_wait_stop((motor_no_t)ROBOT_Y_AXIS,MOTOR_TIMEOUT) == 3){
                        continue;
                    }
                    
                    //除待机位外，其它位置的最后下降速度减半
                    if( robot_msg.type.router_msg.station == DJ_STA ){
                        speed = config_get_ptr()->station_pmt.speed.z;
                    }
                    else{
                        speed = config_get_ptr()->station_pmt.speed.z/2;
                    }
                    
                    dist  = station_target.z;
                    LOG_D("No8. Z-axis move to %s %d.%03d mm speed %d.%03d mm",\
                        station_name[robot_msg.type.router_msg.station],\
                        (int)dist,\
                        float_to_int(dist),\
                        (int)(speed),\
                        float_to_int(speed));
                    
                    robot_move_distance(ROBOT_Z_AXIS, dist, speed);
                    if( motor_wait_stop((motor_no_t)ROBOT_Z_AXIS,MOTOR_TIMEOUT) == 3){
                        continue;
                    }

                    robot_exe_exit:
                    robot_state.curr_station = robot_msg.type.router_msg.station;
                    
                    comm_reply(EXE_OK,RBT_NO_ERR,STATION_MOVE_CMD,RT_NULL,0);
                }
            }
            //手抓控制
            else if( robot_msg.robot_cmd == ROBOT_CMD_CLAMP ){
                //抓取
                 if( robot_msg.type.clamp_msg.clamp == CLAMP_TAKE ){
                     robot_clamp(1);
                     rt_thread_delay(500);
                     if( clamp_status()==CLAMP_TAKE_UP ){
                         comm_reply(EXE_OK,RBT_NO_ERR,STATION_CLAMP_CMD,RT_NULL,0);
                         LOG_D("robot clamp take up OK");
                     }
                     else{
                         comm_reply(EXE_OK,RBT_CLAMP_ERR,STATION_CLAMP_CMD,RT_NULL,0);
                         LOG_E("robot clamp take up error");
                     }
                 }
                 //释放
                 else if( robot_msg.type.clamp_msg.clamp == CLAMP_PUT ){
                     robot_clamp(0);
                     rt_thread_delay(500);
                     if( clamp_status()==CLAMP_PUT_DOWN ){
                         comm_reply(EXE_OK,RBT_NO_ERR,CLAMP_CMD,RT_NULL,0);
                         LOG_D("robot clamp put down OK");
                     }
                     else{
                         comm_reply(EXE_OK,RBT_CLAMP_ERR,CLAMP_CMD,RT_NULL,0);
                         LOG_E("robot clamp put down error");
                     }
                }
                else if( robot_msg.type.clamp_msg.clamp == CLAMP_RESET ){
                    clamp_power_up();
                    comm_reply(EXE_OK,RBT_NO_ERR,CLAMP_CMD,RT_NULL,0);
                    LOG_D("robot clamp reset");
                }
            }
            //仓位移位
            else if( robot_msg.robot_cmd == ROBOT_CMD_DEPOT ){
                
                float offset = 0;
                static const uint8_t depot_cmd[] = {DEPOT_IN_CMD,DEPOT_A_CMD,DEPOT_B_CMD};
                if( robot_msg.type.depot_msg.axle == ROBOT_IN_AXIS ){
                    offset = config_get_ptr()->depot_offset.depot_in;
                }
                else if( robot_msg.type.depot_msg.axle == ROBOT_A_AXIS ){
                    offset = config_get_ptr()->depot_offset.depot_a;
                }
                else if( robot_msg.type.depot_msg.axle == ROBOT_B_AXIS ){
                    offset = config_get_ptr()->depot_offset.depot_b;
                }
                
                if( robot_msg.type.depot_msg.loc == 0x00 ){
                    dist = config_get_ptr()->depot_pmt[robot_msg.type.depot_msg.axle-ROBOT_IN_AXIS].door;
                }
                else if( robot_msg.type.depot_msg.loc == 0x01 ){
                    dist = config_get_ptr()->depot_pmt[robot_msg.type.depot_msg.axle-ROBOT_IN_AXIS].clamp + offset*robot_msg.type.depot_msg.station;
                }
                else if( robot_msg.type.depot_msg.loc == 0x02 ){
                    dist = config_get_ptr()->depot_pmt[robot_msg.type.depot_msg.axle-ROBOT_IN_AXIS].chip + offset*robot_msg.type.depot_msg.station;
                }
                else if( robot_msg.type.depot_msg.loc == 0x03 ){
                    dist = config_get_ptr()->depot_pmt[robot_msg.type.depot_msg.axle-ROBOT_IN_AXIS].scaner + offset*robot_msg.type.depot_msg.station;
                }
                
                speed  = config_get_ptr()->depot_pmt[robot_msg.type.depot_msg.loc].speed;
                
                if( robot_msg.type.depot_msg.action == ACT_MOVE ){
                    
                    robot_move_distance(robot_msg.type.depot_msg.axle, dist, speed);
                    motor_wait_stop(robot_msg.type.depot_msg.axle,MOTOR_TIMEOUT);
                }
                else if( robot_msg.type.depot_msg.action == ACT_STOP ){
                    
                    motor_stop(robot_msg.type.depot_msg.axle);
                }
                
                if( robot_msg.type.depot_msg.axle == ROBOT_IN_AXIS ){
                    LOG_D("depot in move to %d.%d",(int)dist,float_to_int(dist));
                }
                else if( robot_msg.type.depot_msg.axle == ROBOT_A_AXIS ){
                    LOG_D("depot a move to %d.%d",(int)dist,float_to_int(dist));
                }
                else if( robot_msg.type.depot_msg.axle == ROBOT_B_AXIS ){
                    LOG_D("depot b move to %d.%d",(int)dist,float_to_int(dist));
                }
                
                comm_reply(EXE_OK,RBT_NO_ERR,depot_cmd[robot_msg.type.depot_msg.axle-ROBOT_IN_AXIS],RT_NULL,0);
            }
            //电机原点初始化
            else if( robot_msg.robot_cmd == ROBOT_CMD_INIT ){
                if( robot_reset(robot_msg.type.init_msg.motor_no) == RT_EOK ){
                    LOG_D("motor %d reset OK",robot_msg.type.init_msg.motor_no);
                }
                else{
                    LOG_E("motor %d reset failed",robot_msg.type.init_msg.motor_no);
                }
            }
        }
    }
}

比较消息队列发送的运动控制命令，例如单轴移动调试，运动路径路由，电机的初始化，手抓的控制，都将调用电机移位控制函数，执行成功与否通过comm_replay返回执行结果，电机移位控制功能可参考我博客另一篇关于步进电机S曲线计算https://editor.csdn.net/md/?articleId=132959818