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

Compare commits

base: RoboDuoduo:abaca03ecca403ecfee71163f8eed9f6f1c54714
Branches Tags
RoboDuoduo:main
RoboDuoduo:初版
..
compare: RoboDuoduo:f0931e76d2211b1bca1acd847f844b0fb6eb8d17
Branches Tags
RoboDuoduo:初版
RoboDuoduo:main

2 Commits
abaca03ecc ... f0931e76d2

Author SHA1 Message Date
ws
f0931e76d2 初试三摩擦 2025-04-16 16:24:23 +08:00
ws
9d073a0da2 初测自瞄 2025-04-13 14:54:07 +08:00
28 changed files with 358 additions and 253 deletions
Show Stats Expand all files Collapse all files

16
.mxproject
View File

File diff suppressed because one or more lines are too long

2
Core/Src/dma.c
View File

@ -57,7 +57,7 @@ void MX_DMA_Init(void)
HAL_NVIC_SetPriority(DMA2_Stream3_IRQn, 5, 0); HAL_NVIC_SetPriority(DMA2_Stream3_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream3_IRQn); HAL_NVIC_EnableIRQ(DMA2_Stream3_IRQn);
/* DMA2_Stream5_IRQn interrupt configuration */ /* DMA2_Stream5_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream5_IRQn, 0, 0); HAL_NVIC_SetPriority(DMA2_Stream5_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream5_IRQn); HAL_NVIC_EnableIRQ(DMA2_Stream5_IRQn);
} }

2
Core/Src/gpio.c
View File

@ -69,7 +69,7 @@ void MX_GPIO_Init(void)
/*Configure GPIO pin : PtPin */ /*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = ext_up_Pin; GPIO_InitStruct.Pin = ext_up_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(ext_up_GPIO_Port, &GPIO_InitStruct); HAL_GPIO_Init(ext_up_GPIO_Port, &GPIO_InitStruct);

16
Core/Src/stm32f4xx_it.c
View File

@ -279,16 +279,16 @@ void USART1_IRQHandler(void)
/** /**
* @brief This function handles USART3 global interrupt. * @brief This function handles USART3 global interrupt.
*/ */
// void USART3_IRQHandler(void) //void USART3_IRQHandler(void)
// { //{
// /* USER CODE BEGIN USART3_IRQn 0 */ // /* USER CODE BEGIN USART3_IRQn 0 */
// /* USER CODE END USART3_IRQn 0 */ // /* USER CODE END USART3_IRQn 0 */
// HAL_UART_IRQHandler(&huart3); // HAL_UART_IRQHandler(&huart3);
// /* USER CODE BEGIN USART3_IRQn 1 */ // /* 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. * @brief This function handles DMA2 stream1 global interrupt.

36
MDK-ARM/.vscode/c_cpp_properties.json vendored
View File

