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-15 12:55:28 +08:00
parent e747803b0a
commit 6489615e36
15 changed files with 187 additions and 170 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Core/Src/freertos.c
View File

@ -58,7 +58,7 @@ const osThreadAttr_t defaultTask_attributes = {
osThreadId_t initTaskHandle;
const osThreadAttr_t initTask_attributes = {
.name = "initTask",
.stack_size = 128 * 4,
.stack_size = 256 * 4,
.priority = (osPriority_t) osPriorityLow,
};

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

@ -30,3 +30,5 @@
[info] Log at : 2025/6/13|19:56:03|GMT+0800
[info] Log at : 2025/6/15|11:17:22|GMT+0800

4
MDK-ARM/.vscode/settings.json vendored
View File

@ -2,6 +2,8 @@
"files.associations": {
"djimotor.h": "c",
"user_math.h": "c",
"buzzer.h": "c"
"buzzer.h": "c",
"calc_lib.h": "c",
"usertask.h": "c"
}
}

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

@ -2,7 +2,7 @@
Build target 'R1'
compiling calc_lib.c...
linking...
Program Size: Code=31512 RO-data=1884 RW-data=5820 ZI-data=33276
Program Size: Code=30644 RO-data=1884 RW-data=5816 ZI-data=33280
FromELF: creating hex file...
"R1\R1.axf" - 0 Error(s), 0 Warning(s).
Build Time Elapsed: 00:00:03

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

@ -1 +1 @@
2025/6/14 19:24:55
2025/6/14 22:01:44

24
MDK-ARM/R1.uvoptx
View File

@ -153,7 +153,24 @@
<Name>-U-O142 -O2254 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry>
</TargetDriverDllRegistry>
<Breakpoint/>
<Breakpoint>
<Bp>
<Number>0</Number>
<Type>0</Type>
<LineNumber>82</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>0</Address>
<ByteObject>0</ByteObject>
<HtxType>0</HtxType>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
<BreakByAccess>0</BreakByAccess>
<BreakIfRCount>0</BreakIfRCount>
<Filename>../Core/Src/main.c</Filename>
<ExecCommand></ExecCommand>
<Expression></Expression>
</Bp>
</Breakpoint>
<WatchWindow1>
<Ww>
<count>0</count>
@ -190,6 +207,11 @@
<WinNumber>1</WinNumber>
<ItemText>knob_increment</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>task_struct,0x0A</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>

2
R1.ioc
View File

@ -109,7 +109,7 @@ FREERTOS.INCLUDE_xSemaphoreGetMutexHolder=1
FREERTOS.INCLUDE_xTaskAbortDelay=1
FREERTOS.INCLUDE_xTaskGetHandle=1
FREERTOS.IPParameters=Tasks01,INCLUDE_pcTaskGetTaskName,INCLUDE_xSemaphoreGetMutexHolder,INCLUDE_vTaskCleanUpResources,INCLUDE_xEventGroupSetBitFromISR,INCLUDE_uxTaskGetStackHighWaterMark2,INCLUDE_xTaskGetHandle,INCLUDE_xTaskAbortDelay,configCHECK_FOR_STACK_OVERFLOW,configTOTAL_HEAP_SIZE,FootprintOK
FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL;initTask,8,128,initFunction,As weak,NULL,Dynamic,NULL,NULL
FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL;initTask,8,256,initFunction,As weak,NULL,Dynamic,NULL,NULL
FREERTOS.configCHECK_FOR_STACK_OVERFLOW=1
FREERTOS.configTOTAL_HEAP_SIZE=0x6000
File.Version=6

34
User/lib/calc_lib.c
View File

