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/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'
|
||||
Build target 'mycode1'
|
||||
compiling shoot_task.c...
|
||||
compiling shoot.c...
|
||||
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...
|
||||
"mycode1\mycode1.axf" - 0 Error(s), 0 Warning(s).
|
||||
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>
|
||||
<ItemText>trigger_angle_o</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>16</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>speed_6384,0x0A</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<WatchWindow2>
|
||||
<Ww>
|
||||
@ -868,7 +873,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/device</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1004,7 +1009,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/module</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
|
||||
## r1上层
|
||||
|
||||
1. **odrive**推射 **can1** **id 010** **spi2**读取as5047p
|
||||
1. **vesc** **can1 id:77** **spi2**读取as5047p
|
||||
2. go1 RS485模块 01翻转 02云台
|
||||
3. 爪子 左三PC6
|
||||
4. 接球 左一左2 PI7 PI6
|
||||
@ -18,18 +18,18 @@ void can_filter_init(void)
|
||||
can_filter_st.FilterMaskIdHigh = 0x0000;
|
||||
can_filter_st.FilterMaskIdLow = 0x0000;
|
||||
can_filter_st.FilterBank = 0;
|
||||
can_filter_st.SlaveStartFilterBank = 14;
|
||||
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
|
||||
HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
|
||||
HAL_CAN_Start(&hcan1);
|
||||
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
|
||||
|
||||
|
||||
can_filter_st.SlaveStartFilterBank = 14;
|
||||
|
||||
can_filter_st.FilterBank = 14;
|
||||
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO1;
|
||||
HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);
|
||||
HAL_CAN_Start(&hcan2);
|
||||
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
|
||||
@ -23,7 +23,8 @@
|
||||
#include<cmsis_os2.h>
|
||||
#include "FreeRTOS.h"
|
||||
|
||||
#include "odrive_can.h"
|
||||
static CAN_TxHeaderTypeDef vesc_tx_message;
|
||||
static uint8_t vesc_send_data[4];
|
||||
|
||||
#endif
|
||||
|
||||
@ -76,9 +77,36 @@
|
||||
// }
|
||||
/*(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
|
||||
@ -92,12 +120,15 @@ 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()
|
||||
{
|
||||
if(dji_rx_header.IDE == CAN_ID_STD)
|
||||
{
|
||||
switch (dji_rx_header.StdId)
|
||||
{
|
||||
@ -129,6 +160,24 @@ void djiMotorEncode()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
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回调函数,接收电机数据
|
||||
@ -266,3 +315,65 @@ 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);
|
||||
}
|
||||
|
||||
@ -8,7 +8,10 @@ extern "C"{
|
||||
|
||||
#include "struct_typedef.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{
|
||||
GM6020 = 0,
|
||||
@ -44,6 +47,56 @@ typedef struct
|
||||
int16_t last_ecd;
|
||||
} 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
|
||||
|
||||
@ -54,6 +107,8 @@ typedef struct
|
||||
|
||||
#if FREERTOS_DJI == 1
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 大疆电机初始化
|
||||
* @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);
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 数据处理函数
|
||||
* @param[in] none
|
||||
* @retval none
|
||||
*/
|
||||
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
|
||||
}
|
||||
|
||||
@ -61,6 +61,25 @@ void Update_Encoder(Encoder_t *ptr) {
|
||||
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) {
|
||||
return ptr->round_cnt;
|
||||
}
|
||||
|
||||
@ -19,7 +19,7 @@
|
||||
#define SETTINGS2 0x0019
|
||||
|
||||
typedef struct {
|
||||
int32_t round_cnt;
|
||||
float round_cnt;
|
||||
uint16_t ecd;
|
||||
uint16_t last_ecd;
|
||||
uint16_t offset_ecd;
|
||||
|
||||
@ -3,203 +3,7 @@
|
||||
|
||||
extern UART_HandleTypeDef huart3;
|
||||
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双缓冲区+串口空闲中断
|
||||
|
||||
static osEventFlagsId_t eventReceive;
|
||||
@ -351,5 +155,26 @@ RC_data_t* get_rc_data()
|
||||
return &RC_mess.RC_data;
|
||||
}
|
||||
|
||||
/*
|
||||
|
||||
#endif
|
||||
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
|
||||
*/
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#include "calc_lib.h"
|
||||
|
||||
//微秒延时
|
||||
//??
|
||||
void user_delay_us(uint16_t us)
|
||||
{
|
||||
for(; us > 0; us--)
|
||||
@ -12,7 +12,7 @@ void user_delay_us(uint16_t us)
|
||||
}
|
||||
}
|
||||
|
||||
//毫秒延时
|
||||
//??
|
||||
void user_delay_ms(uint16_t ms)
|
||||
{
|
||||
for(; ms > 0; ms--)
|
||||
@ -21,7 +21,7 @@ void user_delay_ms(uint16_t ms)
|
||||
}
|
||||
}
|
||||
|
||||
//浮点数范围限制
|
||||
//??????????????????
|
||||
void abs_limit_fp(fp32 *num, fp32 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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
if (maxValue < minValue)
|
||||
@ -101,12 +101,22 @@ int32_t loop_int32_constrain(int32_t Input, int32_t minValue, int32_t maxValue)
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @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 <math.h>
|
||||
#include <stdbool.h>
|
||||
#include "struct_typedef.h"
|
||||
|
||||
#define M_DEG2RAD_MULT (0.01745329251f)
|
||||
#define M_RAD2DEG_MULT (57.2957795131f)
|
||||
@ -47,6 +48,12 @@ typedef struct {
|
||||
float wz; /* 转动 */
|
||||
} MoveVector_t;
|
||||
|
||||
//定义获取角度的量纲
|
||||
typedef enum {
|
||||
DEGREE,//0-360
|
||||
RADIAN,//(0-2pi)
|
||||
}Angle_e;
|
||||
|
||||
float InvSqrt(float x);
|
||||
|
||||
float AbsClip(float in, float limit);
|
||||
@ -101,6 +108,15 @@ void CircleReverse(float *origin);
|
||||
*/
|
||||
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
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -1,64 +1,82 @@
|
||||
#include "shoot.h"
|
||||
#include "remote_control.h"
|
||||
#include "read_spi.h"
|
||||
|
||||
extern RC_mess_t RC_mess;
|
||||
extern Encoder_t encoder;
|
||||
extern motor_measure_t *trigger_motor_data;//3508电机数据
|
||||
|
||||
int mode=0;
|
||||
int shoot=0;
|
||||
int xxx=0;
|
||||
|
||||
#define GO1_SHOOT 0
|
||||
#define ODRIVE_SHOOT 1
|
||||
#define Stop 0
|
||||
#define Hasten 1
|
||||
#define Slow 2
|
||||
#define Shoot 3
|
||||
#define VESC_ID 77
|
||||
|
||||
#define KP 0.12
|
||||
#define KD 0.008
|
||||
int speed_6384=-1500;
|
||||
|
||||
|
||||
#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;
|
||||
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);
|
||||
goshoot.offestAngle[i] = goshoot.goData[i]->Pos;
|
||||
HAL_Delay(100);
|
||||
// GO_M8010_send_data(&huart1, i,0,0,0,0,0,0);
|
||||
GO_M8010_send_data(&huart6, i,0,0,0,0,0,0);
|
||||
goshoot.offestAngle[i] = goshoot.goData[i]->Pos;
|
||||
HAL_Delay(100);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void shoot_ball(int key)
|
||||
void control_clutch_shoot(void)
|
||||
{
|
||||
|
||||
if(mode == Stop)
|
||||
{
|
||||
|
||||
//蓄力
|
||||
if(trigger_motor_data->real_angle ==60)//扳机已闭合
|
||||
if(encoder.round_cnt == 0)
|
||||
{
|
||||
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);
|
||||
|
||||
CAN_VESC_HEAD(VESC_ID);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
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);
|
||||
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -2,26 +2,11 @@
|
||||
#define _SHOOT_H_
|
||||
|
||||
#include "GO_M8010_6_Driver.h"
|
||||
#include "odrive_can.h"
|
||||
#include "djiMotor.h"
|
||||
#include "calc_lib.h"
|
||||
#include "pid.h"
|
||||
#include "main.h"
|
||||
|
||||
typedef struct
|
||||
{
|
||||
/* 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);
|
||||
void control_clutch_shoot(void);
|
||||
|
||||
#endif
|
||||
|
||||
@ -35,7 +35,7 @@ const osThreadAttr_t attr_motor = {
|
||||
.priority = osPriorityRealtime,
|
||||
.stack_size = 128 * 4,
|
||||
};
|
||||
//odrive_shoot
|
||||
//shoot
|
||||
const osThreadAttr_t attr_shoot = {
|
||||
.name = "shoot",
|
||||
.priority = osPriorityRealtime,
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
#include "TopDefine.h"
|
||||
#include "attrTask.h"
|
||||
#include "djiMotor.h"
|
||||
#include "Xiaomi.h"
|
||||
extern motor_measure_t *motor_3508_data;
|
||||
/**
|
||||
* \brief can任务
|
||||
@ -17,6 +18,7 @@ void Task_Can(void *argument)
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
|
||||
|
||||
djiInit();//FIFO滤波器初始化
|
||||
xia0miInit();//小米电机初始化
|
||||
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
|
||||
@ -25,7 +27,8 @@ void Task_Can(void *argument)
|
||||
{
|
||||
waitNewDji();//等待新数据
|
||||
djiMotorEncode();//数据解析
|
||||
|
||||
XM_MotorEncode();//数据解析
|
||||
vescMotorEncode();//数据解析
|
||||
//将can数据添加到消息队列
|
||||
|
||||
|
||||
|
||||
@ -13,13 +13,10 @@ extern RC_mess_t RC_mess;
|
||||
extern int16_t result;
|
||||
extern int16_t t_result;
|
||||
extern motor_measure_t *trigger_motor_data;
|
||||
Encoder_t encoder;
|
||||
float vofa[8];
|
||||
|
||||
int speed=0;
|
||||
float m=0;
|
||||
float trigger_angle_o=0;
|
||||
int mode=0;
|
||||
float vofa[8];
|
||||
int tangle=0;
|
||||
|
||||
/**
|
||||
* \brief 电机任务
|
||||
*
|
||||
@ -32,35 +29,13 @@ void Task_Motor(void *argument)
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_CHASSIS;
|
||||
|
||||
motor_init();
|
||||
Encoder_Init(&encoder);
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
while(1)
|
||||
{
|
||||
//收到消息队列新数据
|
||||
|
||||
// 更新编码器数据
|
||||
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);
|
||||
|
||||
}
|
||||
|
||||
trigger_pos(tangle);
|
||||
|
||||
CAN_cmd_200(t_result,0,0,0,&hcan1);
|
||||
osDelay(2);
|
||||
|
||||
@ -7,14 +7,19 @@
|
||||
#include "main.h"
|
||||
#include "remote_control.h"
|
||||
#include "led.h"
|
||||
#include "djiMotor.h"
|
||||
#include "Xiaomi.h"
|
||||
|
||||
#define GOUSE 1
|
||||
|
||||
extern RC_mess_t RC_mess;
|
||||
int anglexiaomi = 0;
|
||||
|
||||
void Task_Go(void *argument)
|
||||
{
|
||||
#if GOUSE==1
|
||||
|
||||
HAL_Delay(500); //一定时间延时 等待电机初始化完成
|
||||
init_cybergear(&mi_motor[0], 0x01, Motion_mode);//小米电机 启动!
|
||||
//HAL_Delay(2000);
|
||||
// gimbalInit();
|
||||
//HAL_GPIO_WritePin(ledGreen_GPIO_Port,ledGreen_Pin,GPIO_PIN_RESET);
|
||||
@ -23,6 +28,8 @@ void Task_Go(void *argument)
|
||||
{
|
||||
|
||||
LED_green();
|
||||
LED_bule();
|
||||
motor_controlmode(&mi_motor[0], 0.5, anglexiaomi, 4, 0.5 , 0.5); //小米暂时锁住就行
|
||||
// gimbalFlow();
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
@ -37,7 +37,7 @@ void Task_Init(void *argument) {
|
||||
// dji_motor
|
||||
task_runtime.thread.dji_motor =
|
||||
osThreadNew(Task_Motor, NULL, &attr_motor);
|
||||
// odrive_shoot
|
||||
// shoot
|
||||
task_runtime.thread.shoot =
|
||||
osThreadNew(Task_Shoot, NULL, &attr_shoot);
|
||||
// go
|
||||
|
||||
@ -7,53 +7,32 @@
|
||||
#include "shoot.h"
|
||||
#include "read_spi.h"
|
||||
#include "dji_task.h"
|
||||
#include "djiMotor.h"
|
||||
|
||||
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
|
||||
#define ODRIVE_SHOOT 0
|
||||
|
||||
//odrive发射
|
||||
#define SHOOT3 12
|
||||
#define TOP 6
|
||||
#define MIDDLE 3
|
||||
#define BOTTOM 0
|
||||
Encoder_t encoder;
|
||||
|
||||
void Task_Shoot(void *argument)
|
||||
{
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
|
||||
|
||||
#if GO1_SHOOT == 1
|
||||
shooterInit();
|
||||
#endif
|
||||
Encoder_Init(&encoder);
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
|
||||
|
||||
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)//扳机已闭合
|
||||
{
|
||||
control_clutch_shoot();
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user