下面的为适配Tinymal-B的上位机下发遥控指令协议，开源的上位机代码需要自行确认修改：
def send_float(tx_Buf,data):
temp_B= struct.pack('f',float(data))
tx_Buf.append(temp_B[0])
tx_Buf.append(temp_B[1])
tx_Buf.append(temp_B[2])
tx_Buf.append(temp_B[3])
def send_int(tx_Buf,data):
temp_B= struct.pack('i',int(data))
tx_Buf.append(temp_B[0])
tx_Buf.append(temp_B[1])
tx_Buf.append(temp_B[2])
tx_Buf.append(temp_B[3])
def send_char(tx_Buf,data):
tx_Buf.append(int(data))
def uart_formate_ocu_spd_remote():#速度遥控命令
global rx_data,tx_data,rx_cnt,rx_num_now,uartState,print_cnt,BYTE0,BYTE1,BYTE2,BYTE3
global Baudrate,COM,Stopbits,ser,ReadUARTThread,cond_d,stop_d
sum = 0
_cnt = 0
data_to_send = []
data_to_send.append(0xAA)
data_to_send.append(0xAF)
data_to_send.append(0x32)#机器人参数
data_to_send.append(0)
send_char(data_to_send,gl.get_value('OCU_UPMODE'))#
send_char(data_to_send,gl.get_value('RCORD_EN'))#
send_float(data_to_send,gl.get_value('OCU_SPD_XY')[0])#
send_float(data_to_send,gl.get_value('OCU_SPD_XY')[1])#
send_float(data_to_send,gl.get_value('OCU_ATT_PR')[0])#
send_float(data_to_send,gl.get_value('OCU_ATT_PR')[1])#
send_float(data_to_send,gl.get_value('OCU_YAW_RATE'))#
send_char(data_to_send,gl.get_value('OCU_CMD_START'))#
send_char(data_to_send,gl.get_value('OCU_CMD_BACK'))#
send_int(data_to_send,gl.get_value('OCU_CMD_LR'))#
send_int(data_to_send,gl.get_value('OCU_CMD_FB'))#
send_char(data_to_send,gl.get_value('OCU_CMD_X'))#
send_char(data_to_send,gl.get_value('OCU_CMD_Y'))#
send_char(data_to_send,gl.get_value('OCU_CMD_B'))#
send_char(data_to_send,gl.get_value('OCU_CMD_A'))#
send_char(data_to_send,gl.get_value('OCU_CMD_LL'))#
send_char(data_to_send,gl.get_value('OCU_CMD_RR'))#
#print(gl.get_value('OCU_UPMODE'))
_cnt = len(data_to_send)
data_to_send[3] = _cnt - 4
for i in range(0,_cnt):
sum = sum+ data_to_send[i]
data_to_send.append(BYTE0(int(sum)))
tx_data[0:len(data_to_send)]= data_to_send
gl.set_value('UART_TX_LEN',len(data_to_send))
return len(data_to_send)下面为机器人反馈发送到上位机的接口：
void data_per_usb_robot_state1(void)
{
int i; u8 sum = 0;
u16 _cnt=0;
int cnt_reg=0;
static float timer=0;
timer+=0.2;
//printf("ss\n");
cnt_reg=usb_send_cnt;
SendBuff_USB[usb_send_cnt++]=0xBA;
SendBuff_USB[usb_send_cnt++]=0xBF;
SendBuff_USB[usb_send_cnt++]=0x04;
SendBuff_USB[usb_send_cnt++]=0;
//printf("att0=%f att1=%f att2=%f dt=%f\n",nav_rx.att_now[0],nav_rx.att_now[1],nav_rx.att_now[2], Get_Cycle_T(0));
setDataFloat(nav_rx.att_now[0]);//vmc_all.att[PITr]);//setDataFloat(5*sin(timer));//vmc_all.att[PITr]);
setDataFloat(nav_rx.att_now[1]);//vmc_all.att[ROLr]);
setDataFloat(nav_rx.att_now[2]);//vmc_all.att[YAWr]);
setDataFloat(nav_rx.datt_now[0]);//vmc_all.att[PITr]);
setDataFloat(nav_rx.datt_now[1]);//vmc_all.att[ROLr]);
setDataFloat(nav_rx.datt_now[2]);//vmc_all.att[YAWr]);
setDataFloat(nav_rx.com_n_now[0]);
setDataFloat(nav_rx.com_n_now[1]);
setDataFloat(nav_rx.com_n_now[2]);
setDataFloat(nav_rx.dcom_n_now[0]);
setDataFloat(nav_rx.dcom_n_now[1]);
setDataFloat(nav_rx.dcom_n_now[2]);
setDataFloat(nav_rx.acc_n_now[0]);
setDataFloat(nav_rx.acc_n_now[1]);
setDataFloat(nav_rx.acc_n_now[2]);
setDataFloat(nav_rx.epos_n_now[2].x);
setDataFloat(nav_rx.epos_n_now[2].y);
setDataFloat(nav_rx.epos_n_now[2].z);
setDataFloat(nav_rx.epos_n_now[0].x);
setDataFloat(nav_rx.epos_n_now[0].y);
setDataFloat(nav_rx.epos_n_now[0].z);
setDataFloat(0);//(bat.percent);
setDataFloat(0);//Rc_Get.signal_rate);
setDataFloat(nav_rx.dcom_n_tar[0]);
setDataFloat(nav_rx.dcom_n_tar[1]);
setDataFloat(nav_rx.dcom_n_tar[2]);
setDataFloat(fabs(nav_rx.com_n_tar[2]));
SendBuff_USB[usb_send_cnt++]=nav_rx.gait_state;
//printf("nav_rx.gait_state=%d\n",nav_rx.gait_state);
SendBuff_USB[cnt_reg+3] =(usb_send_cnt-cnt_reg)-4;
for( i=cnt_reg;i<usb_send_cnt;i++)
sum += SendBuff_USB[i];
SendBuff_USB[usb_send_cnt++] = sum;
}
void data_per_usb_robot_state2(void)
{
int i; u8 sum = 0;
u16 _cnt=0,cnt_reg;
cnt_reg=usb_send_cnt;
SendBuff_USB[usb_send_cnt++]=0xBA;
SendBuff_USB[usb_send_cnt++]=0xBF;
SendBuff_USB[usb_send_cnt++]=0x41;
SendBuff_USB[usb_send_cnt++]=0;
setDataFloat(nav_rx.epos_n_now[3].x);
setDataFloat(nav_rx.epos_n_now[3].y);
setDataFloat(nav_rx.epos_n_now[3].z);
setDataFloat(nav_rx.epos_n_now[1].x);
setDataFloat(nav_rx.epos_n_now[1].y);
setDataFloat(nav_rx.epos_n_now[1].z);
char id=2;
if(nav_rx.leg_connect[id] &&nav_rx.motor_connect[id][0]&&nav_rx.motor_ready[id][0])
setDataFloat(nav_rx.sita_now[id][0]);
else{
if(!nav_rx.leg_connect[id])
setDataFloat(999);
else if(!nav_rx.motor_connect[id][0])
setDataFloat(666);
else
setDataFloat(333);
}
if(nav_rx.leg_connect[id] &&nav_rx.motor_connect[id][1]&&nav_rx.motor_ready[id][1])
setDataFloat(nav_rx.sita_now[id][1]);
else{
if(!nav_rx.leg_connect[id])
setDataFloat(999);
else if(!nav_rx.motor_connect[id][1])
setDataFloat(666);
else
setDataFloat(333);
}
if(nav_rx.leg_connect[id] &&nav_rx.motor_connect[id][2]&&nav_rx.motor_ready[id][2])
setDataFloat(nav_rx.sita_now[id][2]);
else{
if(!nav_rx.leg_connect[id])
setDataFloat(999);
else if(!nav_rx.motor_connect[id][2])
setDataFloat(666);
else
setDataFloat(333);
}
id=0;
if(nav_rx.leg_connect[id] &&nav_rx.motor_connect[id][0]&&nav_rx.motor_ready[id][0])
setDataFloat(nav_rx.sita_now[id][0]);
else{
if(!nav_rx.leg_connect[id])
setDataFloat(999);
else if(!nav_rx.motor_connect[id][0])
setDataFloat(666);
else
setDataFloat(333);
}
if(nav_rx.leg_connect[id] &&nav_rx.motor_connect[id][1]&&nav_rx.motor_ready[id][1])
setDataFloat(nav_rx.sita_now[id][1]);
else{
if(!nav_rx.leg_connect[id])
setDataFloat(999);
else if(!nav_rx.motor_connect[id][1])
setDataFloat(666);
else
setDataFloat(333);
}
if(nav_rx.leg_connect[id] &&nav_rx.motor_connect[id][2]&&nav_rx.motor_ready[id][2])
setDataFloat(nav_rx.sita_now[id][2]);
else{
if(!nav_rx.leg_connect[id])
setDataFloat(999);
else if(!nav_rx.motor_connect[id][2])
setDataFloat(666);
else
setDataFloat(333);
}
id=3;
if(nav_rx.leg_connect[id] &&nav_rx.motor_connect[id][0]&&nav_rx.motor_ready[id][0])
setDataFloat(nav_rx.sita_now[id][0]);
else{
if(!nav_rx.leg_connect[id])
setDataFloat(999);
else if(!nav_rx.motor_connect[id][0])
setDataFloat(666);
else
setDataFloat(333);
}
if(nav_rx.leg_connect[id] &&nav_rx.motor_connect[id][1]&&nav_rx.motor_ready[id][1])
setDataFloat(nav_rx.sita_now[id][1]);
else{
if(!nav_rx.leg_connect[id])
setDataFloat(999);
else if(!nav_rx.motor_connect[id][1])
setDataFloat(666);
else
setDataFloat(333);
}
if(nav_rx.leg_connect[id] &&nav_rx.motor_connect[id][2]&&nav_rx.motor_ready[id][2])
setDataFloat(nav_rx.sita_now[id][2]);
else{
if(!nav_rx.leg_connect[id])
setDataFloat(999);
else if(!nav_rx.motor_connect[id][2])
setDataFloat(666);
else
setDataFloat(333);
}
id=1;
if(nav_rx.leg_connect[id] &&nav_rx.motor_connect[id][0]&&nav_rx.motor_ready[id][0])
setDataFloat(nav_rx.sita_now[id][0]);
else{
if(!nav_rx.leg_connect[id])
setDataFloat(999);
else if(!nav_rx.motor_connect[id][0])
setDataFloat(666);
else
setDataFloat(333);
}
if(nav_rx.leg_connect[id] &&nav_rx.motor_connect[id][1]&&nav_rx.motor_ready[id][1])
setDataFloat(nav_rx.sita_now[id][1]);
else{
if(!nav_rx.leg_connect[id])
setDataFloat(999);
else if(!nav_rx.motor_connect[id][1])
setDataFloat(666);
else
setDataFloat(333);
}
if(nav_rx.leg_connect[id] &&nav_rx.motor_connect[id][2]&&nav_rx.motor_ready[id][2])
setDataFloat(nav_rx.sita_now[id][2]);
else{
if(!nav_rx.leg_connect[id])
setDataFloat(999);
else if(!nav_rx.motor_connect[id][2])
setDataFloat(666);
else
setDataFloat(333);
}
setDataFloat(vmc[2].param.sita1_off);
setDataFloat(vmc[2].param.sita2_off);
setDataFloat(vmc[2].param.sita3_off);
setDataFloat(vmc[0].param.sita1_off);
setDataFloat(vmc[0].param.sita2_off);
setDataFloat(vmc[0].param.sita3_off);
setDataFloat(vmc[3].param.sita1_off);
setDataFloat(vmc[3].param.sita2_off);
setDataFloat(vmc[3].param.sita3_off);
setDataFloat(vmc[1].param.sita1_off);
setDataFloat(vmc[1].param.sita2_off);
setDataFloat(vmc[1].param.sita3_off);
if((vmc_all.gait_mode==STAND_IMU||vmc_all.gait_mode==STAND_RC||vmc_all.gait_mode==TROT)){
SendBuff_USB[usb_send_cnt++]=nav_rx.is_ground[2];
SendBuff_USB[usb_send_cnt++]=nav_rx.is_ground[0];
SendBuff_USB[usb_send_cnt++]=nav_rx.is_ground[1];
SendBuff_USB[usb_send_cnt++]=nav_rx.is_ground[3];
}
else{
SendBuff_USB[usb_send_cnt++]=nav_rx.is_touch[2];
SendBuff_USB[usb_send_cnt++]=nav_rx.is_touch[0];
SendBuff_USB[usb_send_cnt++]=nav_rx.is_touch[1];
SendBuff_USB[usb_send_cnt++]=nav_rx.is_touch[3];
}
SendBuff_USB[cnt_reg+3] =(usb_send_cnt-cnt_reg)-4;
for( i=cnt_reg;i<usb_send_cnt;i++)
sum += SendBuff_USB[i];
SendBuff_USB[usb_send_cnt++] = sum;
}开源上位机代码：
链接：https://pan.baidu.com/s/1nyoVk6O6_VOFyVFtxTCPRQ
提取码：npzc
--来自百度网盘超级会员V3的分享