@ -1,7 +1,7 @@
#include "calc_lib.h"
#include <math.h>
//微秒延时
//΢ÃëÑÓʱ
void user_delay_us(uint16_t us)
{
for(; us > 0; us--)
@ -13,7 +13,7 @@ void user_delay_us(uint16_t us)
}
}
//毫秒延时
//ºÁÃëÑÓʱ
void user_delay_ms(uint16_t ms)
{
for(; ms > 0; ms--)
@ -22,7 +22,7 @@ void user_delay_ms(uint16_t ms)
}
}
//浮点数范围限制
//¸¡µãÊý·¶Î§ÏÞÖÆ
void abs_limit_fp(fp32 *num, fp32 Limit)
{
if (*num > Limit)
@ -35,7 +35,7 @@ void abs_limit_fp(fp32 *num, fp32 Limit)
}
}
//整数范围限制
//ÕûÊý·¶Î§ÏÞÖÆ
void abs_limit_int(int64_t *num, int64_t Limit)
{
if (*num > Limit)
@ -48,7 +48,7 @@ void abs_limit_int(int64_t *num, int64_t Limit)
}
}
//循环限幅
//Ñ­»·ÏÞ·ù
fp32 loop_fp32_constrain(fp32 Input, fp32 minValue, fp32 maxValue)
{
if (maxValue < minValue)
@ -102,12 +102,12 @@ int32_t loop_int32_constrain(int32_t Input, int32_t minValue, int32_t maxValue)
return Input;
}
int map(int x, int in_min, int in_max, int out_min, int out_max) //映射函数
int map(int x, int in_min, int in_max, int out_min, int out_max) //Ó³É亯Êý
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
fp32 map_fp32(fp32 x, fp32 in_min, fp32 in_max, fp32 out_min, fp32 out_max) //映射函数
fp32 map_fp32(fp32 x, fp32 in_min, fp32 in_max, fp32 out_min, fp32 out_max) //Ó³É亯Êý
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
@ -117,3 +117,23 @@ float expo_map(float value, float expo_factor) {
return value * (1 - expo_factor) + value * fabs(value) * expo_factor;
}
bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) {
static int count = 0; // 满足条件的计数
if (count >= threshold) {
count=0;//重置
return true; // 如果已经满足条件，返回 1
}
// 判断三个值是否都满足条件
bool all_reached = (fabs(current1 - target1) < mistake) &&
(fabs(current2 - target2) < mistake) &&
(fabs(current3 - target3) < mistake);
if (all_reached) {
count++; // 所有条件都满足，计数加 1
}
return false; // 未满足条件达到阈值，返回 0
}

4
User/lib/calc_lib.h
View File

@ -7,6 +7,7 @@ extern "C"{
#include "struct_typedef.h"
#include<stdint.h>
#include <stdbool.h>
#define rad(code) ((code)*MOTOR_ECD_TO_RAD)
@ -30,6 +31,9 @@ int32_t loop_int32_constrain(int32_t Input, int32_t minValue, int32_t maxValue);
int map(int x, int in_min, int in_max, int out_min, int out_max);
fp32 map_fp32(fp32 x, fp32 in_min, fp32 in_max, fp32 out_min, fp32 out_max);
float expo_map(float value, float expo_factor);
bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold);
#ifdef __cplusplus
}
#endif

205
User/module/ball.cpp
View File

