V2.1.0
1. add thirdparty 2. add State_JointTraj protect when qState != qCmd
This commit is contained in:
Agnel Wang
parent
5f38f0f3de
commit
0fd1ae9b7b
|
|
@ -8,4 +8,5 @@
|
|||
<limitT>10.0</limitT> <!-- N*M -->
|
||||
<load>0.0</load><!-- kg-->
|
||||
</collision>
|
||||
<!-- <plot set=1/> -->
|
||||
</configurartion>
|
||||
|
|
|
|||
|
|
@ -1,214 +0,0 @@
|
|||
#ifndef _UNITREE_ARM_ARM_MOTOR_COMMON_H_
|
||||
#define _UNITREE_ARM_ARM_MOTOR_COMMON_H_
|
||||
|
||||
#include <stdint.h>
|
||||
typedef int16_t q15_t;
|
||||
|
||||
#pragma pack(1)
|
||||
|
||||
// 发送用单个数据数据结构
|
||||
typedef union{
|
||||
int32_t L;
|
||||
uint8_t u8[4];
|
||||
uint16_t u16[2];
|
||||
uint32_t u32;
|
||||
float F;
|
||||
}COMData32;
|
||||
|
||||
typedef struct {
|
||||
// 定义 数据包头
|
||||
unsigned char start[2]; // 包头
|
||||
unsigned char motorID; // 电机ID 0,1,2,3 ... 0xBB 表示向所有电机广播（此时无返回）
|
||||
unsigned char reserved;
|
||||
}COMHead;
|
||||
|
||||
#pragma pack()
|
||||
|
||||
#pragma pack(1)
|
||||
|
||||
typedef struct {
|
||||
|
||||
uint8_t fan_d; // 关节上的散热风扇转速
|
||||
uint8_t Fmusic; // 电机发声频率 /64*1000 15.625f 频率分度
|
||||
uint8_t Hmusic; // 电机发声强度 推荐值4 声音强度 0.1 分度
|
||||
uint8_t reserved4;
|
||||
|
||||
uint8_t FRGB[4]; // 足端LED
|
||||
|
||||
}LowHzMotorCmd;
|
||||
|
||||
typedef struct { // 以 4个字节一组排列 ，不然编译器会凑整
|
||||
// 定义 数据
|
||||
uint8_t mode; // 关节模式选择
|
||||
uint8_t ModifyBit; // 电机控制参数修改位
|
||||
uint8_t ReadBit; // 电机控制参数发送位
|
||||
uint8_t reserved;
|
||||
|
||||
COMData32 Modify; // 电机参数修改 的数据
|
||||
//实际给FOC的指令力矩为：
|
||||
//K_P*delta_Pos + K_W*delta_W + T
|
||||
q15_t T; // 期望关节的输出力矩（电机本身的力矩）x256, 7 + 8 描述
|
||||
q15_t W; // 期望关节速度 （电机本身的速度） x128, 8 + 7描述
|
||||
int32_t Pos; // 期望关节位置 x 16384/6.2832, 14位编码器（主控0点修正，电机关节还是以编码器0点为准）
|
||||
|
||||
q15_t K_P; // 关节刚度系数 x2048 4+11 描述
|
||||
q15_t K_W; // 关节速度系数 x1024 5+10 描述
|
||||
|
||||
uint8_t LowHzMotorCmdIndex; // 电机低频率控制命令的索引, 0-7, 分别代表LowHzMotorCmd中的8个字节
|
||||
uint8_t LowHzMotorCmdByte; // 电机低频率控制命令的字节
|
||||
|
||||
COMData32 Res[1]; // 通讯 保留字节 用于实现别的一些通讯内容
|
||||
|
||||
}MasterComdV3; // 加上数据包的包头 和CRC 34字节
|
||||
|
||||
typedef struct {
|
||||
// 定义 电机控制命令数据包
|
||||
COMHead head;
|
||||
MasterComdV3 Mdata;
|
||||
COMData32 CRCdata;
|
||||
}MasterComdDataV3;//返回数据
|
||||
|
||||
// typedef struct {
|
||||
// // 定义 总得485 数据包
|
||||
|
||||
// MasterComdData M1;
|
||||
// MasterComdData M2;
|
||||
// MasterComdData M3;
|
||||
|
||||
// }DMA485TxDataV3;
|
||||
|
||||
#pragma pack()
|
||||
|
||||
#pragma pack(1)
|
||||
|
||||
typedef struct { // 以 4个字节一组排列 ，不然编译器会凑整
|
||||
// 定义 数据
|
||||
uint8_t mode; // 当前关节模式
|
||||
uint8_t ReadBit; // 电机控制参数修改 是否成功位
|
||||
int8_t Temp; // 电机当前平均温度
|
||||
uint8_t MError; // 电机错误 标识
|
||||
|
||||
COMData32 Read; // 读取的当前 电机 的控制数据
|
||||
int16_t T; // 当前实际电机输出力矩 7 + 8 描述
|
||||
|
||||
int16_t W; // 当前实际电机速度（高速） 8 + 7 描述
|
||||
float LW; // 当前实际电机速度（低速）
|
||||
|
||||
int16_t W2; // 当前实际关节速度（高速） 8 + 7 描述
|
||||
float LW2; // 当前实际关节速度（低速）
|
||||
|
||||
int16_t Acc; // 电机转子加速度 15+0 描述 惯量较小
|
||||
int16_t OutAcc; // 输出轴加速度 12+3 描述 惯量较大
|
||||
|
||||
int32_t Pos; // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准）
|
||||
int32_t Pos2; // 关节编码器位置(输出编码器)
|
||||
|
||||
int16_t gyro[3]; // 电机驱动板6轴传感器数据
|
||||
int16_t acc[3];
|
||||
|
||||
// 力传感器的数据
|
||||
int16_t Fgyro[3]; //
|
||||
int16_t Facc[3];
|
||||
int16_t Fmag[3];
|
||||
uint8_t Ftemp; // 8位表示的温度 7位（-28~100度） 1位0.5度分辨率
|
||||
|
||||
int16_t Force16; // 力传感器高16位数据
|
||||
int8_t Force8; // 力传感器低8位数据
|
||||
|
||||
uint8_t FError; // 足端传感器错误标识
|
||||
|
||||
int8_t Res[1]; // 通讯 保留字节
|
||||
|
||||
}ServoComdV3; // 加上数据包的包头 和CRC 78字节（4+70+4）
|
||||
|
||||
typedef struct {
|
||||
// 定义 电机控制命令数据包
|
||||
COMHead head;
|
||||
ServoComdV3 Mdata;
|
||||
|
||||
COMData32 CRCdata;
|
||||
|
||||
}ServoComdDataV3; //发送数据
|
||||
|
||||
// typedef struct {
|
||||
// // 定义 总的485 接受数据包
|
||||
|
||||
// ServoComdDataV3 M[3];
|
||||
// // uint8_t nullbyte1;
|
||||
|
||||
// }DMA485RxDataV3;
|
||||
|
||||
|
||||
#pragma pack()
|
||||
|
||||
// 00 00 00 00 00
|
||||
// 00 00 00 00 00
|
||||
// 00 00 00 00 00
|
||||
// 00 00 00
|
||||
// 数据包默认初始化
|
||||
// 主机发送的数据包
|
||||
/*
|
||||
Tx485Data[_FR][i].head.start[0] = 0xFE ; Tx485Data[_FR][i].head.start[1] = 0xEE; // 数据包头
|
||||
Tx485Data[_FR][i].Mdata.ModifyBit = 0xFF; Tx485Data[_FR][i].Mdata.mode = 0; // 默认不修改数据 和 电机的默认工作模式
|
||||
Tx485Data[_FR][i].head.motorID = i; 0 // 目标电机标号
|
||||
Tx485Data[_FR][i].Mdata.T = 0.0f; // 默认目标关节输出力矩 motor1.Extra_Torque = motorRxData[1].Mdata.T*0.390625f; // N.M 转化为 N.CM IQ8描述
|
||||
Tx485Data[_FR][i].Mdata.Pos = 0x7FE95C80; // 默认目标关节位置 不启用位置环 14位分辨率
|
||||
Tx485Data[_FR][i].Mdata.W = 16000.0f; // 默认目标关节速度 不启用速度环 1+8+7描述 motor1.Target_Speed = motorRxData[1].Mdata.W*0.0078125f; // 单位 rad/s IQ7描述
|
||||
Tx485Data[_FR][i].Mdata.K_P = (q15_t)(0.6f*(1<<11)); // 默认关节刚度系数 4+11 描述 motor1.K_Pos = ((float)motorRxData[1].Mdata.K_P)/(1<<11); // 描述刚度的通讯数据格式 4+11
|
||||
Tx485Data[_FR][i].Mdata.K_W = (q15_t)(1.0f*(1<<10)); // 默认关节速度系数 5+10 描述 motor1.K_Speed = ((float)motorRxData[1].Mdata.K_W)/(1<<10); // 描述阻尼的通讯数据格式 5+10
|
||||
*/
|
||||
|
||||
enum class MotorType{
|
||||
A1Go1, // 4.8M baudrate, K_W x1024
|
||||
B1 // 6.0M baudrate, K_W x512
|
||||
};
|
||||
|
||||
struct MOTOR_send{
|
||||
// 定义 发送格式化数据
|
||||
MasterComdDataV3 motor_send_data; //电机控制数据结构体
|
||||
MotorType motorType = MotorType::A1Go1;
|
||||
int hex_len = 34; //发送的16进制命令数组长度, 34
|
||||
// long long send_time; //发送该命令的时间, 微秒(us)
|
||||
// 待发送的各项数据
|
||||
unsigned short id; //电机ID，0xBB代表全部电机
|
||||
unsigned short mode; //0:空闲, 5:开环转动, 10:闭环FOC控制
|
||||
//实际给FOC的指令力矩为：
|
||||
//K_P*delta_Pos + K_W*delta_W + T
|
||||
float T; //期望关节的输出力矩（电机本身的力矩）（Nm）
|
||||
float W; //期望关节速度（电机本身的速度）(rad/s)
|
||||
float Pos; //期望关节位置（rad）
|
||||
float K_P; //关节刚度系数
|
||||
float K_W; //关节速度系数
|
||||
COMData32 Res; // 通讯 保留字节 用于实现别的一些通讯内容
|
||||
};
|
||||
|
||||
struct MOTOR_recv{
|
||||
// 定义 接收数据
|
||||
ServoComdDataV3 motor_recv_data; //电机接收数据结构体
|
||||
MotorType motorType = MotorType::A1Go1;
|
||||
int hex_len = 78; //接收的16进制命令数组长度, 78
|
||||
// long long resv_time; //接收该命令的时间, 微秒(us)
|
||||
bool correct = false; //接收数据是否完整（true完整，false不完整）
|
||||
//解读得出的电机数据
|
||||
unsigned char motor_id; //电机ID
|
||||
unsigned char mode; //0:空闲, 5:开环转动, 10:闭环FOC控制
|
||||
int Temp; //温度
|
||||
unsigned char MError; //错误码
|
||||
|
||||
float T; // 当前实际电机输出力矩
|
||||
float W; // 当前实际电机速度（高速）
|
||||
float LW; // 当前实际电机速度（低速）
|
||||
int Acc; // 电机转子加速度
|
||||
float Pos; // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准）
|
||||
|
||||
float gyro[3]; // 电机驱动板6轴传感器数据
|
||||
float acc[3];
|
||||
|
||||
int8_t Res;
|
||||
};
|
||||
|
||||
|
||||
extern void modify_data(MOTOR_send* motor_s);
|
||||
extern bool extract_data(MOTOR_recv* motor_r);
|
||||
|
||||
#endif
|
||||
|
|
@ -7,7 +7,6 @@
|
|||
#include <string.h>
|
||||
#include <vector>
|
||||
#include "common/utilities/timer.h"
|
||||
#include "message/arm_motor_common.h"
|
||||
|
||||
enum class BlockYN{
|
||||
YES,
|
||||
|
|
@ -25,7 +24,6 @@ public:
|
|||
virtual size_t send(uint8_t *sendMsg, size_t sendLength) = 0;
|
||||
virtual size_t recv(uint8_t *recvMsg, size_t recvLength) = 0;
|
||||
virtual size_t recv(uint8_t *recvMsg) = 0;
|
||||
virtual bool sendRecv(std::vector<MOTOR_send> &sendVec, std::vector<MOTOR_recv> &recvVec) = 0;
|
||||
void resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs, bool showInfo = true);
|
||||
bool isDisConnect = false;
|
||||
protected:
|
||||
|
|
@ -50,7 +48,6 @@ public:
|
|||
size_t send(uint8_t *sendMsg, size_t sendMsgLength);
|
||||
size_t recv(uint8_t *recvMsg, size_t recvLength);
|
||||
size_t recv(uint8_t *recvMsg);
|
||||
bool sendRecv(std::vector<MOTOR_send> &sendVec, std::vector<MOTOR_recv> &recvVec);
|
||||
private:
|
||||
size_t _recvBlock(uint8_t *recvMsg, size_t recvLength);
|
||||
size_t _recvUnBlock(uint8_t *recvMsg, size_t recvLength);
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@
|
|||
|
||||
#include <vector>
|
||||
#include "common/math/robotics.h"
|
||||
#include "common/math/quadProgpp/QuadProg++.hh"
|
||||
#include "thirdparty/quadProgpp/QuadProg++.hh"
|
||||
|
||||
using namespace robo;
|
||||
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ s.t.
|
|||
#ifndef _QUADPROGPP
|
||||
#define _QUADPROGPP
|
||||
|
||||
#include "common/math/quadProgpp/Array.hh"
|
||||
#include "thirdparty/quadProgpp/Array.hh"
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
namespace quadprogpp {
|
||||
Binary file not shown.
Binary file not shown.
Loading…
Reference in New Issue