一、协议
数据格式为小端模式，浮点数格式为IEEE754，需与上位机的PC端一致，如window系统，其它系统需要自行测试，用于传输16位、32位、float数据格式，避免只传输字节数据带来转换的繁琐及精度丢失。
二、下位机数据接收线程
csharp
/**
* 通讯主线程.
* .
* @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 接收一帧完整数据包标记，然后进行校验，判断功能码进行命令处理流转
csharp
/**
* 设置工位座标.
* .
* @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));
}
}
csharp
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;
三、下位机数据上报
csharp
/**
* 通讯状态返回.
* .
* @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送
四、运动执行
csharp
/**
* 移动运动轴.
* .
* @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);
}解释移动命令，提取运行座标及速度数据，建立一个消息，将数据放入消息中，由运动线程接收消息将进行电机的控制
五、运动控制
csharp
/**
* 机械臂动作执行线程.
* .
* @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