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

没写完

Browse Source
This commit is contained in:
ws 2025-06-20 09:53:11 +08:00
parent c6ded22f93
commit ffd13d2e63
12 changed files with 269 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -40,3 +40,7 @@
[info] Log at : 2025/6/19|17:34:46|GMT+0800 [info] Log at : 2025/6/19|17:34:46|GMT+0800
[info] Log at : 2025/6/19|20:07:38|GMT+0800
[info] Log at : 2025/6/20|09:52:08|GMT+0800

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

@ -1,4 +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' Build target 'R1'
"R1\R1.axf" - 0 Error(s), 0 Warning(s). "R1\R1.axf" - 0 Error(s), 0 Warning(s).
Build Time Elapsed: 00:00:01 Build Time Elapsed: 00:00:07

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

@ -1 +1 @@
2025/6/18 17:18:51 2025/6/20 9:52:48

40
MDK-ARM/R1.uvoptx
View File

@ -1061,7 +1061,7 @@
<Group> <Group>
<GroupName>User/module</GroupName> <GroupName>User/module</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -1084,18 +1084,6 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\User\module\gimbal.cpp</PathWithFileName>
<FilenameWithoutPath>gimbal.cpp</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>64</FileNumber>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\module\shoot.cpp</PathWithFileName> <PathWithFileName>..\User\module\shoot.cpp</PathWithFileName>
<FilenameWithoutPath>shoot.cpp</FilenameWithoutPath> <FilenameWithoutPath>shoot.cpp</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -1105,13 +1093,13 @@
<Group> <Group>
<GroupName>User/task</GroupName> <GroupName>User/task</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>65</FileNumber> <FileNumber>64</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1123,7 +1111,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>66</FileNumber> <FileNumber>65</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1135,7 +1123,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>67</FileNumber> <FileNumber>66</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1147,7 +1135,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>68</FileNumber> <FileNumber>67</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1159,19 +1147,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>69</FileNumber> <FileNumber>68</FileNumber>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\task\gimbalTask.cpp</PathWithFileName>
<FilenameWithoutPath>gimbalTask.cpp</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>70</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1183,7 +1159,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>71</FileNumber> <FileNumber>69</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>

10
MDK-ARM/R1.uvprojx
View File

@ -733,11 +733,6 @@
<FileType>8</FileType> <FileType>8</FileType>
<FilePath>..\User\module\ball.cpp</FilePath> <FilePath>..\User\module\ball.cpp</FilePath>
</File> </File>
<File>
<FileName>gimbal.cpp</FileName>
<FileType>8</FileType>
<FilePath>..\User\module\gimbal.cpp</FilePath>
</File>
<File> <File>
<FileName>shoot.cpp</FileName> <FileName>shoot.cpp</FileName>
<FileType>8</FileType> <FileType>8</FileType>
@ -768,11 +763,6 @@
<FileType>8</FileType> <FileType>8</FileType>
<FilePath>..\User\task\encodeCan.cpp</FilePath> <FilePath>..\User\task\encodeCan.cpp</FilePath>
</File> </File>
<File>
<FileName>gimbalTask.cpp</FileName>
<FileType>8</FileType>
<FilePath>..\User\task\gimbalTask.cpp</FilePath>
</File>
<File> <File>
<FileName>nucTask.cpp</FileName> <FileName>nucTask.cpp</FileName>
<FileType>8</FileType> <FileType>8</FileType>

5
User/bsp/TopDefine.h
View File

@ -34,13 +34,16 @@
//================任务通知================// //================任务通知================//
//运球 //运球
#define BALL_OK (1<<1) #define BALL_DOWN (1<<1)
//接到放球命令 //接到放球命令
#define PUT_CMD (1<<2) #define PUT_CMD (1<<2)
//运球结束 //运球结束
#define PUT_OK (1<<2) #define PUT_OK (1<<2)
#define EXTEND_OK (1<<5)
//take任务要等待上面两个标志位 //take任务要等待上面两个标志位
#define TAKE_WAIT (0x0C) #define TAKE_WAIT (0x0C)
//要发送ok了 //要发送ok了

4
User/device/djiMotor.c
View File

@ -532,3 +532,7 @@ void CAN_XiaoMi(int id,JZ_xiaomi_t *JZ_xiaomi,CAN_HandleTypeDef*hcan)
HAL_CAN_AddTxMessage(hcan, &tx_message, can_send_data, &send_mail_box); HAL_CAN_AddTxMessage(hcan, &tx_message, can_send_data, &send_mail_box);
} }
MotorFeedback_t * get_CyberGear_point()
{
return &MotorFeedback;
}