@ -6,8 +6,6 @@
#include "user_math.h"
#include "shoot.hpp"
//int jy=1;
extern RC_ctrl_t rc_ctrl;
extern int key;
extern int16_t M2006_result;
@ -15,18 +13,23 @@ extern int16_t M2006_result;
// C键 sw[5]👆 1800 中1000 👇200
// D键 sw[6]👆 200 👇1800
//伸缩
#define M_SPEED 4000
#define I_ANGLE1 180
#define I_ANGLE2 -75
#define O_ANGLE1 340
#define O_ANGLE2 -240
// 三摩擦轮电机
#define M_SPEED1 3000
#define M_BACK -4000
#if HANDLING3 == 1
//三摩擦轮运球！！！
//添加平移3508 得跑位置吧
const fp32 Ball:: M3508_speed_PID[3] = {15, 0.03, 0};
const fp32 Ball:: Extend_speed_PID[3] = { 25, 0.0, 0.1};
const fp32 Ball:: M3508_speed_PID[3] = {30, 0.03, 0};
const fp32 Ball:: Extend_speed_PID[3] = { 40, 0.0, 0.1};
const fp32 Ball:: Extend_angle_PID[3]= { 25, 0.1, 0};
//摩擦轮转速
@ -74,43 +77,6 @@ Ball ::Ball()
}
// void Ball ::Extend_control(int angle)
// {
// int16_t delta;
// angleSet = angle/2;
// fp32 angle_get;
// angle_get=motor_angle_change(Extern_Motor->real_round, angleSet);
// delta = PID_calc(&extend_angle_pid, angle_get, angleSet);
// e_result = PID_calc(&extend_speed_pid, Extern_Motor->speed_rpm, delta);
// CAN_cmd_1FF(0,0,e_result,0,&hcan1);
// osDelay(1);
// }
//左0 右1
//real_round 外 176 240 内 89 326
// 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_angle_change(Extern_Motor[0]->real_round, angleSet[0]);
// angle_get[1] = motor_angle_change(Extern_Motor[1]->real_round, 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]);
// }
void Ball ::Extend_mcontrol(int angle1,int angle2)
{
@ -130,31 +96,17 @@ void Ball ::Extend_mcontrol(int angle1,int angle2)
}
void Ball ::Extend_control(int angle)
{
int16_t delta[2];
angleSet[0] = angle;
angleSet[1] = -angle;
delta[0] = PID_calc(&extend_angle_pid[0],Extern_Motor[0]->total_angle , 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], Extern_Motor[1]->total_angle , angleSet[1]);
e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]);
}
void Ball ::Spin(float speed)
void Ball ::Spin()
{
speedSet[MOTOR_1] = -speed;
speedSet[MOTOR_1] = speed_set;
result[MOTOR_1] = PID_calc(&speed_pid[MOTOR_1],hand_Motor[MOTOR_1]->speed_rpm,speedSet[MOTOR_1]);
speedSet[MOTOR_2] = speed;
speedSet[MOTOR_2] = speed_set;
result[MOTOR_2] = PID_calc(&speed_pid[MOTOR_2],hand_Motor[MOTOR_2]->speed_rpm,speedSet[MOTOR_2]);
speedSet[MOTOR_3] = speed;
speedSet[MOTOR_3] = speed_set;
result[MOTOR_3] = PID_calc(&speed_pid[MOTOR_3],hand_Motor[MOTOR_3]->speed_rpm,speedSet[MOTOR_3]);
}
@ -162,14 +114,7 @@ void Ball ::Spin(float speed)
void Ball::ballDown(void)
{
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//确保闭合气缸
speedm=-500;
}
void Ball::ballStop(void)
{
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保闭合气缸
speedm=0;
speed_set=500;
}
@ -189,73 +134,127 @@ void Ball::Move_Extend(void)
}
}
void Ball::ballHadling(void)
// C键 sw[5]👆 200 中1000 👇1800
// D键 sw[6]👆 200 👇1800
void Ball::rc_mode()
{
if(rc_ctrl.sw[5]==200)
{
rc_key=UP2;
}
if(rc_ctrl.sw[5]==1000)
{
rc_key=MIDDLE2;
}
if(rc_ctrl.sw[5]==1800)
{
rc_key=DOWN2;
}
if(rc_ctrl.sw[6]==200)
{
extern_key=IN;
}
if(rc_ctrl.sw[6]==1800)
{
extern_key=OUT;
}
}
void Ball::ball_control()
{
static int last_sw5 = 0; // 保存上一次拨杆状态
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 有球 1 无球 0
switch (currentState1)
{
case BALL_IDLE:
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保闭合气缸
// 只在拨杆从非200切到200时触发
if ((rc_ctrl.sw[5] == 200 && last_sw5 != 200) || key > 0)
Move_Extend();
switch (rc_key){
case MIDDLE2:
speed_set=0;
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
if (currentState1 == BALL_FINISH)
{
speedm = -4000;
currentState1 = BALL_FORWARD; // 切换到正转状态
currentState1 = BALL_IDLE;
}
else {
currentState1 = BALL_IDLE; // 默认回到空闲状态
}
break;
case BALL_FORWARD:
if (hand_Motor[MOTOR_1]->speed_rpm >= 3980 && hand_Motor[MOTOR_1]->speed_rpm <= 4020 &&
hand_Motor[MOTOR_2]->speed_rpm <= -3980 && hand_Motor[MOTOR_2]->speed_rpm >= -4020 &&
hand_Motor[MOTOR_3]->speed_rpm <= -3980 && hand_Motor[MOTOR_3]->speed_rpm >= -4020)
{
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸
currentState1 = BALL_DROP; // 切换到球下落状态
}
case UP2:
Three_Handing();
break;
case DOWN2:
ballDown();
break;
}
Spin();
Send_control();
}
void Ball::Three_Handing()
{
switch (currentState1){
case BALL_IDLE:
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
currentState1 = BALL_FORWARD; // 切换到正转状态
break;
case BALL_FORWARD:
speed_set=M_SPEED1;
if(is_reached_multiple(hand_Motor[MOTOR_1]->speed_rpm,
hand_Motor[MOTOR_2]->speed_rpm,
hand_Motor[MOTOR_3]->speed_rpm,
speed_set,
speed_set,
speed_set,
50.f,50))
{
currentState1 = BALL_DROP; // 切换到球下落状态
}
break;
case BALL_DROP:
if (ball_state == 0) // 读光电 有球 1 无球 0
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 打开气缸
if (ball_state == 0) // 读光电 有球 1 无球 0
{
osDelay(200); // 延时200ms
speedm = 3500;
speed_set = M_BACK;
currentState1 = BALL_REVERSE; // 切换到反转状态
}
break;
case BALL_REVERSE:
if (ball_state == 1)
{
speedm = 0; // 停止电机
speed_set = 0;
currentState1 = BALL_CLOSE; // 切换到完成状态
}
break;
case BALL_CLOSE:
if (ball_state == 1)
if(ball_state == 1)
{
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
currentState1 = BALL_FINISH; // 切换到完成状态
}
break;
}
case BALL_FINISH:
osDelay(200); // 延时200ms
key = 0; // 重置按键状态
speedm = 0;
speed_set = 0;
osThreadFlagsSet(task_struct.thread.shoot, BALL_OK);
currentState1 = BALL_IDLE; // 直接回到空闲状态
break;
//currentState1 = BALL_IDLE; // 直接回到空闲状态
break;
default:
currentState1 = BALL_IDLE; // 默认回到空闲状态
break;
}
currentState1 = BALL_IDLE; // 默认回到空闲状态
break;
last_sw5 = rc_ctrl.sw[5]; // 更新上一次拨杆状态
}
}
void Ball::Send_control()

