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

初测自瞄

Browse Source
This commit is contained in:
ws 2025-04-13 14:54:07 +08:00
parent abaca03ecc
commit 9d073a0da2
12 changed files with 185 additions and 202 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
README.md
View File

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

22
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,24 +297,6 @@ 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电机转速

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

157
User/module/ball.cpp
View File

@ -4,137 +4,66 @@
#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 5000
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 ++)
{
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//收网
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初始化
}
else if(rc_ctrl.ch[1]<300)
{
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//放网
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);
}
void Ball ::ballHadling(void)
{
if(rc_ctrl.sw[0]==306)
for(int i = 0;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]);
}
CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1);
osDelay(1);
}
void Ball::ballHadling(void)
{
static bool isReversed = false; // 静态变量，记录当前摩擦轮状态，初始为正转
if (key > 0) // 检测按键是否被按下
{
key = 0; // 重置按键状态，防止重复触发
if (isReversed)
{
// 当前为反转，切换为正转
Spin(MOTOR_SPEED); // 正转
isReversed = false;
}
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() {
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;
}
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;
}
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;
// 当前为正转，切换为反转
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
};

12
User/task/ballTask.cpp
View File

@ -15,10 +15,6 @@ 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 +22,8 @@ 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();
//ball.ballHadling(); // 处理摩擦轮转动
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();

10
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();
@ -29,6 +27,10 @@ void FunctionGimbal(void *argument)
#ifdef DEBUG
task_struct.stack_water_mark.gimbal = osThreadGetStackSpace(osThreadGetId());
#endif
cnt1++;
// gimbal.gimbalFlow();
// 从消息队列接收视觉数据
if (osMessageQueueGet(task_struct.msgq.nuc, &nucData, NULL, 0) == osOK)
{
@ -36,8 +38,6 @@ void FunctionGimbal(void *argument)
gimbal.gimbalVision(nucData);
}
gimbal.gimbalZero();
gimbal.gimbalFlow();
osDelay(1);
tick += delay_tick; /* 计算下一个唤醒时刻 */

1
User/task/nucTask.cpp
View File

@ -30,6 +30,7 @@ void Function_nuc(void *argument)
NUC_RawParse(&cmd_fromnuc);
//掉线处理有空写
// if(NUC_WaitDmaCplt())
// {
// NUC_RawParse(&cmd_fromnuc);

4
User/task/shootTask.cpp
View File

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