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

Compare commits

...

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

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_EnableIRQ(DMA2_Stream3_IRQn);
/* 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);
}

2
Core/Src/gpio.c
View File

@ -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;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
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.
*/
// 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
View File

@ -3,26 +3,26 @@
{
"name": "R1-shooter",
"includePath": [
"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:\\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:\\keil\\ARM\\ARMCC\\include",
"D:\\keil\\ARM\\ARMCC\\include\\rw",
"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"
"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"
],
"defines": [
"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/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",
"usertask.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'
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: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>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>abc,0x0A</ItemText>
<ItemText>rc_ctrl,0x0A</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>rc_ctrl,0x0A</ItemText>
<ItemText>aaa,0x0A</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>aaa,0x0A</ItemText>
<ItemText>key,0x0A</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>key,0x0A</ItemText>
<ItemText>task_struct,0x0A</ItemText>
</Ww>
<Ww>
<count>4</count>
<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>
</WatchWindow1>
<Tracepoint>
@ -1010,7 +1060,7 @@
<Group>
<GroupName>User/task</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<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_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\: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.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,8 +291,9 @@ PI6.GPIOParameters=GPIO_Label
PI6.GPIO_Label=up-ball
PI6.Locked=true
PI6.Signal=GPIO_Input
PI7.GPIOParameters=GPIO_Label
PI7.GPIOParameters=GPIO_Label,GPIO_ModeDefaultEXTI
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
View File

@ -1,3 +1,17 @@
# 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;
else if (huart->Instance == USART1)
return BSP_UART_REF;
else if (huart->Instance == USART6)
else if (huart->Instance == USART1)
return BSP_UART_AI;
/*
else if (huart->Instance == USARTX)

2
User/device/detect.c
View File

@ -7,7 +7,7 @@ int key=0;
void detect_exit(void)
{
delay_ms(10); // 延时10ms
delay_ms(100); // 延时10ms
key++; // 按键按下时变量自增
__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_NULL = -2,
DEVICE_ERR_INITED = -3,
DEVICE_ERR_NO_DEV = -4
DEVICE_ERR_NO_DEV = -4,
DEVICE_ERR_OVERFLOW = -5,
}device_status_e;
/*
SIGNAL是有bit位置区分的，一个SIGNAL只能包含一个高bit位。

24
User/device/djiMotor.c
View File

@ -78,9 +78,7 @@
#if DEBUG == 1
//电机回传数据结构体
motor_measure_t motor_chassis[5];
motor_measure_t GM6020_motor[2];
motor_measure_t motor_2006[2];
motor_measure_t motor_chassis[10];
CAN_MotorFeedback_t motor_5065[2];
#else
static motor_measure_t motor_chassis[5];
@ -299,25 +297,7 @@ 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电机转速

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

28
User/device/nuc.c
View File

@ -3,6 +3,7 @@
#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);
@ -39,6 +40,33 @@ 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 {

2
User/device/nuc.h
View File

@ -15,6 +15,7 @@ extern "C" {
#define ODOM (0x04)
#define PICK (0x06)
#define VISION (0x09)
#define SEND (0X07)
// 写结构体存入视觉信息
typedef struct {
@ -38,6 +39,7 @@ 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
}

178
User/module/ball.cpp
View File

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

42
User/module/ball.hpp
View File

@ -4,7 +4,8 @@
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "bsp_delay.h"
#include "GO_M8010_6_Driver.h"
#include "djiMotor.h"
#include "pid.h"
// 定义状态枚举
typedef enum {
@ -16,6 +17,15 @@ 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)
@ -23,23 +33,27 @@ 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);
int ballState; //球状态 0:无球 1:有球
int16_t result[MOTOR_NUM];
private:
//int ballState; //球状态 0:无球 1:有球
int up_ball_state; //上球状态 0:无球 1:有球
GO_Motorfield *goData[GO_NUM]; //电机数据指针
float angleSet[GO_NUM]; //电机目标角度
float offestAngle[GO_NUM]; //电机初始角度
float Kp; //比例系数
float Kd; //微分系数
float allowRange; //可活动角度
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];
};
#endif

54
User/module/gimbal.cpp
View File

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

29
User/module/gimbal.hpp
View File

@ -2,8 +2,11 @@
#define GIMBAL_HPP
#include "GO_M8010_6_Driver.h"
#include "djiMotor.h"
#include "pid.h"
#include "nuc.h"
#define GM6020ING 1
class Gimbal
{
public:
@ -11,17 +14,31 @@ 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:
// //电机速度pid结构体
// pid_type_def speed_pid[GO_NUM];
// //位置环pid
// pid_type_def angle_pid[GO_NUM];
#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];
//视觉发送的要调的角度
float self_angleSet;
GO_Motorfield* goData[GO_NUM];
@ -31,6 +48,8 @@ private:
float Kp;
float Kd;
float allowRange;
#endif
};

16
User/task/ballTask.cpp
View File

@ -9,16 +9,13 @@ 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)
@ -26,14 +23,9 @@ void FunctionBall(void *argument)
#ifdef DEBUG
task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId());
#endif
abc++;
c30=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
ball.ballHadling();
ball.ballStateMachine();
ball.ballCatch();
abc=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
ball.ballHadling(); // 处理摩擦轮转动
// ball.Spin(speed1);
osDelay(3);
tick += delay_tick; /* 计算下一个唤醒时刻 */

2
User/task/encodeCan.cpp
View File

@ -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();

14
User/task/gimbalTask.cpp
View File

@ -10,6 +10,7 @@
Gimbal gimbal;
NUC_t nucData; // 用于存储从队列接收的数据
extern RC_ctrl_t rc_ctrl;
int cnt1=0;
void FunctionGimbal(void *argument)
{
@ -17,9 +18,6 @@ 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();
@ -28,16 +26,18 @@ void FunctionGimbal(void *argument)
{
#ifdef DEBUG
task_struct.stack_water_mark.gimbal = osThreadGetStackSpace(osThreadGetId());
#endif
#endif
cnt1++;
// gimbal.gimbalFlow();
// 从消息队列接收视觉数据
if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
{
// 使用接收到的视觉数据调整云台
gimbal.gimbalVision(nucData);
}
gimbal.gimbalZero();
gimbal.gimbalFlow();
osDelay(1);
tick += delay_tick; /* 计算下一个唤醒时刻 */

4
User/task/nucTask.cpp
View File

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

12
User/task/shootTask.cpp
View File

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