Skip to content

Commit 8ed2567

Browse files
Alex328050Alex328050
committed
2025.3.23 v3.1.0
- 编写了大疆电机、角编码器、光电传感器的初始化检测函数 - 整理了之前编写的角编码器对应的头文件与源文件 - 增加了初始化检测线程，还未测试 - 增加连续拍球功能、连续抛球功能 - 增加状态机状态遥控器监看功能
1 parent a5a6dfc commit 8ed2567

29 files changed

+389
-441
lines changed

‎.eide/eide.json‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,8 @@
88
"Middlewares",
99
"usercode",
1010
"UC",
11-
"Unitree_SDK"
11+
"Unitree_SDK",
12+
"usercode/task"
1213
],
1314
"virtualFolder": {
1415
"name": "<virtual_root>",

‎Core/Src/freertos.c‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -81,7 +81,16 @@ void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
8181
*/
8282
void MX_FREERTOS_Init(void) {
8383
/* USER CODE BEGIN Init */
84-
ENCODER_CANFilterInit(&hcan2);
84+
if (CANFilterInit(&hcan1) == HAL_ERROR)
85+
{
86+
throwhsm.wholestate = WHOLE_ERROR;
87+
throwhsm.errorstate = ERROR_CANINITFAIL1;
88+
}
89+
if (ENCODER_CANFilterInit(&hcan2) == HAL_ERROR)
90+
{
91+
throwhsm.wholestate = WHOLE_ERROR;
92+
throwhsm.errorstate = ERROR_CANINITFAIL2;
93+
}
8594
HAL_CAN_Start(&hcan2); // 启动CAN外设
8695
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING); // 启用接收FIFO0的中断
8796
/* USER CODE END Init */

‎Core/Src/main.c‎

Lines changed: 0 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -104,13 +104,6 @@ int main(void)
104104
MX_USART6_UART_Init();
105105
/* USER CODE BEGIN 2 */
106106
RetargetInit(&huart8);
107-
CANFilterInit(&hcan1);
108-
hDJI[0].motorType = M2006;//自旋电机
109-
hDJI[1].motorType = M3508;
110-
hDJI[2].motorType = M2006;//支撑右电机
111-
hDJI[3].motorType = M2006;
112-
DJI_Init();
113-
114107
throwhsm.wholestate = WHOLE_INITING;
115108
/* USER CODE END 2 */
116109

‎README.md‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,12 @@
11
## 版本历史
22

3+
### [3.1.0] - 2025-3-23
4+
- 编写了大疆电机、角编码器、光电传感器的初始化检测函数
5+
- 整理了之前编写的角编码器对应的头文件与源文件
6+
- 增加了初始化检测线程，还未测试
7+
- 增加连续拍球功能、连续抛球功能
8+
- 增加状态机状态遥控器监看功能
9+
310
### [3.0.1] - 2025-3-18
411
- 调试拍球状态
512
- 修改拍球线程，准备线程

‎UC/encoder.c‎

Lines changed: 10 additions & 109 deletions
Original file line numberDiff line numberDiff line change
@@ -17,9 +17,6 @@ HAL_StatusTypeDef ENCODER_CANFilterInit(CAN_HandleTypeDef* hcan) {
1717
sFilterConfig.FilterIdHigh = (0x50 << 5); // 左移 5 位
1818
sFilterConfig.FilterIdLow = 0x0000;
1919

20-
// 设置掩码为 0xFFFF，表示匹配所有位
21-
// sFilterConfig.FilterMaskIdHigh = 0xFFFF;
22-
// sFilterConfig.FilterMaskIdLow = 0xFFFF;
2320
sFilterConfig.FilterMaskIdHigh = 0x0000;
2421
sFilterConfig.FilterMaskIdLow = 0x0000;
2522

@@ -28,97 +25,19 @@ HAL_StatusTypeDef ENCODER_CANFilterInit(CAN_HandleTypeDef* hcan) {
2825
sFilterConfig.SlaveStartFilterBank = 28;
2926

3027
if (HAL_CAN_ConfigFilter(hcan, &sFilterConfig) != HAL_OK) {
31-
Error_Handler();
28+
return HAL_ERROR;
3229
}
3330

3431
if (HAL_CAN_Start(hcan) != HAL_OK) {
35-
Error_Handler();
32+
return HAL_ERROR;
3633
}
3734

3835
if (HAL_CAN_ActivateNotification(hcan, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) {
39-
Error_Handler();
36+
return HAL_ERROR;
4037
}
4138

4239
return HAL_OK;
4340
}
44-
// void CheckCANError() {
45-
// uint32_t errorCode = HAL_CAN_GetError(&hcan2);
46-
// // 检查是否有错误
47-
// if (errorCode != HAL_CAN_ERROR_NONE) {
48-
// // 打印错误状态
49-
// printf("CAN Error Code: %lu\n", errorCode);
50-
// // 检查特定的错误类型
51-
// if (errorCode & HAL_CAN_ERROR_EWG) {
52-
// printf("Error Warning Flag set\n");
53-
// }
54-
// if (errorCode & HAL_CAN_ERROR_EPV) {
55-
// printf("Error Passive Flag set\n");
56-
// }
57-
// if (errorCode & HAL_CAN_ERROR_BOF) {
58-
// printf("Bus Off Flag set\n");
59-
// }
60-
// if (errorCode & HAL_CAN_ERROR_ACK) {
61-
// printf("ACK Error Flag set\n");
62-
// }
63-
// if (errorCode & HAL_CAN_ERROR_STF) {
64-
// printf("Stuff Error Flag set\n");
65-
// }
66-
// if (errorCode & HAL_CAN_ERROR_FOR) {
67-
// printf("Form Error Flag set\n");
68-
// }
69-
// if (errorCode & HAL_CAN_ERROR_BR) {
70-
// printf("Bit Recessive Error Flag set\n");
71-
// }
72-
// // 去掉 HAL_CAN_ERROR_BS 和 HAL_CAN_ERROR_ID
73-
// if (errorCode & HAL_CAN_ERROR_CRC) {
74-
// printf("CRC Error Flag set\n");
75-
// }
76-
// } else {
77-
// printf("No CAN errors detected\n");
78-
// }
79-
// }
80-
81-
// void ReceiveCANData(CAN_HandleTypeDef* hcan, EncoderData* data) {
82-
// CAN_RxHeaderTypeDef RxHeader;
83-
// uint8_t RxData[8];
84-
// CheckCANError();
85-
// // 检查接收FIFO是否有消息
86-
// if (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) > 0) {
87-
// // 接收消息
88-
// if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData) == HAL_OK) {
89-
// printf("Received message: ");
90-
// for (int i = 0; i < RxHeader.DLC; i++) {
91-
// printf("%02X ", RxData[i]);
92-
// }
93-
// printf("\n");
94-
// // 解析数据
95-
// if (RxData[0] == 0x55 && RxData[1] == 0x55) {
96-
// uint16_t angleReg = (RxData[2] << 8) | RxData[3];
97-
// data->angle = angleReg * 360.0f / 32768.0f;
98-
// uint16_t angularSpeedReg = (RxData[4] << 8) | RxData[5];
99-
// data->angularSpeed = angularSpeedReg * 360.0f / 32768.0f / 0.1f;
100-
// data->revolutions = (RxData[6] << 8) | RxData[7];
101-
// } else if (RxData[0] == 0x55 && RxData[1] == 0x56) {
102-
// uint16_t tempReg = (RxData[2] << 8) | RxData[3];
103-
// data->temperature = tempReg / 100.0f;
104-
// }
105-
// } else {
106-
// printf("Failed to get message from FIFO\n");
107-
// }
108-
// } else {
109-
// printf("No message in FIFO\n");
110-
// }
111-
// if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData) == HAL_OK) {
112-
// // 处理接收到的数据
113-
// printf("Received message ID: %X, Data: ", RxHeader.StdId);
114-
// for (int i = 0; i < RxHeader.DLC; i++) {
115-
// printf("%02X ", RxData[i]);
116-
// }
117-
// printf("\n");
118-
// } else {
119-
// printf("Failed to get message from FIFO\n");
120-
// }
121-
// }
12241

12342
void ParseCANData(CAN_RxHeaderTypeDef *RxHeader, uint8_t *RxData, EncoderData *data,EncoderCalculateData *caldata) {
12443
// 解析数据
@@ -153,30 +72,6 @@ void ParseCANData(CAN_RxHeaderTypeDef *RxHeader, uint8_t *RxData, EncoderData *d
15372
}
15473
}
15574

156-
// void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
157-
// CAN_RxHeaderTypeDef RxHeader;
158-
// uint8_t RxData[8];
159-
// EncoderData data;
160-
// CheckCANError();
161-
// 检查接收FIFO是否有消息
162-
// if (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) > 0) {
163-
// // 接收消息
164-
// if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData) == HAL_OK) {
165-
// printf("Received message ID: %X, Data: ", RxHeader.StdId);
166-
// for (int i = 0; i < RxHeader.DLC; i++) {
167-
// printf("%02X ", RxData[i]);
168-
// }
169-
// printf("\n");
170-
// // 解析数据
171-
// ParseCANData(&RxHeader, RxData, &data);
172-
// } else {
173-
// printf("Failed to get message from FIFO\n");
174-
// }
175-
// } else {
176-
// printf("No message in FIFO\n");
177-
// }
178-
// }
179-
18075
// 发送配置命令的函数
18176
HAL_StatusTypeDef SendConfigCommand(CAN_HandleTypeDef* hcan, uint8_t* command, uint8_t length) {
18277
CAN_TxHeaderTypeDef TxHeader;
@@ -191,7 +86,7 @@ HAL_StatusTypeDef SendConfigCommand(CAN_HandleTypeDef* hcan, uint8_t* command, u
19186

19287
//该角编码器是掉电保存配置，只需要配置一次即可
19388
//白线置零也会置零波特率
194-
void Encoder_Init(CAN_HandleTypeDef* hcan) {
89+
void Encoder_Setup(CAN_HandleTypeDef* hcan) {
19590
HAL_StatusTypeDef status;
19691

19792
// Step 1: 解锁
@@ -254,4 +149,10 @@ void Encoder_Init(CAN_HandleTypeDef* hcan) {
254149
HAL_Delay(10);
255150

256151
DEBUG_PRINT("Encoder Initialization Complete\n");
152+
}
153+
154+
HAL_StatusTypeDef Encoder_Init(void)
155+
{
156+
if (caldata.angle > 19000 || caldata.angle < 17000) return HAL_ERROR;
157+
else return HAL_OK;
257158
}

‎UC/encoder.h‎

Lines changed: 14 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,18 @@
11
#ifndef ENCODER_H
22
#define ENCODER_H
33

4+
/**
5+
* @file encoder.h
6+
* @author Man
7+
* @brief 维特智能JY-ME02-CAN角编码使用代码
8+
* @date 2025-3-22
9+
*/
10+
/**
11+
* 请使用CAN转USB模块连接角编码器，并用上位机进行包括但不限于波特率、初始角度、回传速率等参数的配置
12+
* 产品说明书：https://wit-motion.yuque.com/wumwnr/docs/qcgu36
13+
* 计算数据结构体EncoderCalculateData中的数据均是实际数值的100倍
14+
* 记得在所有任务执行之前写上ENCODER_CANFilterInit(&hcan2);HAL_CAN_Start(&hcan2);HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING);
15+
*/
416
#include "stm32f4xx_hal.h"
517
#include "can.h"
618
#include <stdio.h>
@@ -24,12 +36,10 @@ extern EncoderCalculateData caldata;
2436

2537
// 函数声明
2638
HAL_StatusTypeDef ENCODER_CANFilterInit(CAN_HandleTypeDef* hcan);
27-
//void ReceiveCANData(CAN_HandleTypeDef* hcan, EncoderData* data);
2839
HAL_StatusTypeDef SendConfigCommand(CAN_HandleTypeDef* hcan, uint8_t* command, uint8_t length);
29-
void Encoder_Init(CAN_HandleTypeDef* hcan);
30-
//void CheckCANError();
40+
void Encoder_Setup(CAN_HandleTypeDef* hcan);
3141
void ParseCANData(CAN_RxHeaderTypeDef *RxHeader, uint8_t *RxData, EncoderData *data,EncoderCalculateData *caldata);
32-
//void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan);
42+
HAL_StatusTypeDef Encoder_Init(void);
3343

3444
extern CAN_HandleTypeDef hcan2;
3545
#endif // ENCODER_H

‎usercode/hold/ThrowHSM.h‎

Lines changed: 25 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
* @Author: Alex
44
* @Date: 2025-03-06 22:15:48
55
* @LastEditors: Alex
6-
* @LastEditTime: 2025-03-16 00:17:30
6+
* @LastEditTime: 2025-03-23 23:24:27
77
*/
88

99
#ifndef THROWHSM_H
@@ -23,7 +23,9 @@ typedef enum
2323
WHOLE_INITING, //整车初始化状态
2424
WHOLE_READY, //整车就绪状态
2525
WHOLE_BOUNCE, //整车拍球状态
26+
WHOLE_CONBOUNCE, //整车连续拍球状态
2627
WHOLE_THROW, //整车抛球状态
28+
WHOLE_CONTHROW, //整车连续抛球状态
2729
WHOLE_ERROR, //整车错误状态
2830
}WHOLESTATE;
2931

@@ -39,6 +41,8 @@ typedef enum
3941
THROW_ACCELERATE, //抛球子状态 宇树电机发力加速 夹爪未打开 球未抛出
4042
THROW_THROWOUT, //抛球子状态 夹爪打开 球抛出
4143
THROW_BACK, //抛球子状态 夹爪收回 机构回到悬垂状态
44+
45+
THROW_CATCHING, //连续抛球特有状态 夹球
4246
}THROWSTATE;
4347

4448
/**
@@ -48,14 +52,32 @@ typedef enum
4852
typedef enum
4953
{
5054
BOUNCE_IDLE, //拍球子状态 拍球机构未使用
51-
BOUNCE_WAITBALL, //拍球子状态 夹爪打开等待放球
5255
BOUNCE_CATCHING, //拍球子状态 夹球
5356
BOUNCE_GETTOPOSITION, //拍球子状态 宇树电机向后发力紧靠 机构到达拍球位置
5457
BOUNCE_READY, //拍球子状态 支撑打开 宇树卸力 拍球气缸推出 准备拍球
5558
BOUNCE_BOUNCE, //拍球子状态 拍球
5659
BOUNCE_CATCHANDADJUSTPOSTURE, //拍球子状态 抓球 自旋调整篮球姿态 拍球气缸收回
60+
61+
BOUNCE_WAITBALL, //连续拍球特有状态 夹爪打开等待放球
5762
}BOUNCESTATE;
5863

64+
/**
65+
* @brief 错误态子状态机
66+
*
67+
*/
68+
typedef enum
69+
{
70+
ERROR_DJIINITFAIL, //错误子状态 大疆电机初始化失败
71+
ERROR_UNITREEINITFAIL0, //错误子状态 0号宇树电机初始化失败
72+
ERROR_UNITREEINITFAIL1, //错误子状态 1号宇树电机初始化失败
73+
ERROR_ECODERINITFAIL, //错误子状态 角编码器初始化失败
74+
ERROR_CANINITFAIL1, //错误子状态 can1初始化失败
75+
ERROR_CANINITFAIL2, //错误子状态 can2初始化失败
76+
ERROR_SENSORINITFAIL, //错误子状态 拍球传感器初始化失败
77+
78+
ERROR_DJITRANSMITFAIL, //错误子状态 大疆电机信息传输失败
79+
}ERRORSTATE;
80+
5981
/**
6082
* @brief 整车全状态结构体
6183
*
@@ -65,6 +87,7 @@ typedef struct
6587
WHOLESTATE wholestate;
6688
THROWSTATE throwstate;
6789
BOUNCESTATE bouncestate;
90+
ERRORSTATE errorstate;
6891
}THROWHSM;
6992

7093
extern THROWHSM throwhsm;

‎usercode/hold/pawl.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,8 @@ extern "C" {
1313
*/
1414

1515
#include "DJI.h"
16+
#include "Caculate.h"
17+
#include "cmsis_os.h" //为了不报warning而加
1618

1719
#define CLOSE_STALL_CURRENT -4000.0f //闭合堵转电流标志//需要修改
1820
#define CLOSE_SPEED -7200.0f //关爪子速度rpm

‎usercode/lib/dji_motor/DJI.c‎

Lines changed: 33 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@ static uint32_t TxMailbox;
1111
* @brief DJI电机初始化函数
1212
* @note 使用前应对全局变量hDJI进行类型初始化
1313
*/
14-
void DJI_Init()
14+
HAL_StatusTypeDef DJI_Init()
1515
{
1616
for (int i = 0; i < 8; i++)
1717
{
@@ -39,6 +39,34 @@ void DJI_Init()
3939

4040
hDJI[i].encoder_resolution = 8192.0f;
4141
}
42+
/*夹爪电机参数初始化 */
43+
hDJI[0].posPID.outputMax=4000;
44+
hDJI[0].posPID.KP=60;
45+
hDJI[0].posPID.KI=3;
46+
hDJI[0].posPID.KD=0.0;
47+
hDJI[0].speedPID.outputMax=4000;
48+
hDJI[0].speedPID.KI=0.4;
49+
hDJI[0].speedPID.KD=0.4;
50+
hDJI[0].speedPID.KP=12;
51+
52+
/*自旋电机参数初始化*/
53+
hDJI[1].posPID.outputMax=4000;
54+
hDJI[1].posPID.KP=60;
55+
hDJI[1].posPID.KI=3;
56+
hDJI[1].posPID.KD=0.0;
57+
hDJI[1].speedPID.outputMax=4000;
58+
hDJI[1].speedPID.KI=0.4;
59+
hDJI[1].speedPID.KD=0.4;
60+
hDJI[1].speedPID.KP=12;
61+
62+
for (int i = 0; i < 4; i++)
63+
{
64+
if (hDJI[i].FdbData.rpm < -50 || hDJI[i].FdbData.rpm > 50 || hDJI[i].FdbData.current >9000 || hDJI[i].FdbData.current < -9000) // 允许小范围浮动
65+
{
66+
return HAL_ERROR;
67+
}
68+
}
69+
return HAL_OK;
4270
}
4371

4472
/**
@@ -70,7 +98,8 @@ void CanTransmit_DJI_1234(CAN_HandleTypeDef *hcanx, int16_t cm1_iq, int16_t cm2_
7098
}
7199
if (HAL_CAN_AddTxMessage(hcanx, &TxMessage, TxData, &TxMailbox) != HAL_OK)
72100
{
73-
Error_Handler(); // 如果CAN信息发送失败则进入死循环
101+
throwhsm.wholestate = WHOLE_ERROR;
102+
throwhsm.errorstate = ERROR_DJITRANSMITFAIL;
74103
}
75104
}
76105

@@ -102,7 +131,8 @@ void CanTransmit_DJI_5678(CAN_HandleTypeDef *hcanx, int16_t cm5_iq, int16_t cm6_
102131
;
103132
if (HAL_CAN_AddTxMessage(hcanx, &TxMessage, TxData, &TxMailbox) != HAL_OK)
104133
{
105-
Error_Handler(); // 如果CAN信息发送失败则进入死循环
134+
throwhsm.wholestate = WHOLE_ERROR;
135+
throwhsm.errorstate = ERROR_DJITRANSMITFAIL;
106136
}
107137
}
108138

0 commit comments

Comments
 (0)