24
User/module/ball.hpp
View File

@ -9,7 +9,7 @@
#include "djiMotor.h"
#include "pid.h"
#include "filter.h"
#include "calc_lib.h"
// 定义状态枚举
typedef enum {
@ -36,11 +36,14 @@ typedef enum
MOTOR_NUM
}motorhangd_e;
/* 滤波器 */
typedef struct {
LowPassFilter2p_t *in; /* 反馈值滤波器 */
LowPassFilter2p_t *out; /* 输出值滤波器 */
} Filter;
typedef enum
{
UP2=1,
MIDDLE2,
DOWN2,
IN,
OUT
}ball_rc_mode;
// 定义光电传感器检测宏
#define IS_PHOTOELECTRIC_BALL() (HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin) == GPIO_PIN_RESET)
@ -49,14 +52,16 @@ class Ball
{
public:
Ball();
void Spin(float speed);
void Spin(void);
void ballHadling(void);
void ballDown(void);
void Send_control(void);
void ballStop(void);
void Move_Extend(void);
void Extend_control(int angle);
void rc_mode(void);
void Extend_mcontrol(int angle1,int angle2);
void Three_Handing(void);
void ball_control(void);
BallState_t currentState1; // 当前状态
int flag;//暂时还没用到
@ -66,9 +71,12 @@ public:
int16_t e_result[2];
motor_measure_t * Extern_Motor[2];
float speed_set;
int16_t result[MOTOR_NUM];
motor_measure_t *hand_Motor[MOTOR_NUM];
//==========================公共变量==========================//
int16_t rc_key; //遥控器按键
int16_t extern_key;
//用于传接球,运球后通知
volatile BallState_t ballStatus;//是否有球
volatile uint32_t flag_thread;//接收传回的线程通知

4
User/module/shoot.cpp
View File

@ -198,7 +198,7 @@ void Shoot::shoot_control() {
}
// 发送数据到蓄力电机
//GO_SendData(go1.Pos, 5.0f);
GO_SendData(go1.Pos, 5.0f);
// 控制扳机电机
trigger_control();
@ -235,7 +235,7 @@ void Shoot :: shoot_Current()
if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR) {
BSP_Buzzer_Start();
// BSP_Buzzer_Set(1,5000);
see_you_again();
// see_you_again();
//currentState = SHOOT_WAIT; // 等待发射信号
//在拨杆切换时触发了
}

