Compare commits
No commits in common. "f0931e76d2211b1bca1acd847f844b0fb6eb8d17" and "abaca03ecca403ecfee71163f8eed9f6f1c54714" have entirely different histories.
f0931e76d2
...
abaca03ecc
16
.mxproject
16
.mxproject
File diff suppressed because one or more lines are too long
@ -57,7 +57,7 @@ void MX_DMA_Init(void)
|
||||
HAL_NVIC_SetPriority(DMA2_Stream3_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(DMA2_Stream3_IRQn);
|
||||
/* DMA2_Stream5_IRQn interrupt configuration */
|
||||
HAL_NVIC_SetPriority(DMA2_Stream5_IRQn, 5, 0);
|
||||
HAL_NVIC_SetPriority(DMA2_Stream5_IRQn, 0, 0);
|
||||
HAL_NVIC_EnableIRQ(DMA2_Stream5_IRQn);
|
||||
|
||||
}
|
||||
|
||||
@ -69,7 +69,7 @@ void MX_GPIO_Init(void)
|
||||
|
||||
/*Configure GPIO pin : PtPin */
|
||||
GPIO_InitStruct.Pin = ext_up_Pin;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
HAL_GPIO_Init(ext_up_GPIO_Port, &GPIO_InitStruct);
|
||||
|
||||
|
||||
@ -279,16 +279,16 @@ void USART1_IRQHandler(void)
|
||||
/**
|
||||
* @brief This function handles USART3 global interrupt.
|
||||
*/
|
||||
//void USART3_IRQHandler(void)
|
||||
//{
|
||||
// /* USER CODE BEGIN USART3_IRQn 0 */
|
||||
// void USART3_IRQHandler(void)
|
||||
// {
|
||||
// /* USER CODE BEGIN USART3_IRQn 0 */
|
||||
|
||||
// /* USER CODE END USART3_IRQn 0 */
|
||||
// HAL_UART_IRQHandler(&huart3);
|
||||
// /* USER CODE BEGIN USART3_IRQn 1 */
|
||||
// /* USER CODE END USART3_IRQn 0 */
|
||||
// HAL_UART_IRQHandler(&huart3);
|
||||
// /* USER CODE BEGIN USART3_IRQn 1 */
|
||||
|
||||
// /* USER CODE END USART3_IRQn 1 */
|
||||
//}
|
||||
// /* USER CODE END USART3_IRQn 1 */
|
||||
// }
|
||||
|
||||
/**
|
||||
* @brief This function handles DMA2 stream1 global interrupt.
|
||||
|
||||
36
MDK-ARM/.vscode/c_cpp_properties.json
vendored
36
MDK-ARM/.vscode/c_cpp_properties.json
vendored
@ -3,26 +3,26 @@
|
||||
{
|
||||
"name": "R1-shooter",
|
||||
"includePath": [
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Core\\Inc",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Include",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\bsp",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\module",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\task",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\lib",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\device",
|
||||
"d:\\Desktop\\r1\\Core\\Inc",
|
||||
"d:\\Desktop\\r1\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
|
||||
"d:\\Desktop\\r1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
|
||||
"d:\\Desktop\\r1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
|
||||
"d:\\Desktop\\r1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
|
||||
"d:\\Desktop\\r1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
|
||||
"d:\\Desktop\\r1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
|
||||
"d:\\Desktop\\r1\\Drivers\\CMSIS\\Include",
|
||||
"d:\\Desktop\\r1\\User\\bsp",
|
||||
"d:\\Desktop\\r1\\User\\module",
|
||||
"d:\\Desktop\\r1\\User\\task",
|
||||
"d:\\Desktop\\r1\\User\\lib",
|
||||
"d:\\Desktop\\r1\\User\\device",
|
||||
"D:\\keil\\ARM\\ARMCC\\include",
|
||||
"D:\\keil\\ARM\\ARMCC\\include\\rw",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\MDK-ARM",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Core\\Src",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source",
|
||||
"d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
|
||||
"d:\\Desktop\\r1\\MDK-ARM",
|
||||
"d:\\Desktop\\r1\\Core\\Src",
|
||||
"d:\\Desktop\\r1\\Drivers\\STM32F4xx_HAL_Driver\\Src",
|
||||
"d:\\Desktop\\r1\\Middlewares\\Third_Party\\FreeRTOS\\Source",
|
||||
"d:\\Desktop\\r1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
|
||||
],
|
||||
"defines": [
|
||||
"USE_HAL_DRIVER",
|
||||
|
||||
16
MDK-ARM/.vscode/keil-assistant.log
vendored
16
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -6,19 +6,3 @@
|
||||
|
||||
[info] Log at : 2025/4/11|11:30:04|GMT+0800
|
||||
|
||||
[info] Log at : 2025/4/12|15:24:41|GMT+0800
|
||||
|
||||
[info] Log at : 2025/4/12|16:54:05|GMT+0800
|
||||
|
||||
[info] Log at : 2025/4/12|20:08:54|GMT+0800
|
||||
|
||||
[info] Log at : 2025/4/13|14:54:26|GMT+0800
|
||||
|
||||
[info] Log at : 2025/4/13|20:47:22|GMT+0800
|
||||
|
||||
[info] Log at : 2025/4/14|21:15:31|GMT+0800
|
||||
|
||||
[info] Log at : 2025/4/14|22:54:52|GMT+0800
|
||||
|
||||
[info] Log at : 2025/4/16|16:20:32|GMT+0800
|
||||
|
||||
|
||||
4
MDK-ARM/.vscode/settings.json
vendored
4
MDK-ARM/.vscode/settings.json
vendored
@ -7,8 +7,6 @@
|
||||
"detect.h": "c",
|
||||
"usertask.h": "c",
|
||||
"task.h": "c",
|
||||
"djimotor.h": "c",
|
||||
"nuc.h": "c",
|
||||
"crc_ccitt.h": "c"
|
||||
"djimotor.h": "c"
|
||||
}
|
||||
}
|
||||
6
MDK-ARM/.vscode/uv4.log
vendored
6
MDK-ARM/.vscode/uv4.log
vendored
@ -1,4 +1,8 @@
|
||||
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
|
||||
Build target 'R1-shooter'
|
||||
compiling encodeCan.cpp...
|
||||
linking...
|
||||
Program Size: Code=27336 RO-data=1776 RW-data=244 ZI-data=22764
|
||||
FromELF: creating hex file...
|
||||
"R1-shooter\R1-shooter.axf" - 0 Error(s), 0 Warning(s).
|
||||
Build Time Elapsed: 00:00:01
|
||||
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/4/13 22:09:19
|
||||
2025/4/11 17:42:35
|
||||
@ -153,77 +153,27 @@
|
||||
<Ww>
|
||||
<count>0</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rc_ctrl,0x0A</ItemText>
|
||||
<ItemText>abc,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>1</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>aaa,0x0A</ItemText>
|
||||
<ItemText>rc_ctrl,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>2</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>key,0x0A</ItemText>
|
||||
<ItemText>aaa,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>3</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>task_struct,0x0A</ItemText>
|
||||
<ItemText>key,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>4</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>gimbal,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>5</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>angle1,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>6</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>k,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>7</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>ball,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>8</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cnt1,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>9</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>cmd_fromnuc</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>10</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>nucbuf</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>11</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>nucData</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>12</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>send,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>13</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>speed1,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>14</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>abc,0x0A</ItemText>
|
||||
<ItemText>task_struct,0x0A</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<Tracepoint>
|
||||
@ -1060,7 +1010,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/task</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
|
||||
@ -154,7 +154,7 @@ NVIC.DMA1_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA2_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA2_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA2_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
|
||||
NVIC.DMA2_Stream5_IRQn=true\:5\:0\:false\:false\:true\:false\:false\:true\:true
|
||||
NVIC.DMA2_Stream5_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true\:true
|
||||
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||
NVIC.EXTI0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.EXTI4_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
@ -291,9 +291,8 @@ PI6.GPIOParameters=GPIO_Label
|
||||
PI6.GPIO_Label=up-ball
|
||||
PI6.Locked=true
|
||||
PI6.Signal=GPIO_Input
|
||||
PI7.GPIOParameters=GPIO_Label,GPIO_ModeDefaultEXTI
|
||||
PI7.GPIOParameters=GPIO_Label
|
||||
PI7.GPIO_Label=ext-up
|
||||
PI7.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_RISING_FALLING
|
||||
PI7.Locked=true
|
||||
PI7.Signal=GPXTI7
|
||||
PinOutPanel.CurrentBGAView=Top
|
||||
|
||||
16
README.md
16
README.md
@ -1,17 +1,3 @@
|
||||
# r1upper
|
||||
|
||||
r1上层
|
||||
|
||||
## 外设
|
||||
|
||||
//PE9 刹车光电
|
||||
|
||||
//PE11 接球气缸
|
||||
|
||||
//PE13 爪子气缸
|
||||
|
||||
//PE14 下球气缸
|
||||
|
||||
//PC6 接球光电
|
||||
|
||||
//PI6 运球光电
|
||||
r1上层
|
||||
@ -13,7 +13,7 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
|
||||
return BSP_UART_DR16;
|
||||
else if (huart->Instance == USART1)
|
||||
return BSP_UART_REF;
|
||||
else if (huart->Instance == USART1)
|
||||
else if (huart->Instance == USART6)
|
||||
return BSP_UART_AI;
|
||||
/*
|
||||
else if (huart->Instance == USARTX)
|
||||
|
||||
@ -7,7 +7,7 @@ int key=0;
|
||||
void detect_exit(void)
|
||||
{
|
||||
|
||||
delay_ms(100); // 延时10ms
|
||||
delay_ms(10); // 延时10ms
|
||||
key++; // 按键按下时变量自增
|
||||
__HAL_GPIO_EXTI_CLEAR_IT(KEY_Pin); // 清除中断标志位
|
||||
}
|
||||
|
||||
@ -9,8 +9,7 @@ DEVICE_OK = 0,
|
||||
DEVICE_ERR = -1,
|
||||
DEVICE_ERR_NULL = -2,
|
||||
DEVICE_ERR_INITED = -3,
|
||||
DEVICE_ERR_NO_DEV = -4,
|
||||
DEVICE_ERR_OVERFLOW = -5,
|
||||
DEVICE_ERR_NO_DEV = -4
|
||||
}device_status_e;
|
||||
/*
|
||||
SIGNAL是有bit位置区分的，一个SIGNAL只能包含一个高bit位。
|
||||
|
||||
@ -78,7 +78,9 @@
|
||||
#if DEBUG == 1
|
||||
|
||||
//电机回传数据结构体
|
||||
motor_measure_t motor_chassis[10];
|
||||
motor_measure_t motor_chassis[5];
|
||||
motor_measure_t GM6020_motor[2];
|
||||
motor_measure_t motor_2006[2];
|
||||
CAN_MotorFeedback_t motor_5065[2];
|
||||
#else
|
||||
static motor_measure_t motor_chassis[5];
|
||||
@ -297,7 +299,25 @@ void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeD
|
||||
{
|
||||
return &motor_chassis[i];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 返回GM6020电机数据指针
|
||||
* @param[in] i: 电机编号
|
||||
* @retval 电机数据指针
|
||||
*/
|
||||
motor_measure_t *get_6020_motor_point(uint8_t i)
|
||||
{
|
||||
return &GM6020_motor[i];
|
||||
}
|
||||
/**
|
||||
* @brief 返回M2006电机数据指针
|
||||
* @param[in] i: 电机编号
|
||||
* @retval 电机数据指针
|
||||
*/
|
||||
motor_measure_t *get_2006_motor_point(uint8_t i)
|
||||
{
|
||||
return &motor_2006[i];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 限制vesc电机转速
|
||||
* @param[in/out] rpm: vesce电机转速
|
||||
|
||||
@ -133,6 +133,22 @@ 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 返回GM6020电机数据指针
|
||||
* @param[in] i: 电机编号
|
||||
* @retval 电机数据指针
|
||||
*/
|
||||
motor_measure_t *get_6020_motor_point(uint8_t i);
|
||||
|
||||
/**
|
||||
* @brief 返回M2006电机数据指针
|
||||
* @param[in] i: 电机编号
|
||||
* @retval 电机数据指针
|
||||
*/
|
||||
motor_measure_t *get_2006_motor_point(uint8_t i);
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 数据处理函数
|
||||
* @param[in] none
|
||||
|
||||
@ -3,7 +3,6 @@
|
||||
#include "crc_ccitt.h"
|
||||
|
||||
uint8_t nucbuf[32];
|
||||
uint8_t packet[32]; // 假设最大数据包长度为 32 字节
|
||||
|
||||
static void NUC_IdleCallback(void) {
|
||||
// osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
|
||||
@ -40,33 +39,6 @@ bool_t NUC_WaitDmaCplt(void) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
int8_t NUC_SendPacket(void *data, uint16_t length) {
|
||||
if (data == NULL || length == 0) {
|
||||
return DEVICE_ERR_NULL; // 数据为空或长度为 0
|
||||
}
|
||||
|
||||
uint8_t packet[32]; // 假设最大数据包长度为 32 字节
|
||||
if (length + 5 > sizeof(packet)) { // 帧头 + 帧类型 + 数据 + CRC + 帧尾
|
||||
return DEVICE_ERR_OVERFLOW; // 数据包长度超出限制
|
||||
}
|
||||
|
||||
// 构建数据包
|
||||
packet[0] = HEAD; // 帧头
|
||||
packet[1] = SEND; // 发送帧类型
|
||||
memcpy(&packet[2], data, length); // 数据内容
|
||||
uint16_t crc = do_crc_table(data, length); // 计算 CRC 校验
|
||||
packet[length + 2] = (crc >> 8) & 0xFF; // CRC 高字节
|
||||
packet[length + 3] = crc & 0xFF; // CRC 低字节
|
||||
packet[length + 4] = TAIL; // 帧尾
|
||||
|
||||
// 发送数据包
|
||||
if (HAL_UART_Transmit(BSP_UART_GetHandle(BSP_UART_AI), packet, length + 5, HAL_MAX_DELAY) != HAL_OK) {
|
||||
return DEVICE_ERR; // 发送失败
|
||||
}
|
||||
|
||||
return DEVICE_OK; // 发送成功
|
||||
}
|
||||
|
||||
int8_t NUC_RawParse(NUC_t *n) {
|
||||
if (n == NULL) return DEVICE_ERR_NULL;
|
||||
union {
|
||||
|
||||
@ -15,7 +15,6 @@ extern "C" {
|
||||
#define ODOM (0x04)
|
||||
#define PICK (0x06)
|
||||
#define VISION (0x09)
|
||||
#define SEND (0X07)
|
||||
|
||||
// 写结构体存入视觉信息
|
||||
typedef struct {
|
||||
@ -39,7 +38,6 @@ int8_t NUC_Restart(void);
|
||||
bool_t NUC_WaitDmaCplt(void);
|
||||
int8_t NUC_RawParse(NUC_t *n);
|
||||
int8_t NUC_HandleOffline(NUC_t *cmd);
|
||||
int8_t NUC_SendPacket(void *data, uint16_t length);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
@ -4,87 +4,137 @@
|
||||
#include "detect.h"
|
||||
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
extern int key;
|
||||
#define KP 0.8
|
||||
#define KD 0.008
|
||||
//PE9 刹车光电
|
||||
//PE11 接球气缸
|
||||
//PE13 爪子气缸
|
||||
//PE14 下球气缸
|
||||
//PC6 接球光电
|
||||
//PI6 运球光电
|
||||
|
||||
int aaa=0;
|
||||
// 定义状态机变量
|
||||
BallState_t currentBallState = BALL_IDLE;
|
||||
|
||||
#define MOTOR_SPEED 1000
|
||||
|
||||
const fp32 Ball:: M3508_speed_PID[3] = {5, 0, 0};
|
||||
|
||||
//三摩擦轮运球！！！
|
||||
|
||||
Ball ::Ball()
|
||||
{
|
||||
detect_init();
|
||||
for(int i = 0;i < MOTOR_NUM;i ++)
|
||||
{
|
||||
hand_Motor[i] = get_motor_point(i);
|
||||
if(i <=3)
|
||||
{
|
||||
hand_Motor[i]->type = M3508;//设置电机类型
|
||||
PID_init(&speed_pid[i],PID_POSITION,M3508_speed_PID,16000, 6000);//pid初始化
|
||||
}
|
||||
result[i] = 0;
|
||||
speedSet[i] = 0;
|
||||
}
|
||||
|
||||
{
|
||||
aaa=10;
|
||||
//ballState =HAL_GPIO_ReadPin(in_ball_GPIO_Port, in_ball_Pin);
|
||||
detect_init();
|
||||
|
||||
}
|
||||
|
||||
void Ball ::Spin(float speed)
|
||||
void Ball ::ballCatch(void)
|
||||
{
|
||||
|
||||
speedSet[MOTOR_1] = -speed;
|
||||
result[MOTOR_1] = PID_calc(&speed_pid[MOTOR_1],hand_Motor[MOTOR_1]->speed_rpm,speedSet[MOTOR_1]);
|
||||
|
||||
for(int i = 1;i < MOTOR_NUM;i ++)
|
||||
if(rc_ctrl.ch[1]>=300)
|
||||
{
|
||||
speedSet[i] = speed;
|
||||
result[i] = PID_calc(&speed_pid[i],hand_Motor[i]->speed_rpm,speedSet[i]);
|
||||
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//收网
|
||||
}
|
||||
|
||||
CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1);
|
||||
osDelay(1);
|
||||
|
||||
}
|
||||
|
||||
void Ball::ballHadling(void)
|
||||
{
|
||||
if (key > 0) // 检测按键是否被按下
|
||||
else if(rc_ctrl.ch[1]<300)
|
||||
{
|
||||
if (key % 2 == 1) // key 为奇数，摩擦轮正转
|
||||
{
|
||||
Spin(5000); // 正转
|
||||
}
|
||||
else if (key % 2 == 0) // key 为偶数，摩擦轮反转
|
||||
{
|
||||
Spin(-5000); // 反转
|
||||
}
|
||||
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//放网
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// void Ball::ballHadling(void)
|
||||
// {
|
||||
// static bool isReversed = false; // 静态变量，记录当前摩擦轮状态，初始为正转
|
||||
void Ball ::ballFlow(void)
|
||||
{
|
||||
GO_M8010_send_data(&huart6, 1,0,0,angleSet[1],0.5,KP,KD);
|
||||
}
|
||||
|
||||
// if (key > 0) // 检测按键是否被按下
|
||||
// {
|
||||
// key = 0; // 重置按键状态，防止重复触发
|
||||
void Ball ::ballZero(void)
|
||||
{
|
||||
GO_M8010_send_data(&huart6, 1,0,0,0,0,0,0);
|
||||
}
|
||||
|
||||
// if (isReversed)
|
||||
// {
|
||||
// // 当前为反转，切换为正转
|
||||
// Spin(MOTOR_SPEED); // 正转
|
||||
// isReversed = false;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// // 当前为正转，切换为反转
|
||||
// Spin(-MOTOR_SPEED); // 反转
|
||||
// isReversed = true;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
void Ball ::ballHadling(void)
|
||||
{
|
||||
if(rc_ctrl.sw[0]==306)
|
||||
{
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET);//爪子气缸 开爪
|
||||
osDelay(200);
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);//下球气缸 打出
|
||||
osDelay(500);
|
||||
angleSet[1] = 2;
|
||||
ballFlow();
|
||||
}
|
||||
else
|
||||
{
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET);//爪子气缸 关爪
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET);//下球气缸 打入
|
||||
angleSet[1] = 0;
|
||||
ballFlow();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Ball::ballStateMachine() {
|
||||
switch (currentBallState) {
|
||||
case BALL_IDLE: {
|
||||
// 空闲状态，等待遥控器输入
|
||||
if (rc_ctrl.sw[0] == 306) { // 假设遥控器开关控制开始运球
|
||||
// HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 收网
|
||||
// osDelay(200); // 等待收网完成
|
||||
currentBallState = BALL_RELEASE; // 切换到下球气缸弹出状态
|
||||
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case BALL_RELEASE: {
|
||||
// 下球气缸弹出，爪子张开
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 下球气缸弹出
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET); // 爪子张开
|
||||
osDelay(200); // 等待动作完成
|
||||
currentBallState = BALL_JOINT_REVERSE; // 切换到关节电机反转状态
|
||||
break;
|
||||
}
|
||||
|
||||
case BALL_JOINT_REVERSE: {
|
||||
// 关节电机反转置水平位置
|
||||
angleSet[1] = 2; // 设置关节电机目标角度为水平位置
|
||||
ballFlow(); // 控制关节电机动作
|
||||
osDelay(500); // 等待电机动作完成
|
||||
currentBallState = BALL_FALLING; // 切换到球加速落下状态
|
||||
break;
|
||||
}
|
||||
|
||||
case BALL_FALLING: {
|
||||
// 球加速落下，等待光电检测
|
||||
if (IS_PHOTOELECTRIC_BALL()) {
|
||||
currentBallState = BALL_WAIT_BOUNCE; // 切换到等待反弹状态
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case BALL_WAIT_BOUNCE: {
|
||||
// 等待光电检测反弹
|
||||
osDelay(50); // 简单延时，避免误触发
|
||||
if (IS_PHOTOELECTRIC_BALL()) {
|
||||
currentBallState = BALL_NET_CLOSE; // 切换到收网状态
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case BALL_NET_CLOSE: {
|
||||
// 收网状态
|
||||
// HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 收网
|
||||
// osDelay(200); // 等待收网完成
|
||||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 关闭爪子
|
||||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 下球气缸复位
|
||||
angleSet[1] = 1; // 关节电机复位
|
||||
ballFlow();
|
||||
currentBallState = BALL_IDLE; // 切换回空闲状态
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
|
||||
currentBallState = BALL_IDLE; // 默认回到空闲状态
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -4,8 +4,7 @@
|
||||
#include "cmsis_os.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "bsp_delay.h"
|
||||
#include "djiMotor.h"
|
||||
#include "pid.h"
|
||||
#include "GO_M8010_6_Driver.h"
|
||||
|
||||
// 定义状态枚举
|
||||
typedef enum {
|
||||
@ -17,15 +16,6 @@ typedef enum {
|
||||
BALL_NET_CLOSE // 收网状态
|
||||
} BallState_t;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
MOTOR_1 = 0,
|
||||
MOTOR_2 = 1,
|
||||
MOTOR_3 = 2,
|
||||
MOTOR_DOWN = 3,
|
||||
MOTOR_NUM
|
||||
}motorhangd_e;
|
||||
|
||||
// 定义光电传感器检测宏
|
||||
#define IS_PHOTOELECTRIC_BALL() (HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin) == GPIO_PIN_RESET)
|
||||
|
||||
@ -33,27 +23,23 @@ class Ball
|
||||
{
|
||||
public:
|
||||
Ball();
|
||||
void Spin(float speed);
|
||||
|
||||
void ballHadling(void);
|
||||
void ballCatch(void);
|
||||
void ballFlow(void);
|
||||
void ballStop(void);
|
||||
void ballZero(void);
|
||||
void ballStateMachine(void);
|
||||
|
||||
int16_t result[MOTOR_NUM];
|
||||
|
||||
int ballState; //球状态 0:无球 1:有球
|
||||
private:
|
||||
|
||||
//int ballState; //球状态 0:无球 1:有球
|
||||
int up_ball_state; //上球状态 0:无球 1:有球
|
||||
motor_measure_t *hand_Motor[MOTOR_NUM];
|
||||
//三个摩擦轮
|
||||
static const float M3508_speed_PID[3];
|
||||
static const float M3508_angle_PID[3];
|
||||
|
||||
//电机速度pid结构体
|
||||
pid_type_def speed_pid[MOTOR_NUM];
|
||||
//位置环pid
|
||||
pid_type_def angle_pid[MOTOR_NUM];
|
||||
|
||||
float angleSet;
|
||||
float speedSet[MOTOR_NUM];
|
||||
GO_Motorfield *goData[GO_NUM]; //电机数据指针
|
||||
float angleSet[GO_NUM]; //电机目标角度
|
||||
float offestAngle[GO_NUM]; //电机初始角度
|
||||
float Kp; //比例系数
|
||||
float Kd; //微分系数
|
||||
float allowRange; //可活动角度
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -10,58 +10,8 @@
|
||||
//可活动角度
|
||||
#define ANGLE_ALLOW 1.0f
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
int angle1 = 0;
|
||||
|
||||
NUC_t nuc;
|
||||
|
||||
const fp32 Gimbal:: Gimbal_speed_PID[3] = {50, 0.1, 0};
|
||||
const fp32 Gimbal:: Gimbal_angle_PID[3]= { 5, 0.01, 0};
|
||||
|
||||
#if GM6020ING ==1
|
||||
Gimbal::Gimbal()
|
||||
{
|
||||
GM6020_Motor = get_motor_point(6);
|
||||
GM6020_Motor->type = GM6020;
|
||||
PID_init(&speed_pid,PID_POSITION,Gimbal_speed_PID,16000, 6000);
|
||||
PID_init(&angle_pid,PID_POSITION,Gimbal_angle_PID,5000, 2000);
|
||||
|
||||
result = 0;
|
||||
angleSet = 0;
|
||||
|
||||
}
|
||||
|
||||
void Gimbal::gimbalFlow()
|
||||
{
|
||||
int16_t delta[1];
|
||||
angleSet = angle1;
|
||||
delta[0] = PID_calc(&angle_pid,GM6020_Motor->total_angle,angleSet);
|
||||
result = PID_calc(&speed_pid, GM6020_Motor->speed_rpm, delta[0]);
|
||||
|
||||
CAN_cmd_1FF(0,0,result,0,&hcan1);
|
||||
osDelay(1);
|
||||
|
||||
}
|
||||
|
||||
void Gimbal::gimbalZero()
|
||||
{
|
||||
angleSet=0;
|
||||
//gimbalFlow();
|
||||
|
||||
}
|
||||
|
||||
void Gimbal::gimbalVision(const NUC_t &nuc)
|
||||
{
|
||||
int16_t delta[1];
|
||||
angleSet = nuc.vision.x;
|
||||
delta[0] = PID_calc(&angle_pid,GM6020_Motor->total_angle,angleSet);
|
||||
result = PID_calc(&speed_pid, GM6020_Motor->speed_rpm, delta[0]);
|
||||
|
||||
CAN_cmd_1FF(0,0,result,0,&hcan1);
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
#else
|
||||
Gimbal::Gimbal()
|
||||
{
|
||||
|
||||
@ -110,5 +60,3 @@ void Gimbal::gimbalVision(const NUC_t &nuc)
|
||||
GO_M8010_send_data(&huart6, 0,0,0,angleSet[0],1,KP,KD);
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@ -2,11 +2,8 @@
|
||||
#define GIMBAL_HPP
|
||||
|
||||
#include "GO_M8010_6_Driver.h"
|
||||
#include "djiMotor.h"
|
||||
#include "pid.h"
|
||||
#include "nuc.h"
|
||||
|
||||
#define GM6020ING 1
|
||||
class Gimbal
|
||||
{
|
||||
public:
|
||||
@ -14,31 +11,17 @@ public:
|
||||
void gimbalFlow(void);//云台随遥控器转动
|
||||
void gimbalZero(void);//云台零阻尼模式
|
||||
void gimbalInit(void);//go初始化
|
||||
//void gimbalVision(void);//云台视觉模式
|
||||
void gimbalVision(const NUC_t &nuc); // 接收 NUC_t 数据
|
||||
|
||||
int16_t result;
|
||||
//暂存要发送的扭矩
|
||||
//float result[GO_NUM];
|
||||
// float Kp;
|
||||
// float Kd;
|
||||
private:
|
||||
|
||||
#if GM6020ING == 1
|
||||
//GM6020电机数据
|
||||
motor_measure_t *GM6020_Motor;
|
||||
|
||||
static const float Gimbal_speed_PID[3];
|
||||
static const float Gimbal_angle_PID[3];
|
||||
|
||||
//电机速度pid结构体
|
||||
pid_type_def speed_pid;
|
||||
//位置环pid
|
||||
pid_type_def angle_pid;
|
||||
|
||||
float angleSet;
|
||||
|
||||
#else
|
||||
motor_measure_t *motorData[GO_NUM];
|
||||
// //电机速度pid结构体
|
||||
// pid_type_def speed_pid[GO_NUM];
|
||||
// //位置环pid
|
||||
// pid_type_def angle_pid[GO_NUM];
|
||||
//视觉发送的要调的角度
|
||||
float self_angleSet;
|
||||
GO_Motorfield* goData[GO_NUM];
|
||||
@ -48,8 +31,6 @@ private:
|
||||
float Kp;
|
||||
float Kd;
|
||||
float allowRange;
|
||||
|
||||
#endif
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -9,13 +9,16 @@ extern RC_ctrl_t rc_ctrl;
|
||||
Ball ball;
|
||||
int abc=0;
|
||||
int c30=0;
|
||||
int speed1=500;
|
||||
|
||||
void FunctionBall(void *argument)
|
||||
{
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL;
|
||||
|
||||
ball.ballZero();
|
||||
ball.ballState=abc;
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
|
||||
while(1)
|
||||
@ -23,9 +26,14 @@ void FunctionBall(void *argument)
|
||||
#ifdef DEBUG
|
||||
task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
abc=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
|
||||
ball.ballHadling(); // 处理摩擦轮转动
|
||||
// ball.Spin(speed1);
|
||||
abc++;
|
||||
c30=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
|
||||
|
||||
|
||||
ball.ballHadling();
|
||||
ball.ballStateMachine();
|
||||
ball.ballCatch();
|
||||
|
||||
osDelay(3);
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
|
||||
@ -24,7 +24,7 @@ void FunctionCan(void *argument)
|
||||
{
|
||||
#ifdef DEBUG
|
||||
task_struct.stack_water_mark.can = osThreadGetStackSpace(osThreadGetId());
|
||||
|
||||
task_struct.freq.can = osKernelGetTickFreq() / (osKernelGetTickCount() - tick);
|
||||
#endif
|
||||
waitNewDji();
|
||||
djiMotorEncode();
|
||||
|
||||
@ -10,7 +10,6 @@
|
||||
Gimbal gimbal;
|
||||
NUC_t nucData; // 用于存储从队列接收的数据
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
int cnt1=0;
|
||||
|
||||
void FunctionGimbal(void *argument)
|
||||
{
|
||||
@ -18,6 +17,9 @@ void FunctionGimbal(void *argument)
|
||||
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_GIMBAL;
|
||||
|
||||
HAL_Delay(500);
|
||||
gimbal.gimbalInit();
|
||||
gimbal.gimbalZero();
|
||||
HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);
|
||||
|
||||
uint32_t tick = osKernelGetTickCount();
|
||||
@ -26,18 +28,16 @@ void FunctionGimbal(void *argument)
|
||||
{
|
||||
#ifdef DEBUG
|
||||
task_struct.stack_water_mark.gimbal = osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
|
||||
cnt1++;
|
||||
|
||||
// gimbal.gimbalFlow();
|
||||
#endif
|
||||
// 从消息队列接收视觉数据
|
||||
if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
|
||||
{
|
||||
// 使用接收到的视觉数据调整云台
|
||||
gimbal.gimbalVision(nucData);
|
||||
}
|
||||
|
||||
|
||||
gimbal.gimbalZero();
|
||||
gimbal.gimbalFlow();
|
||||
osDelay(1);
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
|
||||
@ -8,8 +8,6 @@
|
||||
#ifdef DEBUG
|
||||
|
||||
NUC_t cmd_fromnuc;
|
||||
//int send[3]={1,2,3};
|
||||
float send[3]={1,2,3};
|
||||
|
||||
#endif
|
||||
|
||||
@ -32,8 +30,6 @@ void Function_nuc(void *argument)
|
||||
|
||||
NUC_RawParse(&cmd_fromnuc);
|
||||
|
||||
NUC_SendPacket(&send, sizeof(send)); // 发送数据包
|
||||
//掉线处理有空写
|
||||
// if(NUC_WaitDmaCplt())
|
||||
// {
|
||||
// NUC_RawParse(&cmd_fromnuc);
|
||||
|
||||
@ -24,16 +24,16 @@ while(1)
|
||||
{
|
||||
#ifdef DEBUG
|
||||
task_struct.stack_water_mark.shoot = osThreadGetStackSpace(osThreadGetId());
|
||||
#endif
|
||||
|
||||
#endif
|
||||
k++;
|
||||
|
||||
shoot.shootThree();
|
||||
shoot.shootBack();
|
||||
//shoot.shootStateMachine();
|
||||
osDelay(2);
|
||||
|
||||
osDelay(2);
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick);
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user