RoboDuoduo/shooter
Template
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加vesc

Browse Source
This commit is contained in:
ws 2025-03-26 14:54:23 +08:00
parent 8e67acbbe4
commit 1f404b793f
13 changed files with 228 additions and 327 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
User/device/djiMotor.c
View File

@ -25,6 +25,10 @@
#include "odrive_can.h"
static CAN_TxHeaderTypeDef vesc_tx_message;
static uint8_t vesc_send_data[4];
#endif
//motor data read
@ -76,9 +80,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
@ -129,6 +160,22 @@ 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回调函数,接收电机数据
@ -256,6 +303,14 @@ void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeD
HAL_CAN_AddTxMessage(hcan, &tx_meessage_2ff, can_send_data_2ff, &send_mail_box);
}
/**************************************
* 限幅函数
**************************************/
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 返回电机数据指针
* @param[in] i: 电机编号
@ -266,3 +321,29 @@ motor_measure_t *get_motor_point(uint8_t i)
return &motor_chassis[i];
}
/*
用转速模式控制电机
输入参数是controller_id（我用的是65，这个可以在上位机找到）和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);
}

58
User/device/djiMotor.h
View File

@ -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
@ -109,6 +162,9 @@ extern motor_measure_t *get_motor_point(uint8_t i);
* @retval none
*/
void djiMotorEncode(void);
void vescMotorEncode(void);
void CAN_VESC_RPM(uint8_t controller_id, float RPM);
#ifdef __cplusplus
}

219
User/device/remote_control.c
View File

@ -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
*/

24
User/lib/calc_lib.c
View File

@ -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;
}

23
User/lib/user_math.c
View File

@ -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;
}

16
User/lib/user_math.h
View File

@ -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

55
User/module/shoot.c
View File

@ -5,60 +5,5 @@ extern RC_mess_t RC_mess;
extern motor_measure_t *trigger_motor_data;//3508电机数据
#define GO1_SHOOT 0
#define ODRIVE_SHOOT 1
#define KP 0.12
#define KD 0.008
#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)
{
//蓄力
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

17
User/module/shoot.h
View File

@ -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"
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);
#endif

2
User/task/attrTask.c
View File

@ -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,

2
User/task/can_task.c
View File

@ -25,7 +25,7 @@ void Task_Can(void *argument)
{
waitNewDji();//等待新数据
djiMotorEncode();//数据解析
vescMotorEncode();//数据解析
//将can数据添加到消息队列

17
User/task/dji_task.c
View File

@ -43,23 +43,6 @@ void Task_Motor(void *argument)
// 更新编码器数据
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);

2
User/task/initTask.c
View File

@ -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

31
User/task/shoot_task.c
View File

@ -7,53 +7,30 @@
#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
int speed_6384=10000;
void Task_Shoot(void *argument)
{
(void)argument; /* 未使用argument，消除警告 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
#if GO1_SHOOT == 1
shooterInit();
#endif
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
while(1)
{
#if GO1_SHOOT == 1
shoot_ball(0);
CAN_VESC_RPM(77, speed_6384);
#elif ODRIVE_SHOOT == 1
if(mode == THREE)
{
}
else if(mode == DZ)
{
if(trigger_motor_data->total_angle ==0)//扳机已闭合
{
}
}
#endif
tick += delay_tick; /* 计算下一个唤醒时刻 */
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
}