Compare commits
5 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 09d8446f34 | |||
| 6909b34ccd | |||
| 25bdadf878 | |||
| 24eb09e2f7 | |||
| 1f404b793f |
6
MDK-ARM/.vscode/keil-assistant.log
vendored
6
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -62,3 +62,9 @@
|
|||||||
|
|
||||||
[info] Log at : 2025/3/19|14:10:22|GMT+0800
|
[info] Log at : 2025/3/19|14:10:22|GMT+0800
|
||||||
|
|
||||||
|
[info] Log at : 2025/3/26|14:58:18|GMT+0800
|
||||||
|
|
||||||
|
[info] Log at : 2025/3/26|15:24:15|GMT+0800
|
||||||
|
|
||||||
|
[info] Log at : 2025/3/26|17:13:53|GMT+0800
|
||||||
|
|
||||||
|
|||||||
4
MDK-ARM/.vscode/uv4.log
vendored
4
MDK-ARM/.vscode/uv4.log
vendored
@ -1,8 +1,8 @@
|
|||||||
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
|
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
|
||||||
Build target 'mycode1'
|
Build target 'mycode1'
|
||||||
compiling shoot_task.c...
|
compiling shoot.c...
|
||||||
linking...
|
linking...
|
||||||
Program Size: Code=26332 RO-data=1904 RW-data=280 ZI-data=22472
|
Program Size: Code=25844 RO-data=1904 RW-data=248 ZI-data=22512
|
||||||
FromELF: creating hex file...
|
FromELF: creating hex file...
|
||||||
"mycode1\mycode1.axf" - 0 Error(s), 0 Warning(s).
|
"mycode1\mycode1.axf" - 0 Error(s), 0 Warning(s).
|
||||||
Build Time Elapsed: 00:00:03
|
Build Time Elapsed: 00:00:03
|
||||||
|
|||||||
2
MDK-ARM/.vscode/uv4.log.lock
vendored
2
MDK-ARM/.vscode/uv4.log.lock
vendored
@ -1 +1 @@
|
|||||||
2025/3/19 14:04:20
|
2025/3/26 20:14:55
|
||||||
@ -230,6 +230,11 @@
|
|||||||
<WinNumber>1</WinNumber>
|
<WinNumber>1</WinNumber>
|
||||||
<ItemText>trigger_angle_o</ItemText>
|
<ItemText>trigger_angle_o</ItemText>
|
||||||
</Ww>
|
</Ww>
|
||||||
|
<Ww>
|
||||||
|
<count>16</count>
|
||||||
|
<WinNumber>1</WinNumber>
|
||||||
|
<ItemText>speed_6384,0x0A</ItemText>
|
||||||
|
</Ww>
|
||||||
</WatchWindow1>
|
</WatchWindow1>
|
||||||
<WatchWindow2>
|
<WatchWindow2>
|
||||||
<Ww>
|
<Ww>
|
||||||
@ -868,7 +873,7 @@
|
|||||||
|
|
||||||
<Group>
|
<Group>
|
||||||
<GroupName>User/device</GroupName>
|
<GroupName>User/device</GroupName>
|
||||||
<tvExp>1</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<cbSel>0</cbSel>
|
<cbSel>0</cbSel>
|
||||||
<RteFlg>0</RteFlg>
|
<RteFlg>0</RteFlg>
|
||||||
@ -1004,7 +1009,7 @@
|
|||||||
|
|
||||||
<Group>
|
<Group>
|
||||||
<GroupName>User/module</GroupName>
|
<GroupName>User/module</GroupName>
|
||||||
<tvExp>1</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<cbSel>0</cbSel>
|
<cbSel>0</cbSel>
|
||||||
<RteFlg>0</RteFlg>
|
<RteFlg>0</RteFlg>
|
||||||
|
|||||||
@ -2,7 +2,7 @@
|
|||||||
|
|
||||||
## r1上层
|
## r1上层
|
||||||
|
|
||||||
1. **odrive**推射 **can1** **id 010** **spi2**读取as5047p
|
1. **vesc** **can1 id:77** **spi2**读取as5047p
|
||||||
2. go1 RS485模块 01翻转 02云台
|
2. go1 RS485模块 01翻转 02云台
|
||||||
3. 爪子 左三PC6
|
3. 爪子 左三PC6
|
||||||
4. 接球 左一左2 PI7 PI6
|
4. 接球 左一左2 PI7 PI6
|
||||||
@ -18,18 +18,18 @@ void can_filter_init(void)
|
|||||||
can_filter_st.FilterMaskIdHigh = 0x0000;
|
can_filter_st.FilterMaskIdHigh = 0x0000;
|
||||||
can_filter_st.FilterMaskIdLow = 0x0000;
|
can_filter_st.FilterMaskIdLow = 0x0000;
|
||||||
can_filter_st.FilterBank = 0;
|
can_filter_st.FilterBank = 0;
|
||||||
|
can_filter_st.SlaveStartFilterBank = 14;
|
||||||
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
|
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
|
||||||
HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
|
HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
|
||||||
HAL_CAN_Start(&hcan1);
|
HAL_CAN_Start(&hcan1);
|
||||||
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
|
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
|
||||||
|
|
||||||
|
|
||||||
can_filter_st.SlaveStartFilterBank = 14;
|
|
||||||
can_filter_st.FilterBank = 14;
|
can_filter_st.FilterBank = 14;
|
||||||
|
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO1;
|
||||||
HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);
|
HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);
|
||||||
HAL_CAN_Start(&hcan2);
|
HAL_CAN_Start(&hcan2);
|
||||||
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
|
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
314
User/device/Xiaomi.c
Normal file
314
User/device/Xiaomi.c
Normal file
@ -0,0 +1,314 @@
|
|||||||
|
#include "Xiaomi.h"
|
||||||
|
#include "can_it.h"
|
||||||
|
#include "can_init.h"
|
||||||
|
#include "TopDefine.h"
|
||||||
|
#include<cmsis_os2.h>
|
||||||
|
#include "FreeRTOS.h"
|
||||||
|
|
||||||
|
//小米电机
|
||||||
|
CAN_RxHeaderTypeDef rxMsg;//发送接收结构体
|
||||||
|
CAN_TxHeaderTypeDef txMsg;//发送配置结构体
|
||||||
|
|
||||||
|
uint32_t Motor_Can_ID; //接收数据电机ID
|
||||||
|
uint8_t byte[4]; //转换临时数据
|
||||||
|
uint32_t send_mail_box = {0};//NONE
|
||||||
|
|
||||||
|
#define can_txd() HAL_CAN_AddTxMessage(&hcan2, &txMsg, tx_data, &send_mail_box)//CAN发送宏定义
|
||||||
|
|
||||||
|
MI_Motor mi_motor[4];//预先定义四个小米电机
|
||||||
|
|
||||||
|
CAN_RxHeaderTypeDef xm_rx_header;
|
||||||
|
uint8_t xm_rx_data[8];
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 浮点数转4字节函数
|
||||||
|
* @param[in] f:浮点数
|
||||||
|
* @retval 4字节数组
|
||||||
|
* @description : IEEE 754 协议
|
||||||
|
*/
|
||||||
|
static uint8_t* Float_to_Byte(float f)
|
||||||
|
{
|
||||||
|
unsigned long longdata = 0;
|
||||||
|
longdata = *(unsigned long*)&f;
|
||||||
|
byte[0] = (longdata & 0xFF000000) >> 24;
|
||||||
|
byte[1] = (longdata & 0x00FF0000) >> 16;
|
||||||
|
byte[2] = (longdata & 0x0000FF00) >> 8;
|
||||||
|
byte[3] = (longdata & 0x000000FF);
|
||||||
|
return byte;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 小米电机回文16位数据转浮点
|
||||||
|
* @param[in] x:16位回文
|
||||||
|
* @param[in] x_min:对应参数下限
|
||||||
|
* @param[in] x_max:对应参数上限
|
||||||
|
* @param[in] bits:参数位数
|
||||||
|
* @retval 返回浮点值
|
||||||
|
*/
|
||||||
|
static float uint16_to_float(uint16_t x,float x_min,float x_max,int bits)
|
||||||
|
{
|
||||||
|
uint32_t span = (1 << bits) - 1;
|
||||||
|
float offset = x_max - x_min;
|
||||||
|
return offset * x / span + x_min;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 小米电机发送浮点转16位数据
|
||||||
|
* @param[in] x:浮点
|
||||||
|
* @param[in] x_min:对应参数下限
|
||||||
|
* @param[in] x_max:对应参数上限
|
||||||
|
* @param[in] bits:参数位数
|
||||||
|
* @retval 返回浮点值
|
||||||
|
*/
|
||||||
|
static int float_to_uint(float x, float x_min, float x_max, int bits)
|
||||||
|
{
|
||||||
|
float span = x_max - x_min;
|
||||||
|
float offset = x_min;
|
||||||
|
if(x > x_max) x=x_max;
|
||||||
|
else if(x < x_min) x= x_min;
|
||||||
|
return (int) ((x-offset)*((float)((1<<bits)-1))/span);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 写入电机参数
|
||||||
|
* @param[in] Motor:对应控制电机结构体
|
||||||
|
* @param[in] Index:写入参数对应地址
|
||||||
|
* @param[in] Value:写入参数值
|
||||||
|
* @param[in] Value_type:写入参数数据类型
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
static void Set_Motor_Parameter(MI_Motor *Motor,uint16_t Index,float Value,char Value_type){
|
||||||
|
uint8_t tx_data[8];
|
||||||
|
txMsg.ExtId = Communication_Type_SetSingleParameter<<24|Master_CAN_ID<<8|Motor->CAN_ID;
|
||||||
|
tx_data[0]=Index;
|
||||||
|
tx_data[1]=Index>>8;
|
||||||
|
tx_data[2]=0x00;
|
||||||
|
tx_data[3]=0x00;
|
||||||
|
if(Value_type == 'f'){
|
||||||
|
Float_to_Byte(Value);
|
||||||
|
tx_data[4]=byte[3];
|
||||||
|
tx_data[5]=byte[2];
|
||||||
|
tx_data[6]=byte[1];
|
||||||
|
tx_data[7]=byte[0];
|
||||||
|
}
|
||||||
|
else if(Value_type == 's'){
|
||||||
|
tx_data[4]=(uint8_t)Value;
|
||||||
|
tx_data[5]=0x00;
|
||||||
|
tx_data[6]=0x00;
|
||||||
|
tx_data[7]=0x00;
|
||||||
|
}
|
||||||
|
can_txd();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 提取电机回复帧扩展ID中的电机CANID
|
||||||
|
* @param[in] CAN_ID_Frame:电机回复帧中的扩展CANID
|
||||||
|
* @retval 电机CANID
|
||||||
|
*/
|
||||||
|
static uint32_t Get_Motor_ID(uint32_t CAN_ID_Frame)
|
||||||
|
{
|
||||||
|
return (CAN_ID_Frame&0xFFFF)>>8;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 电机回复帧数据处理函数
|
||||||
|
* @param[in] Motor:对应控制电机结构体
|
||||||
|
* @param[in] DataFrame:数据帧
|
||||||
|
* @param[in] IDFrame:扩展ID帧
|
||||||
|
* @retval None
|
||||||
|
*/
|
||||||
|
static void Motor_Data_Handler(MI_Motor *Motor,uint8_t DataFrame[8],uint32_t IDFrame)
|
||||||
|
{
|
||||||
|
Motor->Angle=uint16_to_float(DataFrame[0]<<8|DataFrame[1],MIN_P,MAX_P,16);
|
||||||
|
Motor->Speed=uint16_to_float(DataFrame[2]<<8|DataFrame[3],V_MIN,V_MAX,16);
|
||||||
|
Motor->Torque=uint16_to_float(DataFrame[4]<<8|DataFrame[5],T_MIN,T_MAX,16);
|
||||||
|
Motor->Temp=(DataFrame[6]<<8|DataFrame[7])*Temp_Gain;
|
||||||
|
Motor->error_code=(IDFrame&0x1F0000)>>16;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 小米电机ID检查
|
||||||
|
* @param[in] id: 控制电机CAN_ID【出厂默认0x7F】
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void chack_cybergear(uint8_t ID)
|
||||||
|
{
|
||||||
|
uint8_t tx_data[8] = {0};
|
||||||
|
txMsg.ExtId = Communication_Type_GetID<<24|Master_CAN_ID<<8|ID;
|
||||||
|
can_txd();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 使能小米电机
|
||||||
|
* @param[in] Motor:对应控制电机结构体
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void start_cybergear(MI_Motor *Motor)
|
||||||
|
{
|
||||||
|
uint8_t tx_data[8] = {0};
|
||||||
|
txMsg.ExtId = Communication_Type_MotorEnable<<24|Master_CAN_ID<<8|Motor->CAN_ID;
|
||||||
|
can_txd();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 停止电机
|
||||||
|
* @param[in] Motor:对应控制电机结构体
|
||||||
|
* @param[in] clear_error:清除错误位（0 不清除 1清除）
|
||||||
|
* @retval None
|
||||||
|
*/
|
||||||
|
void stop_cybergear(MI_Motor *Motor,uint8_t clear_error)
|
||||||
|
{
|
||||||
|
uint8_t tx_data[8]={0};
|
||||||
|
tx_data[0]=clear_error;//清除错误位设置
|
||||||
|
txMsg.ExtId = Communication_Type_MotorStop<<24|Master_CAN_ID<<8|Motor->CAN_ID;
|
||||||
|
can_txd();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 设置电机模式(必须停止时调整！)
|
||||||
|
* @param[in] Motor: 电机结构体
|
||||||
|
* @param[in] Mode: 电机工作模式（1.运动模式Motion_mode 2. 位置模式Position_mode 3. 速度模式Speed_mode 4. 电流模式Current_mode）
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void set_mode_cybergear(MI_Motor *Motor,uint8_t Mode)
|
||||||
|
{
|
||||||
|
Set_Motor_Parameter(Motor,Run_mode,Mode,'s');
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 电流控制模式下设置电流
|
||||||
|
* @param[in] Motor: 电机结构体
|
||||||
|
* @param[in] Current:电流设置
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void set_current_cybergear(MI_Motor *Motor,float Current)
|
||||||
|
{
|
||||||
|
Set_Motor_Parameter(Motor,Iq_Ref,Current,'f');
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 设置电机零点
|
||||||
|
* @param[in] Motor: 电机结构体
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void set_zeropos_cybergear(MI_Motor *Motor)
|
||||||
|
{
|
||||||
|
uint8_t tx_data[8]={0};
|
||||||
|
tx_data[0] = 1;
|
||||||
|
txMsg.ExtId = Communication_Type_SetPosZero<<24|Master_CAN_ID<<8|Motor->CAN_ID;
|
||||||
|
can_txd();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 设置电机CANID
|
||||||
|
* @param[in] Motor: 电机结构体
|
||||||
|
* @param[in] Motor: 设置新ID
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void set_CANID_cybergear(MI_Motor *Motor,uint8_t CAN_ID)
|
||||||
|
{
|
||||||
|
uint8_t tx_data[8]={0};
|
||||||
|
txMsg.ExtId = Communication_Type_CanID<<24|CAN_ID<<16|Master_CAN_ID<<8|Motor->CAN_ID;
|
||||||
|
Motor->CAN_ID = CAN_ID;//将新的ID导入电机结构体
|
||||||
|
can_txd();
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief 小米电机初始化
|
||||||
|
* @param[in] Motor: 电机结构体
|
||||||
|
* @param[in] Can_Id: 小米电机ID(默认0x7F)
|
||||||
|
* @param[in] Motor_Num: 电机编号
|
||||||
|
* @param[in] mode: 电机工作模式（0.运动模式Motion_mode 1. 位置模式Position_mode 2. 速度模式Speed_mode 3. 电流模式Current_mode）
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void init_cybergear(MI_Motor *Motor,uint8_t Can_Id, uint8_t mode)
|
||||||
|
{
|
||||||
|
txMsg.StdId = 0; //配置CAN发送：标准帧清零
|
||||||
|
txMsg.ExtId = 0; //配置CAN发送：扩展帧清零
|
||||||
|
txMsg.IDE = CAN_ID_EXT; //配置CAN发送：扩展帧
|
||||||
|
txMsg.RTR = CAN_RTR_DATA; //配置CAN发送：数据帧
|
||||||
|
txMsg.DLC = 0x08; //配置CAN发送：数据长度
|
||||||
|
|
||||||
|
Motor->CAN_ID=Can_Id; //ID设置
|
||||||
|
set_mode_cybergear(Motor,mode);//设置电机模式
|
||||||
|
start_cybergear(Motor); //使能电机
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 小米运控模式指令
|
||||||
|
* @param[in] Motor: 目标电机结构体
|
||||||
|
* @param[in] torque: 力矩设置[-12,12] N*M
|
||||||
|
* @param[in] MechPosition: 位置设置[-12.5,12.5] rad
|
||||||
|
* @param[in] speed: 速度设置[-30,30] rpm
|
||||||
|
* @param[in] kp: 比例参数设置
|
||||||
|
* @param[in] kd: 微分参数设置
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void motor_controlmode(MI_Motor *Motor,float torque, float MechPosition, float speed, float kp, float kd)
|
||||||
|
{
|
||||||
|
uint8_t tx_data[8];//发送数据初始化
|
||||||
|
//装填发送数据
|
||||||
|
tx_data[0]=float_to_uint(MechPosition,P_MIN,P_MAX,16)>>8;
|
||||||
|
tx_data[1]=float_to_uint(MechPosition,P_MIN,P_MAX,16);
|
||||||
|
tx_data[2]=float_to_uint(speed,V_MIN,V_MAX,16)>>8;
|
||||||
|
tx_data[3]=float_to_uint(speed,V_MIN,V_MAX,16);
|
||||||
|
tx_data[4]=float_to_uint(kp,KP_MIN,KP_MAX,16)>>8;
|
||||||
|
tx_data[5]=float_to_uint(kp,KP_MIN,KP_MAX,16);
|
||||||
|
tx_data[6]=float_to_uint(kd,KD_MIN,KD_MAX,16)>>8;
|
||||||
|
tx_data[7]=float_to_uint(kd,KD_MIN,KD_MAX,16);
|
||||||
|
|
||||||
|
txMsg.ExtId = Communication_Type_MotionControl<<24|float_to_uint(torque,T_MIN,T_MAX,16)<<8|Motor->CAN_ID;//装填扩展帧数据
|
||||||
|
can_txd();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 小米电机数据处理函数
|
||||||
|
* @param[in] none
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void XM_MotorEncode()
|
||||||
|
{
|
||||||
|
|
||||||
|
if(xm_rx_header.IDE == CAN_ID_EXT)
|
||||||
|
{
|
||||||
|
Motor_Can_ID=Get_Motor_ID(xm_rx_header.ExtId);//首先获取回传电机ID信息
|
||||||
|
|
||||||
|
switch(Motor_Can_ID)
|
||||||
|
{
|
||||||
|
|
||||||
|
case 0x01:
|
||||||
|
if(xm_rx_header.ExtId>>24 != 0) //检查是否为广播模式
|
||||||
|
Motor_Data_Handler(&mi_motor[0],xm_rx_data,xm_rx_header.ExtId);
|
||||||
|
else
|
||||||
|
mi_motor[0].MCU_ID = xm_rx_data[0];
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static osEventFlagsId_t eventReceive;//事件标志
|
||||||
|
/**
|
||||||
|
* @brief 自定义小米电机回调函数
|
||||||
|
* @param[in] none
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void XM_Motor_CB()
|
||||||
|
{
|
||||||
|
HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &xm_rx_header, xm_rx_data);
|
||||||
|
|
||||||
|
osEventFlagsSet(eventReceive, EVENT_CAN);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 小米电机初始化
|
||||||
|
* @param none
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void xia0miInit(void)
|
||||||
|
{
|
||||||
|
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
|
||||||
|
XM_Motor_CB);
|
||||||
|
// can_filter_init();//dji那里已经初始化了
|
||||||
|
}
|
||||||
|
|
||||||
112
User/device/Xiaomi.h
Normal file
112
User/device/Xiaomi.h
Normal file
@ -0,0 +1,112 @@
|
|||||||
|
#ifndef _XIAOMI_H
|
||||||
|
#define _XIAOMI_H
|
||||||
|
|
||||||
|
#include "struct_typedef.h"
|
||||||
|
#include "can.h"
|
||||||
|
|
||||||
|
/*小米电机部分参数和函数*/
|
||||||
|
//控制参数最值，谨慎更改
|
||||||
|
#define P_MIN -12.5f
|
||||||
|
#define P_MAX 12.5f
|
||||||
|
#define V_MIN -30.0f
|
||||||
|
#define V_MAX 30.0f
|
||||||
|
#define KP_MIN 0.0f
|
||||||
|
#define KP_MAX 500.0f
|
||||||
|
#define KD_MIN 0.0f
|
||||||
|
#define KD_MAX 5.0f
|
||||||
|
#define T_MIN -12.0f
|
||||||
|
#define T_MAX 12.0f
|
||||||
|
#define MAX_P 720
|
||||||
|
#define MIN_P -720
|
||||||
|
//主机CANID设置
|
||||||
|
#define Master_CAN_ID 0x00 //主机ID
|
||||||
|
//控制命令宏定义
|
||||||
|
#define Communication_Type_GetID 0x00 //获取设备的ID和64位MCU唯一标识符
|
||||||
|
#define Communication_Type_MotionControl 0x01 //用来向主机发送控制指令
|
||||||
|
#define Communication_Type_MotorRequest 0x02 //用来向主机反馈电机运行状态
|
||||||
|
#define Communication_Type_MotorEnable 0x03 //电机使能运行
|
||||||
|
#define Communication_Type_MotorStop 0x04 //电机停止运行
|
||||||
|
#define Communication_Type_SetPosZero 0x06 //设置电机机械零位
|
||||||
|
#define Communication_Type_CanID 0x07 //更改当前电机CAN_ID
|
||||||
|
#define Communication_Type_Control_Mode 0x12
|
||||||
|
#define Communication_Type_GetSingleParameter 0x11 //读取单个参数
|
||||||
|
#define Communication_Type_SetSingleParameter 0x12 //设定单个参数
|
||||||
|
#define Communication_Type_ErrorFeedback 0x15 //故障反馈帧
|
||||||
|
//参数读取宏定义
|
||||||
|
#define Run_mode 0x7005
|
||||||
|
#define Iq_Ref 0x7006
|
||||||
|
#define Spd_Ref 0x700A
|
||||||
|
#define Limit_Torque 0x700B
|
||||||
|
#define Cur_Kp 0x7010
|
||||||
|
#define Cur_Ki 0x7011
|
||||||
|
#define Cur_Filt_Gain 0x7014
|
||||||
|
#define Loc_Ref 0x7016
|
||||||
|
#define Limit_Spd 0x7017
|
||||||
|
#define Limit_Cur 0x7018
|
||||||
|
|
||||||
|
#define Gain_Angle 720/32767.0
|
||||||
|
#define Bias_Angle 0x8000
|
||||||
|
#define Gain_Speed 30/32767.0
|
||||||
|
#define Bias_Speed 0x8000
|
||||||
|
#define Gain_Torque 12/32767.0
|
||||||
|
#define Bias_Torque 0x8000
|
||||||
|
#define Temp_Gain 0.1
|
||||||
|
|
||||||
|
#define Motor_Error 0x00
|
||||||
|
#define Motor_OK 0X01
|
||||||
|
|
||||||
|
enum CONTROL_MODE //控制模式定义
|
||||||
|
{
|
||||||
|
Motion_mode = 0,//运控模式
|
||||||
|
Position_mode, //位置模式
|
||||||
|
Speed_mode, //速度模式
|
||||||
|
Current_mode //电流模式
|
||||||
|
};
|
||||||
|
enum ERROR_TAG //错误回传对照
|
||||||
|
{
|
||||||
|
OK = 0,//无故障
|
||||||
|
BAT_LOW_ERR = 1,//欠压故障
|
||||||
|
OVER_CURRENT_ERR = 2,//过流
|
||||||
|
OVER_TEMP_ERR = 3,//过温
|
||||||
|
MAGNETIC_ERR = 4,//磁编码故障
|
||||||
|
HALL_ERR_ERR = 5,//HALL编码故障
|
||||||
|
NO_CALIBRATION_ERR = 6//未标定
|
||||||
|
};
|
||||||
|
|
||||||
|
typedef struct{ //小米电机结构体
|
||||||
|
uint8_t CAN_ID; //CAN ID
|
||||||
|
uint8_t MCU_ID; //MCU唯一标识符[后8位，共64位]
|
||||||
|
float Angle; //回传角度
|
||||||
|
float Speed; //回传速度
|
||||||
|
float Torque; //回传力矩
|
||||||
|
float Temp; //回传温度
|
||||||
|
|
||||||
|
uint16_t set_current;
|
||||||
|
uint16_t set_speed;
|
||||||
|
uint16_t set_position;
|
||||||
|
|
||||||
|
uint8_t error_code;
|
||||||
|
|
||||||
|
float Angle_Bias;
|
||||||
|
|
||||||
|
}MI_Motor;
|
||||||
|
extern MI_Motor mi_motor[4];//预先定义四个小米电机
|
||||||
|
|
||||||
|
//小米
|
||||||
|
|
||||||
|
void XM_MotorEncode(void);
|
||||||
|
void xia0miInit(void);
|
||||||
|
extern void chack_cybergear(uint8_t ID);
|
||||||
|
extern void start_cybergear(MI_Motor *Motor);
|
||||||
|
extern void stop_cybergear(MI_Motor *Motor, uint8_t clear_error);
|
||||||
|
extern void set_mode_cybergear(MI_Motor *Motor, uint8_t Mode);
|
||||||
|
extern void set_current_cybergear(MI_Motor *Motor, float Current);
|
||||||
|
extern void set_zeropos_cybergear(MI_Motor *Motor);
|
||||||
|
extern void set_CANID_cybergear(MI_Motor *Motor, uint8_t CAN_ID);
|
||||||
|
extern void init_cybergear(MI_Motor *Motor, uint8_t Can_Id, uint8_t mode);
|
||||||
|
extern void motor_controlmode(MI_Motor *Motor,float torque, float MechPosition, float speed, float kp, float kd);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
@ -15,254 +15,365 @@
|
|||||||
****************************(C) COPYRIGHT ZhouCc****************************
|
****************************(C) COPYRIGHT ZhouCc****************************
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "djiMotor.h"
|
#include "djiMotor.h"
|
||||||
#include "can_it.h"
|
#include "can_it.h"
|
||||||
#include "can_init.h"
|
#include "can_init.h"
|
||||||
#if FREERTOS_DJI == 1
|
#if FREERTOS_DJI == 1
|
||||||
#include "TopDefine.h"
|
#include "TopDefine.h"
|
||||||
#include<cmsis_os2.h>
|
#include<cmsis_os2.h>
|
||||||
#include "FreeRTOS.h"
|
#include "FreeRTOS.h"
|
||||||
|
|
||||||
#include "odrive_can.h"
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
//motor data read
|
|
||||||
#define get_motor_measure(ptr, data) \
|
|
||||||
{ \
|
|
||||||
(ptr)->last_ecd = (ptr)->ecd; \
|
|
||||||
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
|
|
||||||
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
|
|
||||||
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
|
|
||||||
(ptr)->temperate = (data)[6]; \
|
|
||||||
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
|
|
||||||
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
|
|
||||||
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_3508; \
|
|
||||||
}//ptr指向电机测量数据结构体的指针，data包含电机测量数据的数组.
|
|
||||||
/*(ptr)->real_angle = (ptr)->real_angle % 360; */
|
|
||||||
#define get_motor_offset(ptr, data) \
|
|
||||||
{ \
|
|
||||||
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
|
|
||||||
(ptr)->offset_ecd = (ptr)->ecd; \
|
|
||||||
}
|
|
||||||
#define get_6020_motor_measure(ptr, data) \
|
|
||||||
{ \
|
|
||||||
(ptr)->last_ecd = (ptr)->ecd; \
|
|
||||||
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
|
|
||||||
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
|
|
||||||
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
|
|
||||||
(ptr)->temperate = (data)[6]; \
|
|
||||||
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
|
|
||||||
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
|
|
||||||
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020; \
|
|
||||||
}
|
|
||||||
#define get_2006_motor_measure(ptr, data) \
|
|
||||||
{ \
|
|
||||||
(ptr)->last_ecd = (ptr)->ecd; \
|
|
||||||
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
|
|
||||||
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
|
|
||||||
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
|
|
||||||
(ptr)->temperate = (data)[6]; \
|
|
||||||
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
|
|
||||||
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
|
|
||||||
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_2006; \
|
|
||||||
}
|
|
||||||
// 解析出初始编码器值
|
|
||||||
// #define get_5065motor_measure(ptr, data) \
|
|
||||||
// { \
|
|
||||||
// (ptr)->last_ecd = (ptr)->ecd; \
|
|
||||||
// (ptr)->ecd = (float)((data)[0] << 4 | (data)[1] <<3 |(data)[2] < 2 |(data)[3]); \
|
|
||||||
// (ptr)->speed_rpm = (float)((data)[4] << 4 | (data)[5] <<3 |(data)[6] < 2 |(data)[7]);; \
|
|
||||||
// }
|
|
||||||
/*(ptr)->real_angle = (ptr)->real_angle % 360; */
|
|
||||||
|
|
||||||
|
|
||||||
#if DEBUG == 1
|
|
||||||
motor_measure_t motor_chassis[5];
|
|
||||||
#else
|
|
||||||
static motor_measure_t motor_chassis[5];
|
|
||||||
#endif
|
|
||||||
static CAN_TxHeaderTypeDef tx_meessage_1ff;
|
|
||||||
static uint8_t can_send_data_1ff[8];
|
|
||||||
static CAN_TxHeaderTypeDef tx_meessage_2ff;
|
|
||||||
static uint8_t can_send_data_2ff[8];
|
|
||||||
static CAN_TxHeaderTypeDef tx_message_200;
|
|
||||||
static uint8_t can_send_data_200[8];
|
|
||||||
|
|
||||||
CAN_RxHeaderTypeDef dji_rx_header;
|
|
||||||
uint8_t dji_rx_data[8];
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief 数据处理函数
|
|
||||||
* @param[in] none
|
|
||||||
* @retval none
|
|
||||||
*/
|
|
||||||
void djiMotorEncode()
|
|
||||||
{
|
|
||||||
switch (dji_rx_header.StdId)
|
|
||||||
{
|
|
||||||
case 0x201:
|
|
||||||
case 0x202:
|
|
||||||
case 0x203:
|
|
||||||
case 0x204:
|
|
||||||
case 0x205:
|
|
||||||
case 0x206:
|
|
||||||
case 0x207:
|
|
||||||
{
|
|
||||||
static uint8_t i = 0;
|
|
||||||
//get motor id
|
|
||||||
i = dji_rx_header.StdId - 0x201;
|
|
||||||
if(motor_chassis[i].msg_cnt<=50)
|
|
||||||
{
|
|
||||||
motor_chassis[i].msg_cnt++;
|
|
||||||
get_motor_offset(&motor_chassis[i], dji_rx_data);
|
|
||||||
}else{
|
|
||||||
get_motor_measure(&motor_chassis[i], dji_rx_data);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
default:
|
|
||||||
{
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#if FREERTOS_DJI == 0
|
|
||||||
/**
|
|
||||||
* @brief hal库CAN回调函数,接收电机数据
|
|
||||||
* @param[in] hcan:CAN句柄指针
|
|
||||||
* @retval none
|
|
||||||
*/
|
|
||||||
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
|
|
||||||
{
|
|
||||||
HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
|
|
||||||
|
|
||||||
djiMotorEncode();
|
|
||||||
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
static osEventFlagsId_t eventReceive;//事件标志
|
|
||||||
/**
|
|
||||||
* @brief 自定义大疆电机回调函数
|
|
||||||
* @param[in] none
|
|
||||||
* @retval none
|
|
||||||
*/
|
|
||||||
void Dji_Motor_CB()
|
|
||||||
{
|
|
||||||
HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
|
|
||||||
|
|
||||||
osEventFlagsSet(eventReceive, EVENT_CAN);
|
static CAN_TxHeaderTypeDef vesc_tx_message;
|
||||||
}
|
static uint8_t vesc_send_data[4];
|
||||||
|
|
||||||
/**
|
#endif
|
||||||
* @brief 大疆电机初始化
|
|
||||||
* @param none
|
//motor data read
|
||||||
* @retval none
|
#define get_motor_measure(ptr, data) \
|
||||||
*/
|
{ \
|
||||||
void djiInit(void)
|
(ptr)->last_ecd = (ptr)->ecd; \
|
||||||
{
|
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
|
||||||
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
|
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
|
||||||
Dji_Motor_CB);
|
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
|
||||||
can_filter_init();
|
(ptr)->temperate = (data)[6]; \
|
||||||
}
|
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
|
||||||
|
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
|
||||||
/**
|
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_3508; \
|
||||||
* @brief 等待新数据
|
}//ptr指向电机测量数据结构体的指针，data包含电机测量数据的数组.
|
||||||
*/
|
/*(ptr)->real_angle = (ptr)->real_angle % 360; */
|
||||||
uint32_t waitNewDji()
|
#define get_motor_offset(ptr, data) \
|
||||||
{
|
{ \
|
||||||
return osEventFlagsWait(
|
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
|
||||||
eventReceive, EVENT_CAN,osFlagsWaitAny, osWaitForever);
|
(ptr)->offset_ecd = (ptr)->ecd; \
|
||||||
}
|
}
|
||||||
#endif
|
#define get_6020_motor_measure(ptr, data) \
|
||||||
|
{ \
|
||||||
/**
|
(ptr)->last_ecd = (ptr)->ecd; \
|
||||||
* @brief 发送电机控制电流(0x205,0x206,0x207,0x208)
|
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
|
||||||
* @param[in] motor1: (0x205) 电机控制电流
|
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
|
||||||
* @param[in] motor2: (0x206) 电机控制电流
|
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
|
||||||
* @param[in] motor3: (0x207) 电机控制电流
|
(ptr)->temperate = (data)[6]; \
|
||||||
* @param[in] motor4: (0x208) 电机控制电流
|
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
|
||||||
* @retval none
|
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
|
||||||
|
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020; \
|
||||||
|
}
|
||||||
|
#define get_2006_motor_measure(ptr, data) \
|
||||||
|
{ \
|
||||||
|
(ptr)->last_ecd = (ptr)->ecd; \
|
||||||
|
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
|
||||||
|
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
|
||||||
|
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
|
||||||
|
(ptr)->temperate = (data)[6]; \
|
||||||
|
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
|
||||||
|
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
|
||||||
|
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_2006; \
|
||||||
|
}
|
||||||
|
// 解析出初始编码器值
|
||||||
|
// #define get_5065motor_measure(ptr, data) \
|
||||||
|
// { \
|
||||||
|
// (ptr)->last_ecd = (ptr)->ecd; \
|
||||||
|
// (ptr)->ecd = (float)((data)[0] << 4 | (data)[1] <<3 |(data)[2] < 2 |(data)[3]); \
|
||||||
|
// (ptr)->speed_rpm = (float)((data)[4] << 4 | (data)[5] <<3 |(data)[6] < 2 |(data)[7]);; \
|
||||||
|
// }
|
||||||
|
/*(ptr)->real_angle = (ptr)->real_angle % 360; */
|
||||||
|
|
||||||
|
static void CAN_VescMotor_Decode_1(CAN_MotorFeedback_t *feedback,
|
||||||
|
const uint8_t *raw)
|
||||||
|
{
|
||||||
|
if (feedback == NULL || raw == NULL) return;
|
||||||
|
union
|
||||||
|
{
|
||||||
|
int x;
|
||||||
|
uint8_t data[4];
|
||||||
|
}speed;
|
||||||
|
|
||||||
|
speed.data[0]= raw[3];
|
||||||
|
speed.data[1]= raw[2];
|
||||||
|
speed.data[2]= raw[1];
|
||||||
|
speed.data[3]= raw[0];
|
||||||
|
feedback->rotor_speed = speed.x;
|
||||||
|
union
|
||||||
|
{
|
||||||
|
int16_t y;
|
||||||
|
uint8_t dat[2];
|
||||||
|
}current;
|
||||||
|
current.dat[0]= raw[5];
|
||||||
|
current.dat[1]= raw[4];
|
||||||
|
feedback->torque_current =(fp32)current.y/10;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#if DEBUG == 1
|
||||||
|
motor_measure_t motor_chassis[5];
|
||||||
|
CAN_MotorFeedback_t motor_6384;
|
||||||
|
|
||||||
|
#else
|
||||||
|
static motor_measure_t motor_chassis[5];
|
||||||
|
#endif
|
||||||
|
static CAN_TxHeaderTypeDef tx_meessage_1ff;
|
||||||
|
static uint8_t can_send_data_1ff[8];
|
||||||
|
static CAN_TxHeaderTypeDef tx_meessage_2ff;
|
||||||
|
static uint8_t can_send_data_2ff[8];
|
||||||
|
static CAN_TxHeaderTypeDef tx_message_200;
|
||||||
|
static uint8_t can_send_data_200[8];
|
||||||
|
|
||||||
|
CAN_RxHeaderTypeDef dji_rx_header;
|
||||||
|
uint8_t dji_rx_data[8];
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 数据处理函数
|
||||||
|
* @param[in] none
|
||||||
|
* @retval none
|
||||||
*/
|
*/
|
||||||
extern void CAN_cmd_1FF(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan)
|
void djiMotorEncode()
|
||||||
{
|
{
|
||||||
uint32_t send_mail_box;
|
if(dji_rx_header.IDE == CAN_ID_STD)
|
||||||
tx_meessage_1ff.StdId = 0x1FF;
|
{
|
||||||
tx_meessage_1ff.IDE = CAN_ID_STD;
|
switch (dji_rx_header.StdId)
|
||||||
tx_meessage_1ff.RTR = CAN_RTR_DATA;
|
{
|
||||||
tx_meessage_1ff.DLC = 0x08;
|
case 0x201:
|
||||||
can_send_data_1ff[0] = (motor1 >> 8);
|
case 0x202:
|
||||||
can_send_data_1ff[1] = motor1;
|
case 0x203:
|
||||||
can_send_data_1ff[2] = (motor2 >> 8);
|
case 0x204:
|
||||||
can_send_data_1ff[3] = motor2;
|
case 0x205:
|
||||||
can_send_data_1ff[4] = (motor3 >> 8);
|
case 0x206:
|
||||||
can_send_data_1ff[5] = motor3;
|
case 0x207:
|
||||||
can_send_data_1ff[6] = (motor4 >> 8);
|
{
|
||||||
can_send_data_1ff[7] = motor4;
|
static uint8_t i = 0;
|
||||||
HAL_CAN_AddTxMessage(hcan, &tx_meessage_1ff, can_send_data_1ff, &send_mail_box);
|
//get motor id
|
||||||
}
|
i = dji_rx_header.StdId - 0x201;
|
||||||
|
if(motor_chassis[i].msg_cnt<=50)
|
||||||
/**
|
{
|
||||||
* @brief 发送电机控制电流(0x201,0x202,0x203,0x204)
|
motor_chassis[i].msg_cnt++;
|
||||||
* @param[in] motor1: (0x201) 电机控制电流
|
get_motor_offset(&motor_chassis[i], dji_rx_data);
|
||||||
* @param[in] motor2: (0x202) 电机控制电流
|
}else{
|
||||||
* @param[in] motor3: (0x203) 电机控制电流
|
get_motor_measure(&motor_chassis[i], dji_rx_data);
|
||||||
* @param[in] motor4: (0x204) 电机控制电流
|
}
|
||||||
* @retval none
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
default:
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void vescMotorEncode()
|
||||||
|
{
|
||||||
|
switch (dji_rx_header.ExtId) {
|
||||||
|
|
||||||
|
case CAN_VESC5065_M1_MSG1:
|
||||||
|
// 存储消息到对应的电机结构体中
|
||||||
|
CAN_VescMotor_Decode_1(&motor_6384, dji_rx_data);
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#if FREERTOS_DJI == 0
|
||||||
|
/**
|
||||||
|
* @brief hal库CAN回调函数,接收电机数据
|
||||||
|
* @param[in] hcan:CAN句柄指针
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
|
||||||
|
{
|
||||||
|
HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
|
||||||
|
|
||||||
|
djiMotorEncode();
|
||||||
|
|
||||||
|
}
|
||||||
|
#else
|
||||||
|
static osEventFlagsId_t eventReceive;//事件标志
|
||||||
|
/**
|
||||||
|
* @brief 自定义大疆电机回调函数
|
||||||
|
* @param[in] none
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void Dji_Motor_CB()
|
||||||
|
{
|
||||||
|
HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
|
||||||
|
|
||||||
|
osEventFlagsSet(eventReceive, EVENT_CAN);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 大疆电机初始化
|
||||||
|
* @param none
|
||||||
|
* @retval none
|
||||||
*/
|
*/
|
||||||
void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan)
|
void djiInit(void)
|
||||||
{
|
{
|
||||||
uint32_t send_mail_box;
|
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
|
||||||
tx_message_200.StdId = 0x200;
|
Dji_Motor_CB);
|
||||||
tx_message_200.IDE = CAN_ID_STD;
|
can_filter_init();
|
||||||
tx_message_200.RTR = CAN_RTR_DATA;
|
}
|
||||||
tx_message_200.DLC = 0x08;
|
|
||||||
can_send_data_200[0] = motor1 >> 8;
|
/**
|
||||||
can_send_data_200[1] = motor1;
|
* @brief 等待新数据
|
||||||
can_send_data_200[2] = motor2 >> 8;
|
|
||||||
can_send_data_200[3] = motor2;
|
|
||||||
can_send_data_200[4] = motor3 >> 8;
|
|
||||||
can_send_data_200[5] = motor3;
|
|
||||||
can_send_data_200[6] = motor4 >> 8;
|
|
||||||
can_send_data_200[7] = motor4;
|
|
||||||
|
|
||||||
HAL_CAN_AddTxMessage(hcan, &tx_message_200, can_send_data_200, &send_mail_box);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief 发送电机控制电流(0x205,0x206,0x207,0x208)
|
|
||||||
* @param[in] motor1: (0x209) 电机控制电流
|
|
||||||
* @param[in] motor2: (0x20A) 电机控制电流
|
|
||||||
* @param[in] motor3: (0x20B) 电机控制电流
|
|
||||||
* @retval none
|
|
||||||
*/
|
*/
|
||||||
void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeDef*hcan)
|
uint32_t waitNewDji()
|
||||||
{
|
{
|
||||||
uint32_t send_mail_box;
|
return osEventFlagsWait(
|
||||||
tx_meessage_2ff.StdId = 0x1FF;
|
eventReceive, EVENT_CAN,osFlagsWaitAny, osWaitForever);
|
||||||
tx_meessage_2ff.IDE = CAN_ID_STD;
|
}
|
||||||
tx_meessage_2ff.RTR = CAN_RTR_DATA;
|
#endif
|
||||||
tx_meessage_2ff.DLC = 0x08;
|
|
||||||
can_send_data_2ff[0] = (motor1 >> 8);
|
/**
|
||||||
can_send_data_2ff[1] = motor1;
|
* @brief 发送电机控制电流(0x205,0x206,0x207,0x208)
|
||||||
can_send_data_2ff[2] = (motor2 >> 8);
|
* @param[in] motor1: (0x205) 电机控制电流
|
||||||
can_send_data_2ff[3] = motor2;
|
* @param[in] motor2: (0x206) 电机控制电流
|
||||||
can_send_data_2ff[4] = (motor3 >> 8);
|
* @param[in] motor3: (0x207) 电机控制电流
|
||||||
can_send_data_2ff[5] = motor3;
|
* @param[in] motor4: (0x208) 电机控制电流
|
||||||
can_send_data_2ff[6] = (0 >> 8);
|
* @retval none
|
||||||
can_send_data_2ff[7] = 0;
|
*/
|
||||||
HAL_CAN_AddTxMessage(hcan, &tx_meessage_2ff, can_send_data_2ff, &send_mail_box);
|
extern void CAN_cmd_1FF(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan)
|
||||||
|
{
|
||||||
|
uint32_t send_mail_box;
|
||||||
|
tx_meessage_1ff.StdId = 0x1FF;
|
||||||
|
tx_meessage_1ff.IDE = CAN_ID_STD;
|
||||||
|
tx_meessage_1ff.RTR = CAN_RTR_DATA;
|
||||||
|
tx_meessage_1ff.DLC = 0x08;
|
||||||
|
can_send_data_1ff[0] = (motor1 >> 8);
|
||||||
|
can_send_data_1ff[1] = motor1;
|
||||||
|
can_send_data_1ff[2] = (motor2 >> 8);
|
||||||
|
can_send_data_1ff[3] = motor2;
|
||||||
|
can_send_data_1ff[4] = (motor3 >> 8);
|
||||||
|
can_send_data_1ff[5] = motor3;
|
||||||
|
can_send_data_1ff[6] = (motor4 >> 8);
|
||||||
|
can_send_data_1ff[7] = motor4;
|
||||||
|
HAL_CAN_AddTxMessage(hcan, &tx_meessage_1ff, can_send_data_1ff, &send_mail_box);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 发送电机控制电流(0x201,0x202,0x203,0x204)
|
||||||
|
* @param[in] motor1: (0x201) 电机控制电流
|
||||||
|
* @param[in] motor2: (0x202) 电机控制电流
|
||||||
|
* @param[in] motor3: (0x203) 电机控制电流
|
||||||
|
* @param[in] motor4: (0x204) 电机控制电流
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan)
|
||||||
|
{
|
||||||
|
uint32_t send_mail_box;
|
||||||
|
tx_message_200.StdId = 0x200;
|
||||||
|
tx_message_200.IDE = CAN_ID_STD;
|
||||||
|
tx_message_200.RTR = CAN_RTR_DATA;
|
||||||
|
tx_message_200.DLC = 0x08;
|
||||||
|
can_send_data_200[0] = motor1 >> 8;
|
||||||
|
can_send_data_200[1] = motor1;
|
||||||
|
can_send_data_200[2] = motor2 >> 8;
|
||||||
|
can_send_data_200[3] = motor2;
|
||||||
|
can_send_data_200[4] = motor3 >> 8;
|
||||||
|
can_send_data_200[5] = motor3;
|
||||||
|
can_send_data_200[6] = motor4 >> 8;
|
||||||
|
can_send_data_200[7] = motor4;
|
||||||
|
|
||||||
|
HAL_CAN_AddTxMessage(hcan, &tx_message_200, can_send_data_200, &send_mail_box);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 发送电机控制电流(0x205,0x206,0x207,0x208)
|
||||||
|
* @param[in] motor1: (0x209) 电机控制电流
|
||||||
|
* @param[in] motor2: (0x20A) 电机控制电流
|
||||||
|
* @param[in] motor3: (0x20B) 电机控制电流
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeDef*hcan)
|
||||||
|
{
|
||||||
|
uint32_t send_mail_box;
|
||||||
|
tx_meessage_2ff.StdId = 0x1FF;
|
||||||
|
tx_meessage_2ff.IDE = CAN_ID_STD;
|
||||||
|
tx_meessage_2ff.RTR = CAN_RTR_DATA;
|
||||||
|
tx_meessage_2ff.DLC = 0x08;
|
||||||
|
can_send_data_2ff[0] = (motor1 >> 8);
|
||||||
|
can_send_data_2ff[1] = motor1;
|
||||||
|
can_send_data_2ff[2] = (motor2 >> 8);
|
||||||
|
can_send_data_2ff[3] = motor2;
|
||||||
|
can_send_data_2ff[4] = (motor3 >> 8);
|
||||||
|
can_send_data_2ff[5] = motor3;
|
||||||
|
can_send_data_2ff[6] = (0 >> 8);
|
||||||
|
can_send_data_2ff[7] = 0;
|
||||||
|
HAL_CAN_AddTxMessage(hcan, &tx_meessage_2ff, can_send_data_2ff, &send_mail_box);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 返回电机数据指针
|
||||||
|
* @param[in] i: 电机编号
|
||||||
|
* @retval 电机数据指针
|
||||||
|
*/
|
||||||
|
motor_measure_t *get_motor_point(uint8_t i)
|
||||||
|
{
|
||||||
|
return &motor_chassis[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 限制vesc电机转速
|
||||||
|
* @param[in/out] rpm: vesce电机转速
|
||||||
|
* @retval none
|
||||||
|
*
|
||||||
|
* 如果rpm的绝对值大于wtrcfg_VESC_COMMAND_ERPM_MAX,
|
||||||
|
* 则将rpm的值设置为wtrcfg_VESC_COMMAND_ERPM_MAX或-wtrcfg_VESC_COMMAND_ERPM_MAX
|
||||||
|
*/
|
||||||
|
void assert_param_rpm(float *rpm){
|
||||||
|
if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
|
||||||
|
*rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 使vesc电机进入转速模式
|
||||||
|
* @param[in] controller_id: vesc电机控制器id
|
||||||
|
* @param[in] RPM: 电机转速
|
||||||
|
* @retval RPM（1000-50000之间的数）
|
||||||
|
*/
|
||||||
|
void CAN_VESC_RPM(uint8_t controller_id, float RPM)
|
||||||
|
{
|
||||||
|
uint32_t id;
|
||||||
|
int32_t data;
|
||||||
|
uint32_t send_mail_box;
|
||||||
|
|
||||||
|
id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
|
||||||
|
assert_param_rpm(&RPM);
|
||||||
|
data = (int32_t)(RPM);
|
||||||
|
|
||||||
|
vesc_tx_message.ExtId = id;
|
||||||
|
vesc_tx_message.IDE = CAN_ID_EXT;
|
||||||
|
vesc_tx_message.RTR = CAN_RTR_DATA;
|
||||||
|
vesc_tx_message.DLC = 0x04;
|
||||||
|
vesc_send_data[0] = data >> 24 ;
|
||||||
|
vesc_send_data[1] = data >> 16 ;
|
||||||
|
vesc_send_data[2] = data >> 8 ;
|
||||||
|
vesc_send_data[3] = data ;
|
||||||
|
|
||||||
|
HAL_CAN_AddTxMessage(&hcan1, &vesc_tx_message, vesc_send_data, &send_mail_box);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 使vesc电机进入制动模式
|
||||||
|
* @param[in] controller_id: vesc电机控制器id
|
||||||
|
* @retval none
|
||||||
|
*/
|
||||||
|
void CAN_VESC_HEAD(uint8_t controller_id)
|
||||||
|
{
|
||||||
|
uint32_t id;
|
||||||
|
uint32_t send_mail_box;
|
||||||
|
|
||||||
|
id = controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8);
|
||||||
|
|
||||||
|
vesc_tx_message.ExtId = id;
|
||||||
|
vesc_tx_message.IDE = CAN_ID_EXT;
|
||||||
|
vesc_tx_message.RTR = CAN_RTR_DATA;
|
||||||
|
vesc_tx_message.DLC = 0x04;
|
||||||
|
|
||||||
|
HAL_CAN_AddTxMessage(&hcan1, &vesc_tx_message, vesc_send_data, &send_mail_box);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief 返回电机数据指针
|
|
||||||
* @param[in] i: 电机编号
|
|
||||||
* @retval 电机数据指针
|
|
||||||
*/
|
|
||||||
motor_measure_t *get_motor_point(uint8_t i)
|
|
||||||
{
|
|
||||||
return &motor_chassis[i];
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|||||||
@ -8,7 +8,10 @@ extern "C"{
|
|||||||
|
|
||||||
#include "struct_typedef.h"
|
#include "struct_typedef.h"
|
||||||
#include "can.h"
|
#include "can.h"
|
||||||
#include "odrive_can.h"
|
#include <math.h>
|
||||||
|
|
||||||
|
#define wtrcfg_VESC_COMMAND_ERPM_MAX 35000
|
||||||
|
#define CAN_VESC_CTRL_ID_BASE (0x300)
|
||||||
|
|
||||||
typedef enum{
|
typedef enum{
|
||||||
GM6020 = 0,
|
GM6020 = 0,
|
||||||
@ -44,6 +47,56 @@ typedef struct
|
|||||||
int16_t last_ecd;
|
int16_t last_ecd;
|
||||||
} odrive_measure_t;
|
} odrive_measure_t;
|
||||||
|
|
||||||
|
/* 电机反馈信息 */
|
||||||
|
typedef struct {
|
||||||
|
float rotor_ecd;
|
||||||
|
float rotor_speed;
|
||||||
|
float torque_current;
|
||||||
|
float temp;
|
||||||
|
} CAN_MotorFeedback_t;
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
CAN_PACKET_SET_DUTY = 0,
|
||||||
|
CAN_PACKET_SET_CURRENT = 1,
|
||||||
|
CAN_PACKET_SET_CURRENT_BRAKE =2,
|
||||||
|
CAN_PACKET_SET_RPM = 3,
|
||||||
|
CAN_PACKET_SET_POS = 4,
|
||||||
|
CAN_PACKET_FILL_RX_BUFFER = 5,
|
||||||
|
CAN_PACKET_FILL_RX_BUFFER_LONG = 6,
|
||||||
|
CAN_PACKET_PROCESS_RX_BUFFER = 7,
|
||||||
|
CAN_PACKET_PROCESS_SHORT_BUFFER = 8,
|
||||||
|
CAN_PACKET_STATUS = 9,
|
||||||
|
CAN_PACKET_SET_CURRENT_REL = 10,
|
||||||
|
CAN_PACKET_SET_CURRENT_BRAKE_REL = 11,
|
||||||
|
CAN_PACKET_SET_CURRENT_HANDBRAKE = 12,
|
||||||
|
CAN_PACKET_SET_CURRENT_HANDBRAKE_REL = 13
|
||||||
|
} CAN_PACKET_ID;
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
CAN_M3508_M1_ID = 0x201, /* 1 */
|
||||||
|
CAN_M3508_M2_ID = 0x202, /* 2 */
|
||||||
|
CAN_M3508_M3_ID = 0x203, /* 3 */
|
||||||
|
CAN_M3508_M4_ID = 0x204, /* 4 */
|
||||||
|
|
||||||
|
CAN_G6020_AgvM1 =0x205,
|
||||||
|
CAN_G6020_AgvM2 =0x206,
|
||||||
|
CAN_G6020_AgvM3 =0x207,
|
||||||
|
CAN_G6020_AgvM4 =0x208,
|
||||||
|
|
||||||
|
CAN_SICK_ID=0x301,
|
||||||
|
|
||||||
|
CAN_G6020_Pitch =0x209,
|
||||||
|
|
||||||
|
// CAN_VESC5065_M1 =0x211, //vesc的数据指令使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
|
||||||
|
// CAN_VESC5065_M2 =0x212,
|
||||||
|
// CAN_VESC5065_M3 =0x213,
|
||||||
|
// CAN_VSEC5065_M4 =0x214,
|
||||||
|
|
||||||
|
CAN_VESC5065_M1_MSG1 =0x90a, //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
|
||||||
|
CAN_VESC5065_M2_MSG1 =0x90b,
|
||||||
|
|
||||||
|
//
|
||||||
|
} CAN_MotorId_t;
|
||||||
|
|
||||||
//motor calc ecd to angle
|
//motor calc ecd to angle
|
||||||
|
|
||||||
@ -54,6 +107,8 @@ typedef struct
|
|||||||
|
|
||||||
#if FREERTOS_DJI == 1
|
#if FREERTOS_DJI == 1
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 大疆电机初始化
|
* @brief 大疆电机初始化
|
||||||
* @param none
|
* @param none
|
||||||
@ -103,12 +158,19 @@ extern void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_Hand
|
|||||||
extern motor_measure_t *get_motor_point(uint8_t i);
|
extern motor_measure_t *get_motor_point(uint8_t i);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 数据处理函数
|
* @brief 数据处理函数
|
||||||
* @param[in] none
|
* @param[in] none
|
||||||
* @retval none
|
* @retval none
|
||||||
*/
|
*/
|
||||||
void djiMotorEncode(void);
|
void djiMotorEncode(void);
|
||||||
|
void vescMotorEncode(void);
|
||||||
|
|
||||||
|
void CAN_VESC_RPM(uint8_t controller_id, float RPM);
|
||||||
|
|
||||||
|
void CAN_VESC_HEAD(uint8_t controller_id);
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
|
|||||||
@ -61,6 +61,25 @@ void Update_Encoder(Encoder_t *ptr) {
|
|||||||
ptr->last_ecd = ptr->ecd;
|
ptr->last_ecd = ptr->ecd;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// void Update_Encoder(Encoder_t *ptr) {
|
||||||
|
// ptr->ecd = AS5047_read(ANGLEUNC);
|
||||||
|
|
||||||
|
// // 检测编码器的圈数变化
|
||||||
|
// if (ptr->ecd - ptr->last_ecd > 4096) {
|
||||||
|
// ptr->round_cnt -= 1.0f; // 减去一圈
|
||||||
|
// } else if (ptr->ecd - ptr->last_ecd < -4096) {
|
||||||
|
// ptr->round_cnt += 1.0f; // 加上一圈
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // 计算总角度
|
||||||
|
// ptr->total_angle = (double)((ptr->round_cnt * 8192 + ptr->ecd - ptr->offset_ecd));
|
||||||
|
|
||||||
|
// // 更新 round_cnt 为小数圈数
|
||||||
|
// ptr->round_cnt = (float)(ptr->total_angle / 8192.0f); // 将总角度转换为圈数（浮点数）
|
||||||
|
|
||||||
|
// ptr->last_ecd = ptr->ecd;
|
||||||
|
// }
|
||||||
|
|
||||||
int32_t Get_Encoder_Rounds(Encoder_t *ptr) {
|
int32_t Get_Encoder_Rounds(Encoder_t *ptr) {
|
||||||
return ptr->round_cnt;
|
return ptr->round_cnt;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -19,7 +19,7 @@
|
|||||||
#define SETTINGS2 0x0019
|
#define SETTINGS2 0x0019
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
int32_t round_cnt;
|
float round_cnt;
|
||||||
uint16_t ecd;
|
uint16_t ecd;
|
||||||
uint16_t last_ecd;
|
uint16_t last_ecd;
|
||||||
uint16_t offset_ecd;
|
uint16_t offset_ecd;
|
||||||
|
|||||||
@ -3,203 +3,7 @@
|
|||||||
|
|
||||||
extern UART_HandleTypeDef huart3;
|
extern UART_HandleTypeDef huart3;
|
||||||
extern DMA_HandleTypeDef hdma_usart3_rx;
|
extern DMA_HandleTypeDef hdma_usart3_rx;
|
||||||
#if FREERTOS == 0
|
|
||||||
static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);
|
|
||||||
static int map(int x, int in_min, int in_max, int out_min, int out_max);
|
|
||||||
|
|
||||||
RC_ctrl_t rc_ctrl;
|
|
||||||
static uint8_t sbus_rx_buf[2][RC_FRAME_LENGTH];
|
|
||||||
//串口dma双缓冲区初始化
|
|
||||||
|
|
||||||
#if FREERTOS == 0
|
|
||||||
//串口中断
|
|
||||||
void USART3_IRQHandler(void)
|
|
||||||
{
|
|
||||||
//have received data
|
|
||||||
if(huart3.Instance->SR & UART_FLAG_RXNE)
|
|
||||||
{
|
|
||||||
//如果是接收中断则通过读取dr寄存器清零
|
|
||||||
__HAL_UART_CLEAR_FEFLAG(&huart3);
|
|
||||||
}
|
|
||||||
if(USART3->SR & UART_FLAG_IDLE)
|
|
||||||
{
|
|
||||||
//使用清除pe标志位的函数是因为pe idle等几个中断都是靠先读取sr再读取dr清零的
|
|
||||||
static uint16_t this_time_rx_len = 0;
|
|
||||||
__HAL_UART_CLEAR_PEFLAG(&huart3);
|
|
||||||
|
|
||||||
if( (hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET)
|
|
||||||
{
|
|
||||||
//current memory buffer used is memory0
|
|
||||||
|
|
||||||
//disable dma to change dma register
|
|
||||||
__HAL_DMA_DISABLE(&hdma_usart3_rx);
|
|
||||||
|
|
||||||
//get received data length, length = set_data_length - remain_length
|
|
||||||
this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
|
|
||||||
|
|
||||||
//reset set_data_length
|
|
||||||
hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
|
|
||||||
|
|
||||||
//change memory0 to memory1
|
|
||||||
hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT;
|
|
||||||
|
|
||||||
//enable dma
|
|
||||||
__HAL_DMA_ENABLE(&hdma_usart3_rx);
|
|
||||||
|
|
||||||
//1 frame length is correct data
|
|
||||||
if(this_time_rx_len == RC_FRAME_LENGTH)
|
|
||||||
{
|
|
||||||
sbus_to_rc(sbus_rx_buf[0], &rc_ctrl);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
__HAL_DMA_DISABLE(&hdma_usart3_rx);
|
|
||||||
|
|
||||||
this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
|
|
||||||
|
|
||||||
hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
|
|
||||||
|
|
||||||
//change memory1 to memory0
|
|
||||||
DMA1_Stream1->CR &= ~(DMA_SxCR_CT);
|
|
||||||
|
|
||||||
__HAL_DMA_ENABLE(&hdma_usart3_rx);
|
|
||||||
|
|
||||||
if(this_time_rx_len == RC_FRAME_LENGTH)
|
|
||||||
{
|
|
||||||
sbus_to_rc(sbus_rx_buf[1], &rc_ctrl);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
//如果打开了freertos，则只进行任务通知
|
|
||||||
void USART3_IRQHandler(void)
|
|
||||||
{
|
|
||||||
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num);
|
|
||||||
|
|
||||||
void remote_control_init(void)
|
|
||||||
{
|
|
||||||
// BSP_UART_RegisterCallback(BSP_UART_DR16,BSP_UART_IDLE_LINE_CB,DR16_IDLE_CB);//还没有移入嘉宁的架构，待测试
|
|
||||||
RC_init(sbus_rx_buf[0], sbus_rx_buf[1], RC_FRAME_LENGTH);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
|
|
||||||
{
|
|
||||||
if (sbus_buf == NULL || rc_ctrl == NULL)
|
|
||||||
{
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
rc_ctrl->ch[0] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff; //Channel 1
|
|
||||||
rc_ctrl->ch[1] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //Channel 2
|
|
||||||
rc_ctrl->ch[2] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) | //Channel 3
|
|
||||||
(sbus_buf[5] << 10)) &0x07ff;
|
|
||||||
rc_ctrl->ch[3] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //Channel 4
|
|
||||||
|
|
||||||
rc_ctrl->sw[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF; //Channel 5
|
|
||||||
rc_ctrl->sw[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF; //Channel 6
|
|
||||||
rc_ctrl->sw[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;; //Channel 7
|
|
||||||
rc_ctrl->sw[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF; //Channel 8
|
|
||||||
rc_ctrl->sw[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF; //Channel 9
|
|
||||||
rc_ctrl->sw[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF; //Channel 10
|
|
||||||
rc_ctrl->sw[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF; //Channel 11
|
|
||||||
rc_ctrl->sw[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF; //Channel 12
|
|
||||||
|
|
||||||
|
|
||||||
rc_ctrl->sw[2] = map(rc_ctrl->sw[2],306,1694,1694,306);
|
|
||||||
rc_ctrl->sw[3] = map(rc_ctrl->sw[3],306,1694,1694,306);
|
|
||||||
rc_ctrl->ch[1] = map(rc_ctrl->ch[1],-693,694,-700,700); //x
|
|
||||||
rc_ctrl->ch[0] = map(rc_ctrl->ch[0],-694,693,-700,700); //y
|
|
||||||
|
|
||||||
//死区(-30,30)
|
|
||||||
if(rc_ctrl->ch[0]>-14&&rc_ctrl->ch[0]<6) rc_ctrl->ch[0]=0;
|
|
||||||
if(rc_ctrl->ch[1]>-30&&rc_ctrl->ch[1]<-10) rc_ctrl->ch[1]=0;
|
|
||||||
if(rc_ctrl->ch[2]>=0&&rc_ctrl->ch[2]<=7) rc_ctrl->ch[2]=7;
|
|
||||||
if(rc_ctrl->ch[3]>-22&&rc_ctrl->ch[3]<-2) rc_ctrl->ch[3]=0;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int map(int x, int in_min, int in_max, int out_min, int out_max) //映射函数
|
|
||||||
{
|
|
||||||
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/*
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
306 306
|
|
||||||
sw[] sw[7]
|
|
||||||
1694 1694
|
|
||||||
|
|
||||||
306 306
|
|
||||||
sw[6] sw[4]
|
|
||||||
1694 1694
|
|
||||||
|
|
||||||
306 306 306 306
|
|
||||||
sw[0] sw[2] sw[1]:306-1694 sw[5]:306-1694 sw[]1000 sw[3]
|
|
||||||
1694 1694 1694 1694
|
|
||||||
710
|
|
||||||
688 1425
|
|
||||||
| |
|
|
||||||
| |
|
|
||||||
54 -616------ch[3]770 -354---------ch[0] 339 0
|
|
||||||
| |
|
|
||||||
| |
|
|
||||||
ch[2] ch[1]
|
|
||||||
_699 38
|
|
||||||
*/
|
|
||||||
|
|
||||||
|
|
||||||
void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
|
|
||||||
{
|
|
||||||
|
|
||||||
//enable the dma transfer for the receiver request
|
|
||||||
SET_BIT(huart3.Instance->CR3, USART_CR3_DMAR);
|
|
||||||
|
|
||||||
//enable idle interrupt
|
|
||||||
__HAL_UART_ENABLE_IT(&huart3, UART_IT_IDLE);
|
|
||||||
|
|
||||||
//disable dma, to change the dma register
|
|
||||||
__HAL_DMA_DISABLE(&hdma_usart3_rx);
|
|
||||||
|
|
||||||
while(hdma_usart3_rx.Instance->CR & DMA_SxCR_EN)
|
|
||||||
{
|
|
||||||
__HAL_DMA_DISABLE(&hdma_usart3_rx);
|
|
||||||
}
|
|
||||||
|
|
||||||
hdma_usart3_rx.Instance->PAR = (uint32_t) & (USART3->DR);
|
|
||||||
|
|
||||||
//memory buffer 1
|
|
||||||
hdma_usart3_rx.Instance->M0AR = (uint32_t)(rx1_buf);
|
|
||||||
|
|
||||||
//momory buffer 2
|
|
||||||
hdma_usart3_rx.Instance->M1AR = (uint32_t)(rx2_buf);
|
|
||||||
|
|
||||||
//data length
|
|
||||||
hdma_usart3_rx.Instance->NDTR = dma_buf_num;
|
|
||||||
|
|
||||||
//enable double memory buffer
|
|
||||||
SET_BIT(hdma_usart3_rx.Instance->CR, DMA_SxCR_DBM);
|
|
||||||
|
|
||||||
|
|
||||||
//enable dma
|
|
||||||
__HAL_DMA_ENABLE(&hdma_usart3_rx);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
#else
|
|
||||||
//DMA双缓冲区+串口空闲中断
|
//DMA双缓冲区+串口空闲中断
|
||||||
|
|
||||||
static osEventFlagsId_t eventReceive;
|
static osEventFlagsId_t eventReceive;
|
||||||
@ -351,5 +155,26 @@ RC_data_t* get_rc_data()
|
|||||||
return &RC_mess.RC_data;
|
return &RC_mess.RC_data;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
|
||||||
|
306 306
|
||||||
|
sw[] sw[7]
|
||||||
|
1694 1694
|
||||||
|
|
||||||
|
306 306
|
||||||
|
sw[6] sw[4]
|
||||||
|
1694 1694
|
||||||
|
|
||||||
#endif
|
306 306 306 306
|
||||||
|
sw[0] sw[2] sw[1]:306-1694 sw[5]:306-1694 sw[]1000 sw[3]
|
||||||
|
1694 1694 1694 1694
|
||||||
|
710
|
||||||
|
688 1425
|
||||||
|
| |
|
||||||
|
| |
|
||||||
|
54 -616------ch[3]770 -354---------ch[0] 339 0
|
||||||
|
| |
|
||||||
|
| |
|
||||||
|
ch[2] ch[1]
|
||||||
|
_699 38
|
||||||
|
*/
|
||||||
|
|||||||
@ -1,6 +1,6 @@
|
|||||||
#include "calc_lib.h"
|
#include "calc_lib.h"
|
||||||
|
|
||||||
//微秒延时
|
//??
|
||||||
void user_delay_us(uint16_t us)
|
void user_delay_us(uint16_t us)
|
||||||
{
|
{
|
||||||
for(; us > 0; us--)
|
for(; us > 0; us--)
|
||||||
@ -12,7 +12,7 @@ void user_delay_us(uint16_t us)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//毫秒延时
|
//??
|
||||||
void user_delay_ms(uint16_t ms)
|
void user_delay_ms(uint16_t ms)
|
||||||
{
|
{
|
||||||
for(; ms > 0; ms--)
|
for(; ms > 0; ms--)
|
||||||
@ -21,7 +21,7 @@ void user_delay_ms(uint16_t ms)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//浮点数范围限制
|
//??????????????????
|
||||||
void abs_limit_fp(fp32 *num, fp32 Limit)
|
void abs_limit_fp(fp32 *num, fp32 Limit)
|
||||||
{
|
{
|
||||||
if (*num > Limit)
|
if (*num > Limit)
|
||||||
@ -34,7 +34,7 @@ void abs_limit_fp(fp32 *num, fp32 Limit)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//整数范围限制
|
//??????????????????
|
||||||
void abs_limit_int(int64_t *num, int64_t Limit)
|
void abs_limit_int(int64_t *num, int64_t Limit)
|
||||||
{
|
{
|
||||||
if (*num > Limit)
|
if (*num > Limit)
|
||||||
@ -47,7 +47,7 @@ void abs_limit_int(int64_t *num, int64_t Limit)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//循环限幅
|
//?????? Input ??? minValue ? maxValue ??
|
||||||
fp32 loop_fp32_constrain(fp32 Input, fp32 minValue, fp32 maxValue)
|
fp32 loop_fp32_constrain(fp32 Input, fp32 minValue, fp32 maxValue)
|
||||||
{
|
{
|
||||||
if (maxValue < minValue)
|
if (maxValue < minValue)
|
||||||
@ -101,12 +101,22 @@ int32_t loop_int32_constrain(int32_t Input, int32_t minValue, int32_t maxValue)
|
|||||||
return Input;
|
return Input;
|
||||||
}
|
}
|
||||||
|
|
||||||
int map(int x, int in_min, int in_max, int out_min, int out_max) //映射函数
|
/**
|
||||||
|
* @brief ??????
|
||||||
|
*
|
||||||
|
* @param x ???
|
||||||
|
* @param in_min ????????
|
||||||
|
* @param in_max ????????
|
||||||
|
* @param out_min ????????
|
||||||
|
* @param out_max ????????
|
||||||
|
* @return ???????
|
||||||
|
*/
|
||||||
|
int map(int x, int in_min, int in_max, int out_min, int out_max)
|
||||||
{
|
{
|
||||||
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
||||||
}
|
}
|
||||||
|
|
||||||
fp32 map_fp32(fp32 x, fp32 in_min, fp32 in_max, fp32 out_min, fp32 out_max) //映射函数
|
fp32 map_fp32(fp32 x, fp32 in_min, fp32 in_max, fp32 out_min, fp32 out_max)
|
||||||
{
|
{
|
||||||
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -112,3 +112,26 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
|
|||||||
/* 不为裁判系统设定值时,计算转速 */
|
/* 不为裁判系统设定值时,计算转速 */
|
||||||
return 60.0f * (float)bullet_speed / (M_2PI * fric_radius);
|
return 60.0f * (float)bullet_speed / (M_2PI * fric_radius);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 角度单位转换
|
||||||
|
*
|
||||||
|
* @param[in] hopetype: 需要转换的角度单位
|
||||||
|
* @param[in] angle: 需要转换的角度值
|
||||||
|
* @return 转换后的角度值
|
||||||
|
*/
|
||||||
|
fp32 AngleChange(Angle_e hopetype,fp32 angle)
|
||||||
|
{
|
||||||
|
fp32 hope_angle;
|
||||||
|
if(hopetype==DEGREE)
|
||||||
|
{
|
||||||
|
hope_angle = angle*180/M_PI; //弧度制转角度制
|
||||||
|
}
|
||||||
|
else if(hopetype ==RADIAN)
|
||||||
|
{
|
||||||
|
hope_angle = angle*M_PI/180; //角度值转弧度制
|
||||||
|
}
|
||||||
|
return hope_angle;
|
||||||
|
|
||||||
|
}
|
||||||
|
|||||||
@ -14,6 +14,7 @@ extern "C" {
|
|||||||
#include <float.h>
|
#include <float.h>
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
|
#include "struct_typedef.h"
|
||||||
|
|
||||||
#define M_DEG2RAD_MULT (0.01745329251f)
|
#define M_DEG2RAD_MULT (0.01745329251f)
|
||||||
#define M_RAD2DEG_MULT (57.2957795131f)
|
#define M_RAD2DEG_MULT (57.2957795131f)
|
||||||
@ -47,6 +48,12 @@ typedef struct {
|
|||||||
float wz; /* 转动 */
|
float wz; /* 转动 */
|
||||||
} MoveVector_t;
|
} MoveVector_t;
|
||||||
|
|
||||||
|
//定义获取角度的量纲
|
||||||
|
typedef enum {
|
||||||
|
DEGREE,//0-360
|
||||||
|
RADIAN,//(0-2pi)
|
||||||
|
}Angle_e;
|
||||||
|
|
||||||
float InvSqrt(float x);
|
float InvSqrt(float x);
|
||||||
|
|
||||||
float AbsClip(float in, float limit);
|
float AbsClip(float in, float limit);
|
||||||
@ -101,6 +108,15 @@ void CircleReverse(float *origin);
|
|||||||
*/
|
*/
|
||||||
float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
|
float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 角度单位转换
|
||||||
|
*
|
||||||
|
* @param[in] hopetype: 需要转换的角度单位
|
||||||
|
* @param[in] angle: 需要转换的角度值
|
||||||
|
* @return 转换后的角度值
|
||||||
|
*/
|
||||||
|
fp32 AngleChange(Angle_e hopetype,fp32 angle);
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -1,64 +1,82 @@
|
|||||||
#include "shoot.h"
|
#include "shoot.h"
|
||||||
#include "remote_control.h"
|
#include "remote_control.h"
|
||||||
|
#include "read_spi.h"
|
||||||
|
|
||||||
extern RC_mess_t RC_mess;
|
extern RC_mess_t RC_mess;
|
||||||
|
extern Encoder_t encoder;
|
||||||
extern motor_measure_t *trigger_motor_data;//3508电机数据
|
extern motor_measure_t *trigger_motor_data;//3508电机数据
|
||||||
|
|
||||||
|
int mode=0;
|
||||||
|
int shoot=0;
|
||||||
|
int xxx=0;
|
||||||
|
|
||||||
#define GO1_SHOOT 0
|
#define Stop 0
|
||||||
#define ODRIVE_SHOOT 1
|
#define Hasten 1
|
||||||
|
#define Slow 2
|
||||||
|
#define Shoot 3
|
||||||
|
#define VESC_ID 77
|
||||||
|
|
||||||
#define KP 0.12
|
int speed_6384=-1500;
|
||||||
#define KD 0.008
|
|
||||||
|
|
||||||
|
void control_clutch_shoot(void)
|
||||||
#if GO1_SHOOT == 1
|
|
||||||
GO_SHOOT goshoot;
|
|
||||||
|
|
||||||
void shooterInit(void)
|
|
||||||
{
|
{
|
||||||
int i;
|
|
||||||
GO_M8010_init();
|
|
||||||
for(i = 0;i < GO_NUM;i ++)
|
|
||||||
{
|
|
||||||
goshoot.goData[i] = getGoPoint(i);//获取电机数据指针
|
|
||||||
|
|
||||||
goshoot.angleSetgo[i] = 0;
|
if(mode == Stop)
|
||||||
goshoot.offestAngle[i] = 0;
|
{
|
||||||
// GO_M8010_send_data(&huart1, i,0,0,0,0,0,0);
|
|
||||||
GO_M8010_send_data(&huart6, i,0,0,0,0,0,0);
|
if(encoder.round_cnt == 0)
|
||||||
goshoot.offestAngle[i] = goshoot.goData[i]->Pos;
|
{
|
||||||
HAL_Delay(100);
|
|
||||||
// GO_M8010_send_data(&huart1, i,0,0,0,0,0,0);
|
CAN_VESC_HEAD(VESC_ID);
|
||||||
GO_M8010_send_data(&huart6, i,0,0,0,0,0,0);
|
}
|
||||||
goshoot.offestAngle[i] = goshoot.goData[i]->Pos;
|
|
||||||
HAL_Delay(100);
|
|
||||||
|
}
|
||||||
|
else if(mode == Hasten)
|
||||||
|
{
|
||||||
|
if(shoot ==0)
|
||||||
|
{
|
||||||
|
if(encoder.round_cnt <= 2)
|
||||||
|
{
|
||||||
|
|
||||||
|
xxx=1;
|
||||||
|
CAN_VESC_RPM(VESC_ID, speed_6384);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
else if(shoot ==Shoot)
|
||||||
|
{
|
||||||
|
if(encoder.round_cnt <= 3)
|
||||||
|
{
|
||||||
|
xxx=3;
|
||||||
|
HAL_GPIO_WritePin(key_GPIO_Port,key_Pin,GPIO_PIN_SET);
|
||||||
|
|
||||||
|
CAN_VESC_RPM(VESC_ID, speed_6384);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// xxx=3;
|
||||||
|
// HAL_GPIO_WritePin(key_GPIO_Port,key_Pin,GPIO_PIN_SET);
|
||||||
|
// CAN_VESC_RPM(VESC_ID, speed_6384);
|
||||||
|
|
||||||
|
if(encoder.round_cnt >= 3)
|
||||||
|
{
|
||||||
|
xxx=4;
|
||||||
|
shoot=Slow;
|
||||||
|
HAL_GPIO_WritePin(key_GPIO_Port,key_Pin,GPIO_PIN_RESET);
|
||||||
|
|
||||||
|
CAN_VESC_HEAD(VESC_ID);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
else if(shoot == Slow)
|
||||||
|
{
|
||||||
|
|
||||||
|
CAN_VESC_HEAD(VESC_ID);
|
||||||
|
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void shoot_ball(int key)
|
|
||||||
{
|
|
||||||
|
|
||||||
|
|
||||||
//蓄力
|
|
||||||
if(trigger_motor_data->real_angle ==60)//扳机已闭合
|
|
||||||
{
|
|
||||||
goshoot.angleSetgo[0] = 10;
|
|
||||||
GO_M8010_send_data(&huart6, 0,0,0,goshoot.angleSetgo[0],1,KP,KD);
|
|
||||||
}
|
|
||||||
if (key ==2)//上升准备蓄力
|
|
||||||
{
|
|
||||||
goshoot.angleSetgo[0] = 0;
|
|
||||||
GO_M8010_send_data(&huart6, 0,0,0,goshoot.angleSetgo[0],1,KP,KD);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|||||||
@ -2,26 +2,11 @@
|
|||||||
#define _SHOOT_H_
|
#define _SHOOT_H_
|
||||||
|
|
||||||
#include "GO_M8010_6_Driver.h"
|
#include "GO_M8010_6_Driver.h"
|
||||||
#include "odrive_can.h"
|
|
||||||
#include "djiMotor.h"
|
#include "djiMotor.h"
|
||||||
#include "calc_lib.h"
|
#include "calc_lib.h"
|
||||||
#include "pid.h"
|
#include "pid.h"
|
||||||
|
#include "main.h"
|
||||||
|
|
||||||
typedef struct
|
void control_clutch_shoot(void);
|
||||||
{
|
|
||||||
/* data */
|
|
||||||
GO_Motorfield* goData[GO_NUM];
|
|
||||||
//暂存目标位置
|
|
||||||
//0 left，1 right
|
|
||||||
float angleSetgo[GO_NUM];
|
|
||||||
float offestAngle[GO_NUM];//go数据
|
|
||||||
float Kp;
|
|
||||||
float Kd;
|
|
||||||
float allowRange;
|
|
||||||
}GO_SHOOT;
|
|
||||||
|
|
||||||
void shooterInit(void);
|
|
||||||
void shoot_ball(int key);
|
|
||||||
void shoot_odrive(int angle);
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -35,7 +35,7 @@ const osThreadAttr_t attr_motor = {
|
|||||||
.priority = osPriorityRealtime,
|
.priority = osPriorityRealtime,
|
||||||
.stack_size = 128 * 4,
|
.stack_size = 128 * 4,
|
||||||
};
|
};
|
||||||
//odrive_shoot
|
//shoot
|
||||||
const osThreadAttr_t attr_shoot = {
|
const osThreadAttr_t attr_shoot = {
|
||||||
.name = "shoot",
|
.name = "shoot",
|
||||||
.priority = osPriorityRealtime,
|
.priority = osPriorityRealtime,
|
||||||
|
|||||||
@ -5,6 +5,7 @@
|
|||||||
#include "TopDefine.h"
|
#include "TopDefine.h"
|
||||||
#include "attrTask.h"
|
#include "attrTask.h"
|
||||||
#include "djiMotor.h"
|
#include "djiMotor.h"
|
||||||
|
#include "Xiaomi.h"
|
||||||
extern motor_measure_t *motor_3508_data;
|
extern motor_measure_t *motor_3508_data;
|
||||||
/**
|
/**
|
||||||
* \brief can任务
|
* \brief can任务
|
||||||
@ -16,7 +17,8 @@ void Task_Can(void *argument)
|
|||||||
(void)argument; /* 未使用argument，消除警告 */
|
(void)argument; /* 未使用argument，消除警告 */
|
||||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
|
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
|
||||||
|
|
||||||
djiInit();//FIFO滤波器初始化
|
djiInit();//FIFO滤波器初始化
|
||||||
|
xia0miInit();//小米电机初始化
|
||||||
|
|
||||||
|
|
||||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
|
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
|
||||||
@ -25,7 +27,8 @@ void Task_Can(void *argument)
|
|||||||
{
|
{
|
||||||
waitNewDji();//等待新数据
|
waitNewDji();//等待新数据
|
||||||
djiMotorEncode();//数据解析
|
djiMotorEncode();//数据解析
|
||||||
|
XM_MotorEncode();//数据解析
|
||||||
|
vescMotorEncode();//数据解析
|
||||||
//将can数据添加到消息队列
|
//将can数据添加到消息队列
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -13,13 +13,10 @@ extern RC_mess_t RC_mess;
|
|||||||
extern int16_t result;
|
extern int16_t result;
|
||||||
extern int16_t t_result;
|
extern int16_t t_result;
|
||||||
extern motor_measure_t *trigger_motor_data;
|
extern motor_measure_t *trigger_motor_data;
|
||||||
Encoder_t encoder;
|
|
||||||
float vofa[8];
|
|
||||||
|
|
||||||
int speed=0;
|
float vofa[8];
|
||||||
float m=0;
|
int tangle=0;
|
||||||
float trigger_angle_o=0;
|
|
||||||
int mode=0;
|
|
||||||
/**
|
/**
|
||||||
* \brief 电机任务
|
* \brief 电机任务
|
||||||
*
|
*
|
||||||
@ -32,36 +29,14 @@ void Task_Motor(void *argument)
|
|||||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_CHASSIS;
|
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_CHASSIS;
|
||||||
|
|
||||||
motor_init();
|
motor_init();
|
||||||
Encoder_Init(&encoder);
|
|
||||||
|
|
||||||
uint32_t tick = osKernelGetTickCount();
|
uint32_t tick = osKernelGetTickCount();
|
||||||
|
|
||||||
while(1)
|
while(1)
|
||||||
{
|
{
|
||||||
//收到消息队列新数据
|
|
||||||
|
trigger_pos(tangle);
|
||||||
// 更新编码器数据
|
|
||||||
Update_Encoder(&encoder);
|
|
||||||
m=trigger_angle_o*(8191/360);
|
|
||||||
if( mode == THREE )
|
|
||||||
{
|
|
||||||
//当最高点时进入离合
|
|
||||||
if(encoder.round_cnt>=8)
|
|
||||||
{
|
|
||||||
trigger_angle_o=2;
|
|
||||||
trigger_pos(m);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
else if( mode == DZ )
|
|
||||||
{
|
|
||||||
//退出离合
|
|
||||||
trigger_angle_o=0;
|
|
||||||
trigger_pos(m);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
CAN_cmd_200(t_result,0,0,0,&hcan1);
|
CAN_cmd_200(t_result,0,0,0,&hcan1);
|
||||||
osDelay(2);
|
osDelay(2);
|
||||||
|
|
||||||
|
|||||||
@ -7,14 +7,19 @@
|
|||||||
#include "main.h"
|
#include "main.h"
|
||||||
#include "remote_control.h"
|
#include "remote_control.h"
|
||||||
#include "led.h"
|
#include "led.h"
|
||||||
|
#include "djiMotor.h"
|
||||||
|
#include "Xiaomi.h"
|
||||||
|
|
||||||
#define GOUSE 1
|
#define GOUSE 1
|
||||||
|
|
||||||
extern RC_mess_t RC_mess;
|
extern RC_mess_t RC_mess;
|
||||||
|
int anglexiaomi = 0;
|
||||||
|
|
||||||
void Task_Go(void *argument)
|
void Task_Go(void *argument)
|
||||||
{
|
{
|
||||||
#if GOUSE==1
|
#if GOUSE==1
|
||||||
|
HAL_Delay(500); //一定时间延时 等待电机初始化完成
|
||||||
|
init_cybergear(&mi_motor[0], 0x01, Motion_mode);//小米电机 启动!
|
||||||
//HAL_Delay(2000);
|
//HAL_Delay(2000);
|
||||||
// gimbalInit();
|
// gimbalInit();
|
||||||
//HAL_GPIO_WritePin(ledGreen_GPIO_Port,ledGreen_Pin,GPIO_PIN_RESET);
|
//HAL_GPIO_WritePin(ledGreen_GPIO_Port,ledGreen_Pin,GPIO_PIN_RESET);
|
||||||
@ -23,6 +28,8 @@ void Task_Go(void *argument)
|
|||||||
{
|
{
|
||||||
|
|
||||||
LED_green();
|
LED_green();
|
||||||
|
LED_bule();
|
||||||
|
motor_controlmode(&mi_motor[0], 0.5, anglexiaomi, 4, 0.5 , 0.5); //小米暂时锁住就行
|
||||||
// gimbalFlow();
|
// gimbalFlow();
|
||||||
osDelay(1);
|
osDelay(1);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -37,7 +37,7 @@ void Task_Init(void *argument) {
|
|||||||
// dji_motor
|
// dji_motor
|
||||||
task_runtime.thread.dji_motor =
|
task_runtime.thread.dji_motor =
|
||||||
osThreadNew(Task_Motor, NULL, &attr_motor);
|
osThreadNew(Task_Motor, NULL, &attr_motor);
|
||||||
// odrive_shoot
|
// shoot
|
||||||
task_runtime.thread.shoot =
|
task_runtime.thread.shoot =
|
||||||
osThreadNew(Task_Shoot, NULL, &attr_shoot);
|
osThreadNew(Task_Shoot, NULL, &attr_shoot);
|
||||||
// go
|
// go
|
||||||
|
|||||||
@ -7,53 +7,32 @@
|
|||||||
#include "shoot.h"
|
#include "shoot.h"
|
||||||
#include "read_spi.h"
|
#include "read_spi.h"
|
||||||
#include "dji_task.h"
|
#include "dji_task.h"
|
||||||
|
#include "djiMotor.h"
|
||||||
|
|
||||||
extern RC_mess_t RC_mess;
|
extern RC_mess_t RC_mess;
|
||||||
extern Encoder_t encoder;
|
|
||||||
extern motor_measure_t *trigger_motor_data;//3508电机数据
|
|
||||||
extern int mode;
|
|
||||||
|
|
||||||
#define GO1_SHOOT 0
|
Encoder_t encoder;
|
||||||
#define ODRIVE_SHOOT 0
|
|
||||||
|
|
||||||
//odrive发射
|
|
||||||
#define SHOOT3 12
|
|
||||||
#define TOP 6
|
|
||||||
#define MIDDLE 3
|
|
||||||
#define BOTTOM 0
|
|
||||||
|
|
||||||
void Task_Shoot(void *argument)
|
void Task_Shoot(void *argument)
|
||||||
{
|
{
|
||||||
(void)argument; /* 未使用argument，消除警告 */
|
(void)argument; /* 未使用argument，消除警告 */
|
||||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
|
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
|
||||||
|
|
||||||
#if GO1_SHOOT == 1
|
Encoder_Init(&encoder);
|
||||||
shooterInit();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
|
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
|
||||||
|
|
||||||
while(1)
|
while(1)
|
||||||
{
|
{
|
||||||
#if GO1_SHOOT == 1
|
|
||||||
|
|
||||||
shoot_ball(0);
|
|
||||||
|
|
||||||
#elif ODRIVE_SHOOT == 1
|
//收到消息队列新数据
|
||||||
if(mode == THREE)
|
|
||||||
{
|
// 更新编码器数据
|
||||||
|
Update_Encoder(&encoder);
|
||||||
}
|
|
||||||
else if(mode == DZ)
|
|
||||||
{
|
|
||||||
if(trigger_motor_data->total_angle ==0)//扳机已闭合
|
|
||||||
{
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
control_clutch_shoot();
|
||||||
|
|
||||||
|
|
||||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||||
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
|
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user