-
机械臂运动控制，通讯的解包-＞运动控制-＞数据封包上报过程
一、协议
数据格式为小端模式，浮点数格式为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); } } } }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
- 125
- 126
- 127
- 128
- 129
- 130
- 131
- 132
- 133
- 134
- 135
- 136
- 137
- 138
- 139
- 140
- 141
- 142
- 143
- 144
- 145
- 146
- 147
- 148
- 149
- 150
- 151
- 152
- 153
- 154
- 155
- 156
- 157
- 158
- 159
- 160
- 161
- 162
- 163
- 164
- 165
- 166
- 167
- 168
- 169
- 170
- 171
- 172
- 173
- 174
- 175
- 176
- 177
- 178
- 179
- 180
- 181
- 182
- 183
- 184
- 185
- 186
- 187
- 188
- 189
- 190
- 191
- 192
- 193
- 194
- 195
- 196
- 197
- 198
- 199
- 200
- 201
- 202
- 203
- 204
- 205
- 206
- 207
- 208
- 209
- 210
- 211
- 212
- 213
- 214
- 215
- 216
- 217
- 218
- 219
- 220
- 221
- 222
- 223
- 224
- 225
- 226
- 227
- 228
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)); } }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
typedef struct _depot_pmt_t{ float speed; float door; float clamp; float chip; float scaner; }depot_pmt_t;- 1
- 2
- 3
- 4
- 5
- 6
- 7
这个函数传输各个工位的座标数据到下位机进行保存，传输格式为浮点，单位是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); } }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
所有命令的上报为统一格式，填入状态，错误码，功能码，及命令的私有数据，调用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); }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
解释移动命令，提取运行座标及速度数据，建立一个消息，将数据放入消息中，由运动线程接收消息将进行电机的控制
五、运动控制
/** * 机械臂动作执行线程. * . * @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); } } } } }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
- 125
- 126
- 127
- 128
- 129
- 130
- 131
- 132
- 133
- 134
- 135
- 136
- 137
- 138
- 139
- 140
- 141
- 142
- 143
- 144
- 145
- 146
- 147
- 148
- 149
- 150
- 151
- 152
- 153
- 154
- 155
- 156
- 157
- 158
- 159
- 160
- 161
- 162
- 163
- 164
- 165
- 166
- 167
- 168
- 169
- 170
- 171
- 172
- 173
- 174
- 175
- 176
- 177
- 178
- 179
- 180
- 181
- 182
- 183
- 184
- 185
- 186
- 187
- 188
- 189
- 190
- 191
- 192
- 193
- 194
- 195
- 196
- 197
- 198
- 199
- 200
- 201
- 202
- 203
- 204
- 205
- 206
- 207
- 208
- 209
- 210
- 211
- 212
- 213
- 214
- 215
- 216
- 217
- 218
- 219
- 220
- 221
- 222
- 223
- 224
- 225
- 226
- 227
- 228
- 229
- 230
- 231
- 232
- 233
- 234
- 235
- 236
- 237
- 238
- 239
- 240
- 241
- 242
- 243
- 244
- 245
- 246
- 247
- 248
- 249
- 250
- 251
- 252
- 253
- 254
- 255
- 256
- 257
- 258
- 259
- 260
- 261
- 262
- 263
- 264
- 265
- 266
- 267
- 268
- 269
- 270
- 271
- 272
- 273
- 274
- 275
- 276
- 277
- 278
- 279
- 280
- 281
- 282
- 283
- 284
- 285
- 286
- 287
- 288
- 289
- 290
- 291
- 292
- 293
- 294
- 295
- 296
- 297
- 298
- 299
- 300
- 301
- 302
- 303
- 304
- 305
- 306
- 307
- 308
- 309
- 310
- 311
- 312
- 313
- 314
- 315
- 316
- 317
- 318
- 319
- 320
- 321
- 322
- 323
比较消息队列发送的运动控制命令，例如单轴移动调试，运动路径路由，电机的初始化，手抓的控制，都将调用电机移位控制函数，执行成功与否通过comm_replay返回执行结果，电机移位控制功能可参考我博客另一篇关于步进电机S曲线计算https://editor.csdn.net/md/?articleId=132959818
-
相关阅读:
论文-分布式-拜占庭将军问题
（附源码）springboot跨境电商系统 毕业设计 211003
机器学习 | MATLAB实现GLM广义线性模型参数设定
2022年9月19日--9月25日(ue4热更新视频教程为主，)
打开回收站提示“回收站已损坏是否清空该驱动器上的回收站“解决方法
ffmpeg分离左右声道到多音轨
Linux 线程同步、互斥锁、避免死锁、条件变量
网络安全笔记 -- RCE代码及命令执行漏洞
ThinkPHP V6.0.12在php8.1下验证码出现问题
MyBatis入门详解
- 原文地址：https://blog.csdn.net/dmjkun/article/details/133633927
