Compare commits
3 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 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
|
||||
@ -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回调函数,接收电机数据
|
||||
@ -261,8 +308,70 @@ void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeD
|
||||
* @param[in] i: 电机编号
|
||||
* @retval 电机数据指针
|
||||
*/
|
||||
motor_measure_t *get_motor_point(uint8_t i)
|
||||
{
|
||||
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
|
||||
|
||||
@ -109,6 +162,11 @@ 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);
|
||||
|
||||
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)
|
||||
void control_clutch_shoot(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);
|
||||
if(mode == Stop)
|
||||
{
|
||||
|
||||
if(encoder.round_cnt == 0)
|
||||
{
|
||||
|
||||
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);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
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_
|
||||
|
||||
#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,
|
||||
|
||||
@ -25,7 +25,7 @@ void Task_Can(void *argument)
|
||||
{
|
||||
waitNewDji();//等待新数据
|
||||
djiMotorEncode();//数据解析
|
||||
|
||||
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;
|
||||
|
||||
/**
|
||||
* \brief 电机任务
|
||||
*
|
||||
@ -32,34 +29,11 @@ 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);
|
||||
|
||||
}
|
||||
|
||||
|
||||
CAN_cmd_200(t_result,0,0,0,&hcan1);
|
||||
|
||||
@ -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,31 @@
|
||||
#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