50
User/device/djiMotor.h
View File

@ -98,6 +98,29 @@ typedef struct
} motor_measure_t; } motor_measure_t;
//小米
typedef struct
{
int16_t position;
int16_t speed;
int16_t K_P;
int16_t K_D;
int16_t K_C;
int16_t Pmax;//决定最大角度值，+-1 -> 最大+-360°
}JZ_xiaomi_t;
typedef struct {
uint8_t can_id;
float position_deg;
float speed_rads;
float current_A;
} MotorFeedback_t;
//motor calc ecd to angle //motor calc ecd to angle
#define MOTOR_ECD_TO_ANGLE_3508 (360.0 / 8191.0 / (3591.0/187.0)) #define MOTOR_ECD_TO_ANGLE_3508 (360.0 / 8191.0 / (3591.0/187.0))
@ -156,6 +179,8 @@ 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);
extern CAN_MotorFeedback_t *get_vesc_point(uint8_t i); extern CAN_MotorFeedback_t *get_vesc_point(uint8_t i);
extern MotorFeedback_t *get_CyberGear_point(void);
/** /**
* @brief 数据处理函数 * @brief 数据处理函数
* @param[in] none * @param[in] none
@ -172,31 +197,6 @@ void CAN_VESC_HEAD(uint8_t controller_id);
int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan); int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan);
//小米
typedef struct
{
int16_t position;
int16_t speed;
int16_t K_P;
int16_t K_D;
int16_t K_C;
int16_t Pmax;//决定最大角度值，+-1 -> 最大+-360°
}JZ_xiaomi_t;
typedef struct {
uint8_t can_id;
float position_deg;
float speed_rads;
float current_A;
} MotorFeedback_t;
void Parse_CAN_Response(uint8_t* rx_data, MotorFeedback_t* feedback) ; void Parse_CAN_Response(uint8_t* rx_data, MotorFeedback_t* feedback) ;

275
User/module/ball.cpp
View File

@ -10,16 +10,10 @@ extern RC_ctrl_t rc_ctrl;
extern int key; extern int key;
//伸缩 //伸缩
#define M_SPEED 4000 #define I_ANGLE 0
#define I_ANGLE1 180 #define O_ANGLE 140
#define I_ANGLE2 -73.5
#define O_ANGLE1 340
#define O_ANGLE2 -239
const fp32 Ball:: Extend_speed_PID[3] = { 50, 0.0, 0.1};
const fp32 Ball:: Extend_angle_PID[3]= { 50, 0.3, 0};
//PE11 气缸 //PE11 气缸
@ -27,57 +21,30 @@ Ball ::Ball()
{ {
detect_init(); detect_init();
//两个伸缩6020 左207 右208 //小米电机
Extern_Motor[0] = get_motor_point(6); feedback=get_CyberGear_point();
Extern_Motor[1] = get_motor_point(7);
Extern_Motor[0]->type = GM6020; //小米电机初始化
Extern_Motor[1]->type = GM6020; xiaomi.position = 0; //
PID_init(&extend_speed_pid[0],PID_POSITION,Extend_speed_PID,20000, 2000); xiaomi.speed = 20; //
PID_init(&extend_angle_pid[0],PID_POSITION,Extend_angle_PID,20000, 1000); xiaomi.K_P = 100; //
xiaomi.K_D =20; //
PID_init(&extend_speed_pid[1],PID_POSITION,Extend_speed_PID,15000, 5000); xiaomi.K_C = 2 ;
PID_init(&extend_angle_pid[1],PID_POSITION,Extend_angle_PID,15000, 5000); xiaomi.Pmax =1;
e_result[0] = 0;
e_result[1] = 0;
angleSet[0] = 0;
angleSet[1] = 0;
//状态机状态初始化
currentState1= BALL_IDLE;
//状态机状态初始化
currentState1= BALL_IDLE;
} }
void Ball ::Extend_mcontrol(int angle1,int angle2)
{
int16_t delta[2];
angleSet[0] = angle1/2;
angleSet[1] = angle2/2;
fp32 angle_get[2];
angle_get[0] = motor_ecd_to_angle_change(Extern_Motor[0]->ecd, angleSet[0]);
angle_get[1] = motor_ecd_to_angle_change(Extern_Motor[1]->ecd, angleSet[1]);
delta[0] = PID_calc(&extend_angle_pid[0], angle_get[0] , angleSet[0]);
e_result[0] = PID_calc(&extend_speed_pid[0], Extern_Motor[0]->speed_rpm, delta[0]);
delta[1] = PID_calc(&extend_angle_pid[1], angle_get[1] , angleSet[1]);
e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]);
}
// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]👇1800 // E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]👇1800
// G键 sw[6]👆 1800 中 1000 👇200 // G键 sw[6]👆 200 中 1000 👇1800
// sw[5] 👆 200 👇1800 // sw[5] 👆 200 👇1800
//左旋 sw[7] 200 --1800 //左旋 sw[7] 200 --1800
void Ball::rc_mode() void Ball::rc_mode()
{ {
if(rc_ctrl.sw[6]==1800) if(rc_ctrl.sw[6]==200)
{ {
rc_key=UP2; rc_key=UP2;
} }
@ -85,7 +52,7 @@ void Ball::rc_mode()
{ {
rc_key=MIDDLE2; rc_key=MIDDLE2;
} }
if(rc_ctrl.sw[6]==200) if(rc_ctrl.sw[6]==1800)
{ {
rc_key=DOWN2; rc_key=DOWN2;
} }
@ -101,21 +68,7 @@ void Ball::rc_mode()
} }
void Ball::Move_Extend(void) #if TESTBALL
{
if(extern_key==IN)
{
Extend_mcontrol(I_ANGLE1, I_ANGLE2); // 内伸
}
else if(extern_key==OUT)
{
Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸
}
else
{
Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸
}
}
void Ball::ballDown(void) void Ball::ballDown(void)
{ {
@ -125,6 +78,18 @@ void Ball::ballDown(void)
} }
void Ball::Move_Extend()
{
if(extern_key==IN)
{
xiaomi.position = 0;
}
if(extern_key==OUT)
{
xiaomi.position = 90;
}
}
void Ball::ball_control() void Ball::ball_control()
{ {
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 1，无球 0) ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 1，无球 0)
@ -163,7 +128,7 @@ void Ball::ball_control()
void Ball::Send_control() void Ball::Send_control()
{ {
CAN_cmd_1FF(0,0,e_result[0],e_result[1],&hcan1); CAN_XiaoMi(1,&xiaomi,&hcan2);
osDelay(1); osDelay(1);
} }
@ -245,5 +210,185 @@ void Ball::ballHadling(void)
currentState1 = BALL_IDLE; // 默认回到空闲状态 currentState1 = BALL_IDLE; // 默认回到空闲状态
break; break;
} }
} }
#endif
#if ONE_CONTROL
//任务通知来作全过程
void Ball::ballDown(void)
{
xiaomi.position = I_ANGLE;
if(feedback->position_deg== I_ANGLE) // 确保伸缩电机到位
{
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET); // 打开气缸爪子
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
}
if(ball_state==0)//检测到球
{
osThreadFlagsSet(task_struct.thread.ball, BALL_DOWN);
//osThreadFlagsSet(task_struct.thread.shoot, BALL_DOWN);
xiaomi.position = O_ANGLE;
}
if(hand_thread & BALL_DOWN)
{
if(feedback->position_deg== O_ANGLE)
{
osThreadFlagsSet(task_struct.thread.shoot, EXTEND_OK); //告诉发射球出去了
osThreadFlagsSet(task_struct.thread.ball, EXTEND_OK); //告诉发射球出去了
}
}
}
void Ball::Idle_control()
{
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
if(hand_thread & EXTEND_OK)
{
xiaomi.position = O_ANGLE; // 伸缩电机伸出去
}
else
{
xiaomi.position = I_ANGLE; // 伸缩电机回位,底盘可能跑的更好
}
if (currentState1 == BALL_FINISH)
{
currentState1 = BALL_IDLE;
}
else {
currentState1 = BALL_IDLE; // 默认回到空闲状态
}
}
void Ball::ball_control()
{
hand_thread = osThreadFlagsGet(); // 获取任务通知标志位
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 1，无球 0)
switch (rc_key){
case MIDDLE2:
Idle_control();
break;
case UP2:
ballDown();
break;
case DOWN2:
ballHadling();
break;
}
Send_control();
}
void Ball::Send_control()
{
CAN_XiaoMi(1,&xiaomi,&hcan2);
osDelay(1);
}
int ball_state = 0;
int triggerCount = 0; // 光电传感器触发计数
int last_ball_state = 1; // 上一次的光电状态
//(有球 0,无球 1)
void Ball::ballHadling(void)
{
switch (currentState1)
{
case BALL_IDLE:
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
if (key > 0 ||rc_key == DOWN2) // 检测按键是否被按下
{
key = 0; // 重置按键状态
triggerCount = 0; // 重置触发计数
currentState1 = BALL_FORWARD;
}
break;
case BALL_FORWARD:
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET); // 打开气缸爪子
osDelay(5);
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
currentState1 = BALL_DROP; // 切换到球下落状态
break;
case BALL_DROP:
osDelay(100); // 延时 100ms
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
if (ball_state == 0 && last_ball_state == 1) // 检测到状态从无球变为有球
{
//osDelay(10); // 延时去抖动
triggerCount++; // 增加触发计数
if (triggerCount == 1) // 第一次触发
{
currentState1 = BALL_FLAG; // 切换到等待第二次触发状态
}
}
last_ball_state = ball_state; // 更新上一次的状态
break;
case BALL_FLAG:
osDelay(10); // 延时 50ms
if (triggerCount == 1 && ball_state == 0 && last_ball_state == 1) // 第二次检测到球
{
triggerCount++; // 增加触发计数
currentState1 = BALL_CLOSE; // 切换到闭合气缸状态
}
last_ball_state = ball_state; // 更新上一次的状态
break;
case BALL_CLOSE:
if (triggerCount == 2) // 确保是第二次触发
{
osDelay(50);
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
currentState1 = BALL_FINISH; // 切换到反转状态
}
break;
case BALL_FINISH:
osDelay(50); // 延时 50ms
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
key = 0; // 重置按键状态
triggerCount = 0; // 重置触发计数
//currentState1 = BALL_IDLE; // 回到空闲状态
break;
default:
currentState1 = BALL_IDLE; // 默认回到空闲状态
break;
}
}
#endif

21
User/module/ball.hpp
View File

@ -11,6 +11,9 @@
#include "filter.h" #include "filter.h"
#include "calc_lib.h" #include "calc_lib.h"
#define TESTBALL 0
#define ONE_CONTROL 1
// 定义状态枚举 // 定义状态枚举
typedef enum { typedef enum {
BALL_IDLE, // 空闲状态 BALL_IDLE, // 空闲状态
@ -56,17 +59,19 @@ public:
void ballDown(void); void ballDown(void);
void Send_control(void); void Send_control(void);
void Move_Extend(void); void Move_Extend(void);
void Idle_control(void);
void rc_mode(void); void rc_mode(void);
void Extend_mcontrol(int angle1,int angle2); void Extend_mcontrol(int angle1,int angle2);
void ball_control(void); void ball_control(void);
BallState_t currentState1; // 当前状态 BallState_t currentState1; // 当前状态
int flag;//暂时还没用到 int flag_thread;//暂时还没用到
int ball_state ;//光电检测 int ball_state ;//光电检测
//伸缩6020
int16_t e_result[2]; //小米电机伸缩
motor_measure_t * Extern_Motor[2]; MotorFeedback_t *feedback;
JZ_xiaomi_t xiaomi;
//==========================公共变量==========================// //==========================公共变量==========================//
@ -74,17 +79,11 @@ public:
int16_t extern_key; int16_t extern_key;
//用于传接球,运球后通知 //用于传接球,运球后通知
volatile BallState_t ballStatus;//是否有球 volatile BallState_t ballStatus;//是否有球
volatile uint32_t flag_thread;//接收传回的线程通知 volatile uint32_t hand_thread;//接收传回的线程通知
private: private:
static const float Extend_speed_PID[3];
static const float Extend_angle_PID[3];
pid_type_def extend_speed_pid[2];
pid_type_def extend_angle_pid[2];
float angleSet[2];
}; };

1
User/module/shoot.cpp
View File

@ -316,6 +316,7 @@ void Shoot::RemoveError() {
{ {
//认为完全松开 //认为完全松开
control_pos=INIT_POS; control_pos=INIT_POS;
BSP_Buzzer_Stop();
} }

2
User/task/shootTask.cpp
View File

@ -30,7 +30,7 @@ while(1)
//我放的任务通知 运球成功放置过来后 //我放的任务通知 运球成功放置过来后
shoot.flag_thread=osThreadFlagsGet(); shoot.flag_thread=osThreadFlagsGet();
if(shoot.flag_thread & BALL_OK) if(shoot.flag_thread & BALL_DOWN)
{ {
} }