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

一、协议
在这里插入图片描述
数据格式为小端模式，浮点数格式为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;        //修改波特率为 9600config.data_bits = DATA_BITS_8;           //数据位 8config.stop_bits = STOP_BITS_1;           //停止位 1config.bufsz     = RECV_BUF_LEN;            //修改缓冲区 buff size 为 128config.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:                //废料位1station_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:                //废料位2station_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

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.xdnf.cn/news/150492.html

如若内容造成侵权/违法违规/事实不符，请联系一条长河网进行投诉反馈，一经查实，立即删除！

相关文章

机器学习：随机森林

机器学习：随机森林

集成学习 集成学习&#xff08;Ensemble Learning&#xff09;是一种机器学习方法&#xff0c;通过将多个基本学习算法的预测结果进行组合&#xff0c;以获得更好的预测性能。集成学习的基本思想是通过结合多个弱分类器或回归器的预测结果&#xff0c;来构建一个更强大的集成模…
阅读更多...
springboot+jsp+ssm高校图书馆图书借阅收藏评论管理系统617w1

springboot+jsp+ssm高校图书馆图书借阅收藏评论管理系统617w1

本图书管理系统系统采用B/S架构&#xff0c;数据库是MySQL&#xff0c;网站的搭建与开发采用了先进的Java进行编写&#xff0c;使用了SSM&#xff08;Spring、SpringMVC、Mybits&#xff09;框架。该系统从两个对象&#xff1a;由管理员和用户来对系统进行设计构建。前台主要功…
阅读更多...
面试题：在大型分布式系统中，给你一条 SQL，让你优化，你会怎么做？

面试题：在大型分布式系统中，给你一条 SQL，让你优化，你会怎么做？

亲爱的小伙伴们&#xff0c;大家好呀&#xff01;我是小米&#xff0c;一个热爱技术、乐于分享的90后程序猿。今天&#xff0c;我要和大家聊聊一个在大型分布式系统中非常有趣和挑战性的话题——如何优化 SQL 查询&#xff01; 这个问题可不简单&#xff0c;但不要担心&#x…
阅读更多...
python练习4

python练习4

前言&#xff1a;相信看到这篇文章的小伙伴都或多或少有一些编程基础&#xff0c;懂得一些linux的基本命令了吧&#xff0c;本篇文章将带领大家服务器如何部署一个使用django框架开发的一个网站进行云服务器端的部署。 文章使用到的的工具 Python&#xff1a;一种编程语言&…
阅读更多...
Vue3最佳实践 第七章 TypeScript 创建Trello 任务管理器

Vue3最佳实践 第七章 TypeScript 创建Trello 任务管理器

| ​ 我们将探讨如何使用Vue.js从零开始创建一个类似于Trello的任务管理应用程序。如果你不熟悉Trello&#xff0c;它是一款非常流行的任务管理工具&#xff0c;允许你把任务写在卡片上&#xff0c;然后通过一个看板的方式来直观地管理这些任务。Trello不仅可以用于个人的任务…
阅读更多...
电子地图 | VINS-FUSION | 小觅相机D系列

电子地图 | VINS-FUSION | 小觅相机D系列

目录 一、相关介绍 二、VINS-FUSION环境安装及使用 &#xff08;一&#xff09;Ubuntu18.04安装配置 1、Ubuntu下载安装 2、设置虚拟内存&#xff08;可选&#xff09; &#xff08;二&#xff09;VINS-FUSION环境配置 1、ros安装 2、ceres-solver安装 3、vins-fusion…
阅读更多...
JavaScript中的map()和forEach()方法有什么区别？

JavaScript中的map()和forEach()方法有什么区别？

聚沙成塔每天进步一点点 ⭐ 专栏简介 前端入门之旅&#xff1a;探索Web开发的奇妙世界 欢迎来到前端入门之旅&#xff01;感兴趣的可以订阅本专栏哦&#xff01;这个专栏是为那些对Web开发感兴趣、刚刚踏入前端领域的朋友们量身打造的。无论你是完全的新手还是有一些基础的开发…
阅读更多...
工信部教考中心：什么是《研发效能(DevOps)工程师》认证，拿到证书之后有什么作用！（上篇）丨IDCF

工信部教考中心：什么是《研发效能(DevOps)工程师》认证，拿到证书之后有什么作用！（上篇）丨IDCF

在计算机行业中&#xff0c;资质认证可以证明在该领域内的专业能力和知识水平。各种技术水平认证也是层出不穷&#xff0c;而考取具有公信力和权威性的认证是从业者的首选。同时&#xff0c;随着国内企业技术实力的提升和国家对于自主可控的重视程度不断提高&#xff0c;国产证…
阅读更多...
基于Java的教学评价管理系统设计与实现(源码+lw+部署文档+讲解等)

基于Java的教学评价管理系统设计与实现(源码+lw+部署文档+讲解等)