@ -3,26 +3,26 @@
{ {
"name": "R1-shooter", "name": "R1-shooter",
"includePath": [ "includePath": [
"d:\\Desktop\\r1\\Core\\Inc", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Core\\Inc",
"d:\\Desktop\\r1\\Drivers\\STM32F4xx_HAL_Driver\\Inc", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
"d:\\Desktop\\r1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
"d:\\Desktop\\r1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
"d:\\Desktop\\r1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
"d:\\Desktop\\r1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
"d:\\Desktop\\r1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
"d:\\Desktop\\r1\\Drivers\\CMSIS\\Include", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\CMSIS\\Include",
"d:\\Desktop\\r1\\User\\bsp", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\bsp",
"d:\\Desktop\\r1\\User\\module", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\module",
"d:\\Desktop\\r1\\User\\task", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\task",
"d:\\Desktop\\r1\\User\\lib", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\lib",
"d:\\Desktop\\r1\\User\\device", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\User\\device",
"D:\\keil\\ARM\\ARMCC\\include", "D:\\keil\\ARM\\ARMCC\\include",
"D:\\keil\\ARM\\ARMCC\\include\\rw", "D:\\keil\\ARM\\ARMCC\\include\\rw",
"d:\\Desktop\\r1\\MDK-ARM", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\MDK-ARM",
"d:\\Desktop\\r1\\Core\\Src", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Core\\Src",
"d:\\Desktop\\r1\\Drivers\\STM32F4xx_HAL_Driver\\Src", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Drivers\\STM32F4xx_HAL_Driver\\Src",
"d:\\Desktop\\r1\\Middlewares\\Third_Party\\FreeRTOS\\Source", "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source",
"d:\\Desktop\\r1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang" "d:\\Desktop\\r1\\r1_upper\\r1upper-1\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
], ],
"defines": [ "defines": [
"USE_HAL_DRIVER", "USE_HAL_DRIVER",

16
MDK-ARM/.vscode/keil-assistant.log vendored
View File

@ -6,3 +6,19 @@
[info] Log at : 2025/4/11|11:30:04|GMT+0800 [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
View File

@ -7,6 +7,8 @@
"detect.h": "c", "detect.h": "c",
"usertask.h": "c", "usertask.h": "c",
"task.h": "c", "task.h": "c",
"djimotor.h": "c" "djimotor.h": "c",
"nuc.h": "c",
"crc_ccitt.h": "c"
} }
} }

6
MDK-ARM/.vscode/uv4.log vendored
View File

@ -1,8 +1,4 @@
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin' *** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
Build target 'R1-shooter' 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). "R1-shooter\R1-shooter.axf" - 0 Error(s), 0 Warning(s).
Build Time Elapsed: 00:00:03 Build Time Elapsed: 00:00:01

2
MDK-ARM/.vscode/uv4.log.lock vendored
View File

@ -1 +1 @@
2025/4/11 17:42:35 2025/4/13 22:09:19

62
MDK-ARM/R1-shooter.uvoptx
View File

@ -153,27 +153,77 @@
<Ww> <Ww>
<count>0</count> <count>0</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>abc,0x0A</ItemText> <ItemText>rc_ctrl,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>1</count> <count>1</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>rc_ctrl,0x0A</ItemText> <ItemText>aaa,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>2</count> <count>2</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>aaa,0x0A</ItemText> <ItemText>key,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>3</count> <count>3</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>key,0x0A</ItemText> <ItemText>task_struct,0x0A</ItemText>
</Ww> </Ww>
<Ww> <Ww>
<count>4</count> <count>4</count>
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>task_struct,0x0A</ItemText> <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>
</Ww> </Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
@ -1010,7 +1060,7 @@
<Group> <Group>
<GroupName>User/task</GroupName> <GroupName>User/task</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>

5
R1-shooter.ioc
View File

@ -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_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_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_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream5_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true\:true NVIC.DMA2_Stream5_IRQn=true\:5\:0\:false\:false\:true\:false\:false\:true\:true
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false 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.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 NVIC.EXTI4_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
@ -291,8 +291,9 @@ PI6.GPIOParameters=GPIO_Label
PI6.GPIO_Label=up-ball PI6.GPIO_Label=up-ball
PI6.Locked=true PI6.Locked=true
PI6.Signal=GPIO_Input PI6.Signal=GPIO_Input
PI7.GPIOParameters=GPIO_Label PI7.GPIOParameters=GPIO_Label,GPIO_ModeDefaultEXTI
PI7.GPIO_Label=ext-up PI7.GPIO_Label=ext-up
PI7.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_RISING_FALLING
PI7.Locked=true PI7.Locked=true
PI7.Signal=GPXTI7 PI7.Signal=GPXTI7
PinOutPanel.CurrentBGAView=Top PinOutPanel.CurrentBGAView=Top

14
README.md
View File

@ -1,3 +1,17 @@
# r1upper # r1upper
r1上层 r1上层
## 外设
//PE9 刹车光电
//PE11 接球气缸
//PE13 爪子气缸
//PE14 下球气缸
//PC6 接球光电
//PI6 运球光电

2
User/bsp/uart_it.c
View File