42
User/task/ballTask.cpp
View File

@ -39,45 +39,9 @@ void FunctionBall(void *argument)
abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
// if(rc_ctrl.sw[6] == 200)
// {
// angle1=85;
// }
// if(rc_ctrl.sw[6]==1800)
// {
// angle1=10;
// }
// //运球
// if(rc_ctrl.sw[5]==200)
// {
// //👇
// ball.ballHadling(); // 处理摩擦轮转动
// }
// //下球
// if(rc_ctrl.sw[5]==1800)
// {
// //👆
// ball.ballDown();
// }
// //中间
// if(rc_ctrl.sw[7]==200)
// {
// if(rc_ctrl.sw[5]==1000)
// {
// //中
// ball.ballStop();
// ball.ballHadling(); // 处理摩擦轮转动
// }
// }
//ball.Extend_control(test111);
ball.Move_Extend();
// ball.Extend_mcontrol(angle1,angle2); // 内伸
ball.Spin(speedm);
ball.Send_control();
ball.rc_mode(); // 遥控器模式
ball.ball_control(); // 控制球的动作
// vofa[0] = -speedm; // 发送电机角度数据
// vofa[1] = ball.hand_Motor[0]->speed_rpm; // 发送电机角度数据

2
User/task/initTask.c
View File

@ -17,8 +17,6 @@ void initFunction(void *argument)
osKernelLock();
/* 创建任务 */
task_struct.thread.gimbal =
osThreadNew(FunctionGimbal, NULL, &attr_gimbal);
task_struct.thread.shoot =
osThreadNew(FunctionShoot, NULL, &attr_shoot);
task_struct.thread.can =

4
User/task/userTask.h
View File

@ -19,7 +19,7 @@ extern "C" {
#define TASK_FREQ_MONITOR (2u)
#define TASK_FREQ_CAN (1500u)
#define TASK_FREQ_AI (500u)
#define TASK_FREQ_BALL (500u)
#define TASK_FREQ_BALL (700u)
#define TASK_INIT_DELAY_INFO (500u)
#define TASK_INIT_DELAY_MONITOR (10)
@ -99,7 +99,6 @@ typedef struct
/* Exported functions ------------------------------------------------------- */
extern task_t task_struct; /* 任务结构体实例化 */
extern const osThreadAttr_t attr_shoot;
extern const osThreadAttr_t attr_gimbal;
extern const osThreadAttr_t attr_can;
extern const osThreadAttr_t attr_ball;
extern const osThreadAttr_t attr_nuc;
@ -108,7 +107,6 @@ extern const osEventFlagsAttr_t attr_event;
/* Exported functions prototypes -------------------------------------------- */
void FunctionTake(void *argument);
void FunctionGimbal(void *argument);
void FunctionShoot(void *argument);
void FunctionCan(void *argument);
void FunctionRc(void *argument);