添加vesc
This commit is contained in:
parent
8e67acbbe4
commit
1f404b793f
@ -25,6 +25,10 @@
|
|||||||
|
|
||||||
#include "odrive_can.h"
|
#include "odrive_can.h"
|
||||||
|
|
||||||
|
|
||||||
|
static CAN_TxHeaderTypeDef vesc_tx_message;
|
||||||
|
static uint8_t vesc_send_data[4];
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//motor data read
|
//motor data read
|
||||||
@ -75,10 +79,37 @@
|
|||||||
// (ptr)->speed_rpm = (float)((data)[4] << 4 | (data)[5] <<3 |(data)[6] < 2 |(data)[7]);; \
|
// (ptr)->speed_rpm = (float)((data)[4] << 4 | (data)[5] <<3 |(data)[6] < 2 |(data)[7]);; \
|
||||||
// }
|
// }
|
||||||
/*(ptr)->real_angle = (ptr)->real_angle % 360; */
|
/*(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
|
#if DEBUG == 1
|
||||||
motor_measure_t motor_chassis[5];
|
motor_measure_t motor_chassis[5];
|
||||||
|
CAN_MotorFeedback_t motor_6384;
|
||||||
|
|
||||||
#else
|
#else
|
||||||
static motor_measure_t motor_chassis[5];
|
static motor_measure_t motor_chassis[5];
|
||||||
#endif
|
#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
|
#if FREERTOS_DJI == 0
|
||||||
/**
|
/**
|
||||||
* @brief hal库CAN回调函数,接收电机数据
|
* @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);
|
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 返回电机数据指针
|
* @brief 返回电机数据指针
|
||||||
* @param[in] i: 电机编号
|
* @param[in] i: 电机编号
|
||||||
@ -266,3 +321,29 @@ motor_measure_t *get_motor_point(uint8_t i)
|
|||||||
return &motor_chassis[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);
|
||||||
|
}
|
||||||
|
|
||||||
|
|||||||
@ -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
|
||||||
|
|
||||||
@ -109,6 +162,9 @@ extern motor_measure_t *get_motor_point(uint8_t i);
|
|||||||
* @retval none
|
* @retval none
|
||||||
*/
|
*/
|
||||||
void djiMotorEncode(void);
|
void djiMotorEncode(void);
|
||||||
|
void vescMotorEncode(void);
|
||||||
|
|
||||||
|
void CAN_VESC_RPM(uint8_t controller_id, float RPM);
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -5,60 +5,5 @@ extern RC_mess_t RC_mess;
|
|||||||
extern motor_measure_t *trigger_motor_data;//3508电机数据
|
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
|
|
||||||
|
|
||||||
|
|||||||
@ -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"
|
||||||
|
|
||||||
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
|
#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,
|
||||||
|
|||||||
@ -25,7 +25,7 @@ void Task_Can(void *argument)
|
|||||||
{
|
{
|
||||||
waitNewDji();//等待新数据
|
waitNewDji();//等待新数据
|
||||||
djiMotorEncode();//数据解析
|
djiMotorEncode();//数据解析
|
||||||
|
vescMotorEncode();//数据解析
|
||||||
//将can数据添加到消息队列
|
//将can数据添加到消息队列
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -43,25 +43,8 @@ void Task_Motor(void *argument)
|
|||||||
// 更新编码器数据
|
// 更新编码器数据
|
||||||
Update_Encoder(&encoder);
|
Update_Encoder(&encoder);
|
||||||
m=trigger_angle_o*(8191/360);
|
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);
|
||||||
|
|
||||||
|
|||||||
@ -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,30 @@
|
|||||||
#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 Encoder_t encoder;
|
||||||
extern motor_measure_t *trigger_motor_data;//3508电机数据
|
extern motor_measure_t *trigger_motor_data;//3508电机数据
|
||||||
extern int mode;
|
extern int mode;
|
||||||
|
|
||||||
#define GO1_SHOOT 0
|
int speed_6384=10000;
|
||||||
#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
|
|
||||||
shooterInit();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
|
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
|
||||||
|
|
||||||
while(1)
|
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; /* 计算下一个唤醒时刻 */
|
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||||
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
|
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user