@ -13,7 +13,7 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
return BSP_UART_DR16; return BSP_UART_DR16;
else if (huart->Instance == USART1) else if (huart->Instance == USART1)
return BSP_UART_REF; return BSP_UART_REF;
else if (huart->Instance == USART6) else if (huart->Instance == USART1)
return BSP_UART_AI; return BSP_UART_AI;
/* /*
else if (huart->Instance == USARTX) else if (huart->Instance == USARTX)

2
User/device/detect.c
View File

@ -7,7 +7,7 @@ int key=0;
void detect_exit(void) void detect_exit(void)
{ {
delay_ms(10); // 延时10ms delay_ms(100); // 延时10ms
key++; // 按键按下时变量自增 key++; // 按键按下时变量自增
__HAL_GPIO_EXTI_CLEAR_IT(KEY_Pin); // 清除中断标志位 __HAL_GPIO_EXTI_CLEAR_IT(KEY_Pin); // 清除中断标志位
} }

3
User/device/device.h
View File

@ -9,7 +9,8 @@ DEVICE_OK = 0,
DEVICE_ERR = -1, DEVICE_ERR = -1,
DEVICE_ERR_NULL = -2, DEVICE_ERR_NULL = -2,
DEVICE_ERR_INITED = -3, DEVICE_ERR_INITED = -3,
DEVICE_ERR_NO_DEV = -4 DEVICE_ERR_NO_DEV = -4,
DEVICE_ERR_OVERFLOW = -5,
}device_status_e; }device_status_e;
/* /*
SIGNAL是有bit位置区分的，一个SIGNAL只能包含一个高bit位。 SIGNAL是有bit位置区分的，一个SIGNAL只能包含一个高bit位。

22
User/device/djiMotor.c
View File

@ -78,9 +78,7 @@
#if DEBUG == 1 #if DEBUG == 1
//电机回传数据结构体 //电机回传数据结构体
motor_measure_t motor_chassis[5]; motor_measure_t motor_chassis[10];
motor_measure_t GM6020_motor[2];
motor_measure_t motor_2006[2];
CAN_MotorFeedback_t motor_5065[2]; CAN_MotorFeedback_t motor_5065[2];
#else #else
static motor_measure_t motor_chassis[5]; static motor_measure_t motor_chassis[5];
@ -299,24 +297,6 @@ void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeD
{ {
return &motor_chassis[i]; 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电机转速 * @brief 限制vesc电机转速

16
User/device/djiMotor.h
View File

@ -133,22 +133,6 @@ 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); 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 数据处理函数 * @brief 数据处理函数
* @param[in] none * @param[in] none

28
User/device/nuc.c
View File

@ -3,6 +3,7 @@
#include "crc_ccitt.h" #include "crc_ccitt.h"
uint8_t nucbuf[32]; uint8_t nucbuf[32];
uint8_t packet[32]; // 假设最大数据包长度为 32 字节
static void NUC_IdleCallback(void) { static void NUC_IdleCallback(void) {
// osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY); // osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
@ -39,6 +40,33 @@ bool_t NUC_WaitDmaCplt(void) {
return 1; 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) { int8_t NUC_RawParse(NUC_t *n) {
if (n == NULL) return DEVICE_ERR_NULL; if (n == NULL) return DEVICE_ERR_NULL;
union { union {

2
User/device/nuc.h
View File

@ -15,6 +15,7 @@ extern "C" {
#define ODOM (0x04) #define ODOM (0x04)
#define PICK (0x06) #define PICK (0x06)
#define VISION (0x09) #define VISION (0x09)
#define SEND (0X07)
// 写结构体存入视觉信息 // 写结构体存入视觉信息
typedef struct { typedef struct {
@ -38,6 +39,7 @@ int8_t NUC_Restart(void);
bool_t NUC_WaitDmaCplt(void); bool_t NUC_WaitDmaCplt(void);
int8_t NUC_RawParse(NUC_t *n); int8_t NUC_RawParse(NUC_t *n);
int8_t NUC_HandleOffline(NUC_t *cmd); int8_t NUC_HandleOffline(NUC_t *cmd);
int8_t NUC_SendPacket(void *data, uint16_t length);
#ifdef __cplusplus #ifdef __cplusplus
} }

170
User/module/ball.cpp
View File

@ -4,137 +4,87 @@
#include "detect.h" #include "detect.h"
extern RC_ctrl_t rc_ctrl; extern RC_ctrl_t rc_ctrl;
#define KP 0.8 extern int key;
#define KD 0.008
//PE9 刹车光电
//PE11 接球气缸
//PE13 爪子气缸
//PE14 下球气缸
//PC6 接球光电
//PI6 运球光电
int aaa=0;
// 定义状态机变量 // 定义状态机变量
BallState_t currentBallState = BALL_IDLE; BallState_t currentBallState = BALL_IDLE;
#define MOTOR_SPEED 1000
const fp32 Ball:: M3508_speed_PID[3] = {5, 0, 0};
//三摩擦轮运球！！！
Ball ::Ball() Ball ::Ball()
{ {
aaa=10; detect_init();
//ballState =HAL_GPIO_ReadPin(in_ball_GPIO_Port, in_ball_Pin); for(int i = 0;i < MOTOR_NUM;i ++)
detect_init(); {
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;
}
} }
void Ball ::ballCatch(void) void Ball ::Spin(float speed)
{ {
if(rc_ctrl.ch[1]>=300) 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 ++)
{ {
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//收网 speedSet[i] = speed;
} result[i] = PID_calc(&speed_pid[i],hand_Motor[i]->speed_rpm,speedSet[i]);
else if(rc_ctrl.ch[1]<300)
{
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//放网
} }
CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1);
osDelay(1);
} }
void Ball ::ballFlow(void) void Ball::ballHadling(void)
{ {
GO_M8010_send_data(&huart6, 1,0,0,angleSet[1],0.5,KP,KD); if (key > 0) // 检测按键是否被按下
}
void Ball ::ballZero(void)
{
GO_M8010_send_data(&huart6, 1,0,0,0,0,0,0);
}
void Ball ::ballHadling(void)
{
if(rc_ctrl.sw[0]==306)
{ {
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET);//爪子气缸 开爪 if (key % 2 == 1) // key 为奇数，摩擦轮正转
osDelay(200); {
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);//下球气缸 打出 Spin(5000); // 正转
osDelay(500); }
angleSet[1] = 2; else if (key % 2 == 0) // key 为偶数，摩擦轮反转
ballFlow(); {
} Spin(-5000); // 反转
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() { // void Ball::ballHadling(void)
switch (currentBallState) { // {
case BALL_IDLE: { // static bool isReversed = false; // 静态变量，记录当前摩擦轮状态，初始为正转
// 空闲状态，等待遥控器输入
if (rc_ctrl.sw[0] == 306) { // 假设遥控器开关控制开始运球
// HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 收网
// osDelay(200); // 等待收网完成
currentBallState = BALL_RELEASE; // 切换到下球气缸弹出状态
} // if (key > 0) // 检测按键是否被按下
break; // {
} // key = 0; // 重置按键状态，防止重复触发
case BALL_RELEASE: { // if (isReversed)
// 下球气缸弹出，爪子张开 // {
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 下球气缸弹出 // // 当前为反转，切换为正转
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET); // 爪子张开 // Spin(MOTOR_SPEED); // 正转
osDelay(200); // 等待动作完成 // isReversed = false;
currentBallState = BALL_JOINT_REVERSE; // 切换到关节电机反转状态 // }
break; // else
} // {
// // 当前为正转，切换为反转
case BALL_JOINT_REVERSE: { // Spin(-MOTOR_SPEED); // 反转
// 关节电机反转置水平位置 // isReversed = true;
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;
}
}
}

42
User/module/ball.hpp
View File

@ -4,7 +4,8 @@
#include "cmsis_os.h" #include "cmsis_os.h"
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "bsp_delay.h" #include "bsp_delay.h"
#include "GO_M8010_6_Driver.h" #include "djiMotor.h"
#include "pid.h"
// 定义状态枚举 // 定义状态枚举
typedef enum { typedef enum {
@ -16,6 +17,15 @@ typedef enum {
BALL_NET_CLOSE // 收网状态 BALL_NET_CLOSE // 收网状态
} BallState_t; } 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) #define IS_PHOTOELECTRIC_BALL() (HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin) == GPIO_PIN_RESET)
@ -23,23 +33,27 @@ class Ball
{ {
public: public:
Ball(); Ball();
void Spin(float speed);
void ballHadling(void); void ballHadling(void);
void ballCatch(void);
void ballFlow(void);
void ballStop(void);
void ballZero(void);
void ballStateMachine(void); void ballStateMachine(void);
int ballState; //球状态 0:无球 1:有球
int16_t result[MOTOR_NUM];
private: private:
//int ballState; //球状态 0:无球 1:有球
int up_ball_state; //上球状态 0:无球 1:有球 int up_ball_state; //上球状态 0:无球 1:有球
GO_Motorfield *goData[GO_NUM]; //电机数据指针 motor_measure_t *hand_Motor[MOTOR_NUM];
float angleSet[GO_NUM]; //电机目标角度 //三个摩擦轮
float offestAngle[GO_NUM]; //电机初始角度 static const float M3508_speed_PID[3];
float Kp; //比例系数 static const float M3508_angle_PID[3];
float Kd; //微分系数
float allowRange; //可活动角度 //电机速度pid结构体
pid_type_def speed_pid[MOTOR_NUM];
//位置环pid
pid_type_def angle_pid[MOTOR_NUM];
float angleSet;
float speedSet[MOTOR_NUM];
}; };
#endif #endif

54
User/module/gimbal.cpp
View File

@ -10,8 +10,58 @@
//可活动角度 //可活动角度
#define ANGLE_ALLOW 1.0f #define ANGLE_ALLOW 1.0f
extern RC_ctrl_t rc_ctrl; extern RC_ctrl_t rc_ctrl;
int angle1 = 0;
NUC_t nuc; 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() Gimbal::Gimbal()
{ {
@ -60,3 +110,5 @@ void Gimbal::gimbalVision(const NUC_t &nuc)
GO_M8010_send_data(&huart6, 0,0,0,angleSet[0],1,KP,KD); GO_M8010_send_data(&huart6, 0,0,0,angleSet[0],1,KP,KD);
osDelay(1); osDelay(1);
} }
#endif

29
User/module/gimbal.hpp
View File

@ -2,8 +2,11 @@
#define GIMBAL_HPP #define GIMBAL_HPP
#include "GO_M8010_6_Driver.h" #include "GO_M8010_6_Driver.h"
#include "djiMotor.h"
#include "pid.h" #include "pid.h"
#include "nuc.h" #include "nuc.h"
#define GM6020ING 1
class Gimbal class Gimbal
{ {
public: public:
@ -11,17 +14,31 @@ public:
void gimbalFlow(void);//云台随遥控器转动 void gimbalFlow(void);//云台随遥控器转动
void gimbalZero(void);//云台零阻尼模式 void gimbalZero(void);//云台零阻尼模式
void gimbalInit(void);//go初始化 void gimbalInit(void);//go初始化
//void gimbalVision(void);//云台视觉模式
void gimbalVision(const NUC_t &nuc); // 接收 NUC_t 数据 void gimbalVision(const NUC_t &nuc); // 接收 NUC_t 数据
int16_t result;
//暂存要发送的扭矩 //暂存要发送的扭矩
//float result[GO_NUM]; //float result[GO_NUM];
// float Kp; // float Kp;
// float Kd; // float Kd;
private: private:
// //电机速度pid结构体
// pid_type_def speed_pid[GO_NUM]; #if GM6020ING == 1
// //位置环pid //GM6020电机数据
// pid_type_def angle_pid[GO_NUM]; 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];
//视觉发送的要调的角度 //视觉发送的要调的角度
float self_angleSet; float self_angleSet;
GO_Motorfield* goData[GO_NUM]; GO_Motorfield* goData[GO_NUM];
@ -31,6 +48,8 @@ private:
float Kp; float Kp;
float Kd; float Kd;
float allowRange; float allowRange;
#endif
}; };

16
User/task/ballTask.cpp
View File

@ -9,16 +9,13 @@ extern RC_ctrl_t rc_ctrl;
Ball ball; Ball ball;
int abc=0; int abc=0;
int c30=0; int c30=0;
int speed1=500;
void FunctionBall(void *argument) void FunctionBall(void *argument)
{ {
(void)argument; /* 未使用argument，消除警告 */ (void)argument; /* 未使用argument，消除警告 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL; const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL;
ball.ballZero();
ball.ballState=abc;
uint32_t tick = osKernelGetTickCount(); uint32_t tick = osKernelGetTickCount();
while(1) while(1)
@ -26,14 +23,9 @@ void FunctionBall(void *argument)
#ifdef DEBUG #ifdef DEBUG
task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId()); task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId());
#endif #endif
abc++; abc=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
c30=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin); ball.ballHadling(); // 处理摩擦轮转动
// ball.Spin(speed1);
ball.ballHadling();
ball.ballStateMachine();
ball.ballCatch();
osDelay(3); osDelay(3);
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */

2
User/task/encodeCan.cpp
View File

@ -24,7 +24,7 @@ void FunctionCan(void *argument)
{ {
#ifdef DEBUG #ifdef DEBUG
task_struct.stack_water_mark.can = osThreadGetStackSpace(osThreadGetId()); task_struct.stack_water_mark.can = osThreadGetStackSpace(osThreadGetId());
task_struct.freq.can = osKernelGetTickFreq() / (osKernelGetTickCount() - tick);
#endif #endif
waitNewDji(); waitNewDji();
djiMotorEncode(); djiMotorEncode();

10
User/task/gimbalTask.cpp
View File

@ -10,6 +10,7 @@
Gimbal gimbal; Gimbal gimbal;
NUC_t nucData; // 用于存储从队列接收的数据 NUC_t nucData; // 用于存储从队列接收的数据
extern RC_ctrl_t rc_ctrl; extern RC_ctrl_t rc_ctrl;
int cnt1=0;
void FunctionGimbal(void *argument) void FunctionGimbal(void *argument)
{ {
@ -17,9 +18,6 @@ void FunctionGimbal(void *argument)
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_GIMBAL; 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); HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);
uint32_t tick = osKernelGetTickCount(); uint32_t tick = osKernelGetTickCount();
@ -29,6 +27,10 @@ void FunctionGimbal(void *argument)
#ifdef DEBUG #ifdef DEBUG
task_struct.stack_water_mark.gimbal = osThreadGetStackSpace(osThreadGetId()); task_struct.stack_water_mark.gimbal = osThreadGetStackSpace(osThreadGetId());
#endif #endif
cnt1++;
// gimbal.gimbalFlow();
// 从消息队列接收视觉数据 // 从消息队列接收视觉数据
if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK) if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
{ {
@ -36,8 +38,6 @@ void FunctionGimbal(void *argument)
gimbal.gimbalVision(nucData); gimbal.gimbalVision(nucData);
} }
gimbal.gimbalZero();
gimbal.gimbalFlow();
osDelay(1); osDelay(1);
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */

4
User/task/nucTask.cpp
View File

@ -8,6 +8,8 @@
#ifdef DEBUG #ifdef DEBUG
NUC_t cmd_fromnuc; NUC_t cmd_fromnuc;
//int send[3]={1,2,3};
float send[3]={1,2,3};
#endif #endif
@ -30,6 +32,8 @@ void Function_nuc(void *argument)
NUC_RawParse(&cmd_fromnuc); NUC_RawParse(&cmd_fromnuc);
NUC_SendPacket(&send, sizeof(send)); // 发送数据包
//掉线处理有空写
// if(NUC_WaitDmaCplt()) // if(NUC_WaitDmaCplt())
// { // {
// NUC_RawParse(&cmd_fromnuc); // NUC_RawParse(&cmd_fromnuc);

10
User/task/shootTask.cpp
View File

@ -25,15 +25,15 @@ while(1)
#ifdef DEBUG #ifdef DEBUG
task_struct.stack_water_mark.shoot = osThreadGetStackSpace(osThreadGetId()); task_struct.stack_water_mark.shoot = osThreadGetStackSpace(osThreadGetId());
#endif #endif
k++;
k++;
shoot.shootThree(); shoot.shootThree();
shoot.shootBack(); shoot.shootBack();
//shoot.shootStateMachine();
osDelay(2);
tick += delay_tick; /* 计算下一个唤醒时刻 */ osDelay(2);
osDelayUntil(tick);
tick += delay_tick; /* 计算下一个唤醒时刻 */
osDelayUntil(tick);
} }
} }