文章目录 前言系统功能结构图系统ER图具体实现截图论文参考详细视频演示为什么选择我自己的网站自己的小程序&#xff08;小蔡coding&#xff09;有保障的售后福利 代码参考源码获取 前言 &#x1f497;博主介绍&#xff1a;✌全网粉丝10W,CSDN特邀作者、博客专家、CSDN新星计划…
阅读更多...
Flink+Doris 实时数仓

Flink+Doris 实时数仓

Flink+Doris 实时数仓 Doris基本原理 Doris基本架构非常简单，只有FE(Frontend)、BE(Backend)两种角色，不依赖任何外部组件，对部署和运维非常友好。架构图如下 可以 看到Doris 的数仓架构十分简洁，不依赖 Hadoop 生态组件，构建及运维成本较低。 FE（Frontend）以 Java 语…
阅读更多...
用 Pytorch 自己构建一个Transformer

用 Pytorch 自己构建一个Transformer

一、说明 用pytorch自己构建一个transformer并不是难事，本篇使用pytorch随机生成五千个32位数的词向量做为源语言词表，再生成五千个32位数的词向量做为目标语言词表，让它们模拟翻译过程，transformer全部用pytorch实现，具备一定实战意义。 二、论文和概要 …
阅读更多...
【数据结构--八大排序】之希尔排序

【数据结构--八大排序】之希尔排序

&#x1f490; &#x1f338; &#x1f337; &#x1f340; &#x1f339; &#x1f33b; &#x1f33a; &#x1f341; &#x1f343; &#x1f342; &#x1f33f; &#x1f344;&#x1f35d; &#x1f35b; &#x1f364; &#x1f4c3;个人主页 &#xff1a;阿然成长日记 …
阅读更多...
STM32--人体红外感应开关

STM32--人体红外感应开关

本文主要介绍基于STM32F103C8T6和人体红外感应开关实现的控制算法 简介 人体红外模块选用HC-SR501人体红外传感器&#xff0c;人体红外感应的主要器件为人体热释电红外传感器。人体都有恒定的体温&#xff0c;一般在36~37度&#xff0c;所以会发出特定波长的红外线&#xff0…
阅读更多...
Mac上protobuf环境构建-java

Mac上protobuf环境构建-java

参考文献 getting-started 官网pb java介绍 maven protobuf插件 简单入门1 简单入门2 1. protoc编译器下载安装 https://github.com/protocolbuffers/protobuf/releases?page10 放入.zshrc中配置环境变量  ~/IdeaProjects/test2/ protoc --version libprotoc 3.12.1  …
阅读更多...
国庆假期作业6

国庆假期作业6

一、ARM的工作模式 1、非特权模式 user模式&#xff1a;非特权模式&#xff0c;大部分任务执行在这种模式 2、特权模式 异常模式&#xff1a; FIQ : 当一个快速&#xff08;fast) 中断产生时将会进入这种模式 IRQ : 当一个通用&#xff08;normal) 中断产生时将会进入这种模式…
阅读更多...
中国企业400电话在线申请办理

中国企业400电话在线申请办理

在当今竞争激烈的商业环境中&#xff0c;企业需要寻求各种方式来提升客户服务和市场竞争力。而拥有一个专属的400电话号码&#xff0c;不仅可以为企业带来更多的商机&#xff0c;还能提升企业形象和客户满意度。本文将介绍如何在线申请办理中国企业400电话&#xff0c;并提供一…
阅读更多...
总结一：C++面经（五万字长文）

总结一：C++面经（五万字长文）

文章目录 一、C基础部分1、C特点。2、说说C语言和C的区别。3、说说 C中 struct 和 class 的区别。4、 include头文件的顺序以及双引号""和尖括号<>的区别。5、说说C结构体和C结构体的区别。6、导入C函数的关键字是什么&#xff0c;C编译时和C有什么不同&#x…
阅读更多...
EV证书与OV证书的区别

EV证书与OV证书的区别

在保护网站和用户数据的过程中&#xff0c;选择适当的SSL证书至关重要。EV&#xff08;Extended Validation&#xff09;证书和OV&#xff08;Organization Validation&#xff09;证书是SSL证书的两种常见类型&#xff0c;它们在验证过程和信任指示方面有着显著的区别。让我们…
阅读更多...
HDLbits: ece241 2014 q4

HDLbits: ece241 2014 q4

module top_module (input clk,input x,output z ); reg [2:0] Q;always(posedge clk)beginQ[0] < Q[0] ^ x;Q[1] < (~Q[1]) & x;Q[2] < (~Q[2]) | x;z < ~(| Q[2:0]); //错误&#xff01;&#xff01;&#xff01;&#xff01;endendmodule 正确答案&#xf…
阅读更多...
Java基于SpringBoot的车辆充电桩

Java基于SpringBoot的车辆充电桩

博主介绍&#xff1a;✌程序员徐师兄、7年大厂程序员经历。全网粉丝30W,Csdn博客专家、掘金/华为云/阿里云/InfoQ等平台优质作者、专注于Java技术领域和毕业项目实战✌ 文章目录 1、效果演示效果图 技术栈2、 前言介绍&#xff08;完整源码请私聊&#xff09;3、主要技术3.4.1…
阅读更多...
最新文章

Copyright @ 2022~2023