快速部署应用
大约 9 分钟KmdFoc使用指南
快速部署应用
请确保你已经完成了之前的组装，并检测了电源是否正常之后开始使用。在此提供KmdFoc通信库，你只需要在此完成数据通信接口的函数即可快速使用。
底层通信依赖
#include "hal_kmd_interface.h"
#include "hal_foc_struct.h"
#include "hal_kmd_foc.h"
#include "bsp_can.h"
tHalFocInfo mHalFocInfo1
=
{
.node_id = 0X01,
};
tHalFocInfo mHalFocInfo2 =
{
.node_id = 0X02,
};
//消息发送接口函数 ->>需要自行实现 CAN2_transmit 发送函数
bool kmd_interface_transmit(hal_frame_struct *pFrame)
{
CAN2_transmit(pFrame->can_id,pFrame->data,pFrame->data_len);
}
//消息接收接口函数 ->>需要自行实现 CAN中断函数，并将CAN数据通过 hal_frame_struct 结构体传入
bool kmd_interface_receive_callback(hal_frame_struct *pFrame)
{
hal_kmd_frame_receive_callback(pFrame,&mHalFocInfo1);
hal_kmd_frame_receive_callback(pFrame,&mHalFocInfo2);
}//需要自行实现的CAN发送函数
void CAN2_transmit(uint16_t can_id,uint8_t *pData,uint16_t DataLen)
{
CAN_TxHeaderTypeDef transmit_message;
uint32_t send_mail_box;
transmit_message.StdId = can_id;
transmit_message.IDE = CAN_ID_STD;
transmit_message.RTR = CAN_RTR_DATA;
transmit_message.DLC = 0x08;
while( HAL_CAN_GetTxMailboxesFreeLevel(&hcan2 )==0);
HAL_CAN_AddTxMessage(&hcan2, &transmit_message, pData, &send_mail_box);
}//需要自行实现的CAN接收函数
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
hal_frame_struct mFrame;
CAN_RxHeaderTypeDef rx_header;
HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header,&mFrame.data[0]);
mFrame.can_id = rx_header.StdId;
mFrame.data_len = 8 ;
kmd_interface_receive_callback(&mFrame);
}对于KmdFoc的通信控制，其主要依赖有
hal_kmd_interface.c
hal_kmd_foc.c
hal_foc_enum.h
hal_foc_struct.h
hal_kmd_foc.h
hal_kmd_interface.h特别的使用串口或USB通信方式进行控制
串口通信或USB通信，则需要初始化 Seasky 协议
bsp_protocol.c
bsp_protocol.h
bsp_crc8.c
bsp_crc8.h
bsp_crc16.c
bsp_crc16.h串口协议初始化
在使用串口协议前，应当分配合适的内存大小，而 Seasky 协议提供了两种方式分配内存空间
/// <summary>
/// 方式一 初始化,并初始化内存
/// </summary>
/// <param name="pProtocol"></param>
/// <param name="uLen"></param>
void init_protocol(protocol_struct* pProtocol, uint16_t uLen)
{
/*初始化crc*/
init_crc16_tab();
/*分配内存空间*/
if (init_frame_struct(&pProtocol->frame_st, uLen) != 0)
{
PROTOCOL_ERROR_PRINTF("pProtocol init_frame_struct err!\n");
}
PROTOCOL_DEBUG_PRINTF("pProtocol init_frame_struct ok!\n");
if (init_message_struct(&pProtocol->message_st, get_protocol_size(uLen)) != 0)
{
PROTOCOL_ERROR_PRINTF("pProtocol init_message_struct err!\n");
}
PROTOCOL_DEBUG_PRINTF("pProtocol init_message_struct ok!\n");
}
/// <summary>
/// 方式二 外部预先分配好内存空间
/// </summary>
/// <param name="pProtocol"></param>
/// <param name="pFrameSt"></param>
/// <param name="pMessageSt"></param>
/// <param name="uLen"></param>
/// <returns></returns>
int init_protocol_pointer(protocol_struct* pProtocol, void *pFrameSt,void *pMessageSt,uint16_t uLen)
{
/*初始化crc*/
init_crc16_tab();
/*分配内存空间*/
if (init_frame_pointer(&pProtocol->frame_st,pFrameSt, uLen) != 0)
{
PROTOCOL_ERROR_PRINTF("pProtocol init_frame_struct err!\n");
return PROTOCOL_RESULT_ERR;
}
PROTOCOL_DEBUG_PRINTF("pProtocol init_frame_struct ok!\n");
if (init_message_pointer(&pProtocol->message_st,pMessageSt,get_protocol_size(uLen)) != 0)
{
PROTOCOL_ERROR_PRINTF("pProtocol init_message_struct err!\n");
return PROTOCOL_RESULT_ERR;
}
PROTOCOL_DEBUG_PRINTF("pProtocol init_message_struct ok!\n");
return PROTOCOL_RESULT_OK;
}对于方式一，可以直接传入需要的数据长度大小，内部会自动分配内存，使用更为简单，但是数据相对来说不透明。
kmd_usb kmd_usb_t;
void bsp_kmd_init(void)
{
kmd_usb_t.pTxProtocol.frame_st.frame_user.cmd_data.pData = NULL;
kmd_usb_t.pTxProtocol.message_st.pData = NULL;
init_protocol(&kmd_usb_t.pTxProtocol, KMD_U32_LEN);
kmd_usb_t.pRxProtocol.frame_st.frame_user.cmd_data.pData = NULL;
kmd_usb_t.pRxProtocol.message_st.pData = NULL;
init_protocol(&kmd_usb_t.pRxProtocol, KMD_U32_LEN);
}对于方式二，需要自行先分配号需要的内存空间，然后将数据长度和数据地址传入，在此请合理分配内存，并计算长度关系，避免内存越界。
kmd_usb kmd_usb_t;
// 分配seasky协议数据处理所需内存
uint32_t pTxData[KMD_U32_LEN];
uint8_t pTxBuffer[KMD_U32_LEN * 4 + PROTOCOL_DATA_OFFSET+2];
uint32_t pRxData[KMD_U32_LEN];
uint8_t pRxBuffer[KMD_U32_LEN * 4 + PROTOCOL_DATA_OFFSET+2];
/**
* @description: 初始化Seasky协议所需内存，手动分配内存
* @return {*}
*/
void bsp_kmd_init(void)
{
kmd_usb_t.pTxProtocol.frame_st.frame_user.cmd_data.pData = NULL;
kmd_usb_t.pTxProtocol.message_st.pData = NULL;
init_protocol_pointer(&kmd_usb_t.pTxProtocol, pTxData, pTxBuffer, KMD_U32_LEN);
kmd_usb_t.pRxProtocol.frame_st.frame_user.cmd_data.pData = NULL;
kmd_usb_t.pRxProtocol.message_st.pData = NULL;
init_protocol_pointer(&kmd_usb_t.pRxProtocol, pRxData, pRxBuffer, KMD_U32_LEN);
}初始化 Seasky 协议后，你可以按如下方式使用它
在 Seasky 协议中，通过将协议结构体数据，转换为CAN消息数据，实现函数共用
typedef struct
{
/*字节偏移 4 -->> 字节大小 2*/
uint16_t equipment_type; //设备类型
/*字节偏移 6 -->> 字节大小 2*/
uint16_t equipment_id; //设备ID
/*字节偏移 8 -->> 字节大小 data_len+2 */
uint16_t data_id; //数据ID
struct
{
uint16_t data_len; //data_union数据长度
uint16_t max_data_len; //数据最长长度
uint32_t* pData; //数据(data_union),推荐使用 data_union * pDataUnion = &pData[];
}cmd_data;
} user_data_struct;//数据内容对于以上结构体在 KmdFoc 的案例中
equipment_type = BSP_EQ_TYPE_KMD_FOC; \ BSP_EQ_TYPE_KMD_FOC = 1,表示设备类型为KmdFoc
equipment_id = can_id; \ equipment_id代替CAN数据帧中的CAN_ID
data_id = 0 ; \ 不使用
pData[] \ 替代CAN_BUFF
因此CAN的消息的发送和接收就可以通过以下方式转换为串口或USB方式
/**
* @description: USB和串口共用接收数据处理函数
* @param {uint8_t} *pData
* @param {uint16_t} uLen
* @param {BSP_KMD_COM_TYPE} pMsgType
* @return {*}
*/
bool bsp_kmd_callback(uint8_t *pData,uint16_t uLen,BSP_KMD_COM_TYPE pMsgType)
{
uint16_t uPos;
if((uLen>0)&&(uLen<=kmd_usb_t.pRxProtocol.message_st.max_data_len))
{
memcpy(pRxBuffer,pData,uLen);
//进行数据解析
parse_protocol(&kmd_usb_t.pRxProtocol,uLen);
switch (kmd_usb_t.pRxProtocol.frame_st.frame_user.equipment_type)
{
case BSP_EQ_TYPE_NULL:
;
break;
case BSP_EQ_TYPE_KMD_FOC:
{
return bsp_kmd_foc_callback(&kmd_usb_t.pRxProtocol,pMsgType);
}
break;
default:
;
break;
}
}
return false;
}/**
* @description: USB和串口共用接收数据处理函数
* @param {BSP_KMD_COM_TYPE} pMsgType
* @return {*}
*/
bool bsp_kmd_foc_callback(protocol_struct *pProtocol,BSP_KMD_COM_TYPE pMsgType)
{
bsp_frame frame;
frame.can_dlc = 0;
frame.msg_type = pMsgType;
frame.can_id = pProtocol->frame_st.frame_user.equipment_id;
memcpy(frame.data,pProtocol->frame_st.frame_user.cmd_data.pData,8);
return bsp_receive_callback(&frame);
}/**
* @description: USB和串口共用发送协议数据处理函数
* @param {BSP_KMD_COM_TYPE} tKmdCom_type
* @return {*}
*/
bool bsp_kmd_transmit(hal_frame_struct *tx_frame,BSP_KMD_COM_TYPE tKmdCom_type)
{
const uint8_t dataLen = 8;
// 设置设备类型为BSP_EQ_TYPE_KMD_FOC
kmd_usb_t.pTxProtocol.frame_st.frame_user.equipment_type = BSP_EQ_TYPE_KMD_FOC;
// 设置设备ID为 can_id
kmd_usb_t.pTxProtocol.frame_st.frame_user.equipment_id = tx_frame->can_id;
// 数据ID 对于Kmdfoc 而言 不使用
kmd_usb_t.pTxProtocol.frame_st.frame_user.data_id = 0;
// 设置 数据长度，uint32_t
kmd_usb_t.pTxProtocol.frame_st.frame_user.cmd_data.data_len = dataLen/4;
// 赋值待发送数据到协议解析区
memcpy((char*)(kmd_usb_t.pTxProtocol.frame_st.frame_user.cmd_data.pData),(char*)tx_frame->data,dataLen);
// 根据SEASKY协议生成发送缓冲
make_protocol(&kmd_usb_t.pTxProtocol);
switch(tKmdCom_type)
{
case BSP_MSG_FRAME_UART:
{
// 串口方式发送消息
return bsp_uart_transmit((uint8_t*)(kmd_usb_t.pTxProtocol.message_st.pData),kmd_usb_t.pTxProtocol.message_st.data_len);
}break;
#ifdef KMD_USB_USER
case BSP_MSG_FRAME_USB:
{
// USB方式发送消息
return bsp_kmd_usb_transmit((uint8_t*)(kmd_usb_t.pTxProtocol.message_st.pData),kmd_usb_t.pTxProtocol.message_st.data_len);
}break;
#endif
default:break;
}
return false;
}初始化KmdFoc
通信接口准备好之后，你将可以开始使用 KmdFoc
KmdFoc 提供了多种通信方式，包括USB VCP、UART、CAN，在此第一种获取版本号的通信方式将作为 KmdFoc 的控制方式，此后将屏蔽其余通信方式对 KmdFoc 进行控制，以避免控制冲突，重新设置通信方式可以在发送复位信号或重新上电后通过获取版本号的方式设定通信方式。
在获取版本号之后，还建议检验一次设置是否正常
获取KmdFoc版本号，以固定通信方式
获取KmdFoc的一些具体参数，并检查是否正常
按自己需求设置参数
以下是一个完整的平衡车案例
#include "balance_car_task.h"
#include "bsp_usb.h"
#include "cmsis_os.h"
#include "FreeRtos.h"
#include "hal_kmd_interface.h"
#include "hal_foc_enum.h"
#include "vSkyAhrs.h"
#include "arm_math.h"
#define AngleToRadian (0.01745329251994329576923690768489f)
#define MaxLimit(input, max) \
{ \
if (input > max) \
{ \
input = max; \
} \
else if (input < -max) \
{ \
input = -max; \
} \
}
//内部闭环已经有了，此处闭环周期没必要太高
#define BALANCE_CAR_TASK_TIME 5
//extern tHalFocInfo mHalFocInfo1;
//extern tHalFocInfo mHalFocInfo2;
//extern hal_kmd_user_struct tHalKmdUser;
extern vsky_ahrs_info mvsky_ahrs_info;
uint8_t check_cali[2] = {0,0};
uint8_t check_mode[2] = {0,0};
uint8_t check_encoder[2] = {0,0};
int hal_cpr[2] = {0,0};
float kmd_foc_vel_kp = 1.5;
float kmd_foc_vel_ki = 0.00;
//设置每5ms自动上报一次数据，如果需要上报所有数据，则第一个5ms上报HAL_KMD_FSM_HEARTBEAT0,第二个上报HAL_KMD_POS_HEARTBEAT1...
int kmd_foc_report_ms = 5;
//设置需要上报的数据，在此只需要转速
int kmd_foc_report_ch = ((0<<(HAL_KMD_FSM_HEARTBEAT0-HAL_KMD_FSM_HEARTBEAT0))|
(0<<(HAL_KMD_POS_HEARTBEAT1-HAL_KMD_FSM_HEARTBEAT0))|
(1<<(HAL_KMD_VEL_HEARTBEAT2-HAL_KMD_FSM_HEARTBEAT0))|
(0<<(HAL_KMD_CUR_HEARTBEAT3-HAL_KMD_FSM_HEARTBEAT0))|
(0<<(HAL_KMD_BUS_HEARTBEAT4-HAL_KMD_FSM_HEARTBEAT0)));
//float balance_pid_kp = 0.5;
//float balance_pid_kd = 0.035;
float balance_pid_kp = 60.0;
float balance_pid_kd = 5.6;
//float balance_pid_kp = 0;
//float balance_pid_kd = 0;
//float balance_last_angle = 0;
float balance_out = 0;
float balance_pout = 0;
float balance_dout = 0;
float balance_sin = 0;
float velicoty_pid_kp = 10;
float velicoty_pid_ki = 10;
float velicoty_last_speed[2] = {0,0};
float velicoty_out[2] = {0,0};
float turn_pid_kp = 10;
float turn_last_yaw;
osThreadId BalanceCarTaskHandle;
static void balance_task(void const *pvParameters);
void balance_task_creat(void)
{
osThreadDef(BalanceCarTask,balance_task,osPriorityRealtime,0,128);
BalanceCarTaskHandle = osThreadCreate(osThread(BalanceCarTask), NULL);
}
static void balance_task(void const *pvParameters)
{
TickType_t peroid = osKernelSysTick();
osDelay(3000);
/*KmdFoc将上电后第一个获取版本号的通信方式设定本次上电的控制方式，其它接口将无法用于控制*/
tHalKmdUser.hal_kmd_get_version_hook(&mHalFocInfo1.__tx_frame,mHalFocInfo1.node_id);
osDelay(1);
tHalKmdUser.hal_kmd_get_version_hook(&mHalFocInfo2.__tx_frame,mHalFocInfo2.node_id);
osDelay(1);
/*发送获取配置信息请求,配置信息在CAN中断接收后会更新在 mHalFocInfo1.__kmd_user.__config 数组中,而具体的参数类型是int或float可以查阅 kmd_config_map[][2]数组*/
for(uint8_t i=0; i<KMD_CONFIG_MAP_MAX_LENGTH; i++)
{
uint8_t mData[4];
if(kmd_config_map[i][1] == HAL_USER_CONFIG_VEL_GAIN)
{
float_to_data(kmd_foc_vel_kp,mData);
tHalKmdUser.kmd_config.hal_kmd_user_config_set_hook(&mHalFocInfo1.__tx_frame,mHalFocInfo1.node_id,kmd_config_map[i][1],&mData[0]);
osDelay(1);
float_to_data(kmd_foc_vel_kp,mData);
tHalKmdUser.kmd_config.hal_kmd_user_config_set_hook(&mHalFocInfo2.__tx_frame,mHalFocInfo2.node_id,kmd_config_map[i][1],&mData[0]);
osDelay(1);
}
if(kmd_config_map[i][1] == HAL_USER_CONFIG_VEL_INTEGRATOR_GAIN)
{
float_to_data(kmd_foc_vel_ki,mData);
tHalKmdUser.kmd_config.hal_kmd_user_config_set_hook(&mHalFocInfo1.__tx_frame,mHalFocInfo1.node_id,kmd_config_map[i][1],&mData[0]);
osDelay(1);
float_to_data(kmd_foc_vel_ki,mData);
tHalKmdUser.kmd_config.hal_kmd_user_config_set_hook(&mHalFocInfo2.__tx_frame,mHalFocInfo2.node_id,kmd_config_map[i][1],&mData[0]);
osDelay(1);
}
if(kmd_config_map[i][1] == HAL_USER_CONFIG_CAN_HEARTBEAT_MS)
{
int_to_data(kmd_foc_report_ms,mData);
tHalKmdUser.kmd_config.hal_kmd_user_config_set_hook(&mHalFocInfo1.__tx_frame,mHalFocInfo1.node_id,kmd_config_map[i][1],&mData[0]);
osDelay(1);
tHalKmdUser.kmd_config.hal_kmd_user_config_set_hook(&mHalFocInfo2.__tx_frame,mHalFocInfo2.node_id,kmd_config_map[i][1],&mData[0]);
osDelay(1);
}
if(kmd_config_map[i][1] == HAL_USER_CONFIG_CAN_HEARTBEAT_CH)
{
int_to_data(kmd_foc_report_ms,mData);
tHalKmdUser.kmd_config.hal_kmd_user_config_set_hook(&mHalFocInfo1.__tx_frame,mHalFocInfo1.node_id,kmd_config_map[i][1],&mData[0]);
osDelay(1);
tHalKmdUser.kmd_config.hal_kmd_user_config_set_hook(&mHalFocInfo2.__tx_frame,mHalFocInfo2.node_id,kmd_config_map[i][1],&mData[0]);
osDelay(1);
}
}
/*发送获取配置信息请求,配置信息在CAN中断接收后会更新在 mHalFocInfo1.__kmd_user.__config 数组中,而具体的参数类型是int或float可以查阅 kmd_config_map[][2]数组*/
for(uint8_t i=0; i<KMD_CONFIG_MAP_MAX_LENGTH; i++)
{
tHalKmdUser.kmd_config.hal_kmd_user_config_get_hook(&mHalFocInfo1.__tx_frame,mHalFocInfo1.node_id,kmd_config_map[i][1]);
osDelay(1);
tHalKmdUser.kmd_config.hal_kmd_user_config_get_hook(&mHalFocInfo2.__tx_frame,mHalFocInfo2.node_id,kmd_config_map[i][1]);
osDelay(1);
}
// /*如果是单独获取某个参数，则可以*/
// {
// /*是否已经校准*/
// tHalKmdUser.kmd_config.hal_kmd_user_config_get_hook(&mHalFocInfo1.__tx_frame,mHalFocInfo1.node_id,HAL_USER_CONFIG_CALIB_VALID);
// osDelay(1);
// tHalKmdUser.kmd_config.hal_kmd_user_config_get_hook(&mHalFocInfo2.__tx_frame,mHalFocInfo2.node_id,HAL_USER_CONFIG_CALIB_VALID);
// osDelay(1);
// /*控制模式*/
// tHalKmdUser.kmd_config.hal_kmd_user_config_get_hook(&mHalFocInfo1.__tx_frame,mHalFocInfo1.node_id,HAL_USER_CONFIG_CONTROL_MODE);
// osDelay(1);
// tHalKmdUser.kmd_config.hal_kmd_user_config_get_hook(&mHalFocInfo2.__tx_frame,mHalFocInfo2.node_id,HAL_USER_CONFIG_CONTROL_MODE);
// osDelay(1);
// }
/*等待数据返回*/
osDelay(10);
/*查询数据*/
for(uint8_t i=0; i<KMD_CONFIG_MAP_MAX_LENGTH; i++)
{
if(kmd_config_map[i][1] == HAL_USER_CONFIG_CONTROL_MODE)
{
//是否工作于转速模式
if(mHalFocInfo1.__kmd_user.__config[i].value_int == HAL_CONTROL_MODE_CURRENT)
{
check_mode[0] = 1;
}
if(mHalFocInfo2.__kmd_user.__config[i].value_int == HAL_CONTROL_MODE_CURRENT)
{
check_mode[1] = 1;
}
}
if(kmd_config_map[i][1] == HAL_USER_CONFIG_CALIB_VALID)
{
//是否已经校准
if(mHalFocInfo1.__kmd_user.__config[i].value_int == 1)
{
check_cali[0] = 1;
}
if(mHalFocInfo2.__kmd_user.__config[i].value_int == 1)
{
check_cali[1] = 1;
}
}
if(kmd_config_map[i][1] == HAL_USER_CONFIG_ENCODER_TYPE)
{
//检查编码器是否为HALL
if(mHalFocInfo1.__kmd_user.__config[i].value_int == HAL_ENCODER_HALL)
{
check_encoder[0] = 1;
}
if(mHalFocInfo2.__kmd_user.__config[i].value_int == HAL_ENCODER_HALL)
{
check_encoder[1] = 1;
}
}
if(kmd_config_map[i][1] == HAL_USER_CONFIG_ENCODER_CPR)
{
/*检查轮毂电机编码器CPR = 6*极对数*/
/*轮毂电机的编码器必须设置为 6*极对数并进行校准，绝对值编码器则会自动设置*/
hal_cpr[0] = mHalFocInfo1.__kmd_user.__config[i].value_int;
hal_cpr[1] = mHalFocInfo2.__kmd_user.__config[i].value_int;
}
/*必要的，你还可以检查闭环等诸多参数，以检测KmdFoc的值是否被有效的保存，或者你在使用上位机后未保存到Flash*/
/************************************************************************************************/
}
if((check_cali[0]==0)|(check_cali[1]==0))
{
//debug_printf()
while(1);
}
if((check_mode[0]==0)|(check_mode[1]==0))
{
//debug_printf()
while(1);
}
if((check_encoder[0]==0)|(check_encoder[1]==0))
{
//debug_printf()
while(1);
}
if((check_cali[0]==0)|(check_cali[1]==0))
{
//debug_printf()
while(1);
}
if((hal_cpr[0]==90)&(hal_cpr[1]==90))
{
tHalKmdUser.kmd_fsm.hal_kmd_fsm_motor_enable_hook(&mHalFocInfo1.__tx_frame,mHalFocInfo1.node_id);
osDelay(1);
tHalKmdUser.kmd_fsm.hal_kmd_fsm_motor_enable_hook(&mHalFocInfo2.__tx_frame,mHalFocInfo2.node_id);
osDelay(1);
}
else
{
//debug_printf()
while(1);
}
//设定一个较小的转速进行测试
{
//左右电机的转向应该反向
tHalKmdUser.hal_kmd_set_current_hook(&mHalFocInfo1.__tx_frame,mHalFocInfo1.node_id,0);
tHalKmdUser.hal_kmd_set_current_hook(&mHalFocInfo2.__tx_frame,mHalFocInfo2.node_id,0);
}
{
}
balance_out = 0;
balance_pout = 0;
balance_dout = 0;
while(1)
{
//等待陀螺仪校准Ok，再开始控制
if(SKY_IMU_CALI_OK == mvsky_ahrs_info.imu_cali)
{
/*计算平衡环-双电机共用*/
{
if(fabs(mvsky_ahrs_info.vAngle_data[1]-0.4)<45)
{
balance_sin = sinf((mvsky_ahrs_info.vAngle_data[1]-0.40)*AngleToRadian);
//差分->>一般都是用的 kp*Angle,相比之下使用 kp*sin(Angle)的鲁棒性会更高
balance_pout = balance_pid_kp*balance_sin;
//角度微分->>转速，因此直接用Gyro替代微分项，以获得更准确的角速度
balance_dout = balance_pid_kd*(mvsky_ahrs_info.vGyro_data[1]);
MaxLimit(balance_pout,40);//35A
MaxLimit(balance_dout,40);
balance_out = balance_pout+balance_dout;
MaxLimit(balance_out,40);
}
else
{
balance_sin = 0;
balance_dout = 0;
balance_out = 0;
}
}
/*计算速度环*/
{
}
/*计算转向环*/
{
}
/*给定速度*/
{
//左右电机的转向应该反向
tHalKmdUser.hal_kmd_set_current_hook(&mHalFocInfo1.__tx_frame,mHalFocInfo1.node_id,-balance_out);
tHalKmdUser.hal_kmd_set_current_hook(&mHalFocInfo2.__tx_frame,mHalFocInfo2.node_id,balance_out);
}
}
else
{
balance_out = 0;
/*给定速度*/
{
//左右电机的转向应该反向
tHalKmdUser.hal_kmd_set_current_hook(&mHalFocInfo1.__tx_frame,mHalFocInfo1.node_id,-balance_out);
tHalKmdUser.hal_kmd_set_current_hook(&mHalFocInfo2.__tx_frame,mHalFocInfo2.node_id,balance_out);
}
}
osDelayUntil(&peroid,BALANCE_CAR_TASK_TIME);
}
}