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

1

Browse Source
This commit is contained in:
ws 2025-04-28 21:57:59 +08:00
parent 2cebb50876
commit 793e40985b
14 changed files with 276 additions and 232 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
User/bsp/TopDefine.h
View File

@ -1,10 +1,63 @@
#ifndef TOP_DEFINE_H #ifndef TOP_DEFINE_H
#define TOP_DEFINE_H #define TOP_DEFINE_H
#define RTOS 1
#define DEBUG 1
#define AUTO 0
//===================用户配置===================
//是否使用freertos
#ifndef _FREERTOS
#define _FREERTOS 1
#endif
//是否开启调试
#ifndef DEBUG
#define DEBUG 1
#endif
//是否使用自动
#ifndef _AUTO
#define _AUTO 0
#endif
//是否使用大疆DT7遥控器
#ifndef DT7
#define DT7 0
#endif
//是否使用三摩擦
#ifndef HANDLING3
#define HANDLING3 1
#endif
//云台使用类型
#ifndef GM6020ING
#define GM6020ING 1
#endif
//=============================================
//================任务通知,时间组================//
//事件组 //事件组
#define EVENT_RC (1<<1) #define EVENT_RC (1<<1)
#define EVENT_CAN (1<<2) #define EVENT_CAN (1<<2)
//================任务通知================//
//运球
#define BALL_OK (1<<1)
//接到放球命令
#define PUT_CMD (1<<2)
//运球结束
#define PUT_OK (1<<2)
//take任务要等待上面两个标志位
#define TAKE_WAIT (0x0C)
//要发送ok了
#define BALL_SEND (1<<6)
//能够处理放球命令
#define PUT_ENABLE (1<<7)
//userTask里定义机器人状态
typedef enum
{
ROBOT_IN_ONE = 1,
// ROBOT_IN_TWO,
ROBOT_FIND_BALL,
ROBOT_PUT_BALL,
ROBOT_ERROR
}robot_status_e;
#endif #endif

10
User/device/device.h
View File

@ -29,11 +29,15 @@ DEVICE_ERR_OVERFLOW = -5,
#define SIGNAL_IST8310_MAGN_NEW_DATA (1u << 8) #define SIGNAL_IST8310_MAGN_NEW_DATA (1u << 8)
#define SIGNAL_IST8310_MAGN_RAW_REDY (1u << 9) #define SIGNAL_IST8310_MAGN_RAW_REDY (1u << 9)
#define SIGNAL_REFEREE_RAW_REDY (1u << 10) #define SIGNAL_R12DS_BUF0_REDY (1u << 7)
#define SIGNAL_REFEREE_FAST_REFRESH_UI (1u << 11) #define SIGNAL_NUC_RAW_REDY (1u << 8)
#define SIGNAL_REFEREE_SLOW_REFRESH_UI (1u << 12) #define SIGNAL_IST8310_MAGN_RAW_REDY (1u << 9)
#define SIGNAL_ACTION_RAW_REDY (1u << 10)
#define SIGNAL_OPSTIMER_REDY (1u << 11)
#define SIGNAL_R12DS_BUF1_REDY (1u << 12)
#define SIGNAL_AI_RAW_REDY (1u << 14) #define SIGNAL_AI_RAW_REDY (1u << 14)
#define SIGNAL_KEY_REDY (1u << 15)
#ifdef __cplusplus #ifdef __cplusplus
} }

209
User/device/djiMotor.c
View File

@ -159,7 +159,7 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
} }
#else #else
static osEventFlagsId_t eventReceive; //static osEventFlagsId_t eventReceive;
static osThreadId_t thread_alert; static osThreadId_t thread_alert;
/** /**
* @brief 自定义大疆电机回调函数 * @brief 自定义大疆电机回调函数
@ -285,6 +285,110 @@ void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeD
HAL_CAN_AddTxMessage(hcan, &tx_meessage_2ff, can_send_data_2ff, &send_mail_box); HAL_CAN_AddTxMessage(hcan, &tx_meessage_2ff, can_send_data_2ff, &send_mail_box);
} }
//控制四个vesc的发送
int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan)
{
uint32_t send_mail_box;
int Byte[4];
Vesc_ByteGet raw[4];
//将期望的四个电机输出值分别对应到四个联合体 为了下面的拆分字节
raw[0].as_array = speed;
raw[1].as_array = speed;
for(int i=0 ; i < 2 ; i ++)
{
//将单个电机的期望输出值通过联合体拆分
Byte[0] = raw[i].byte.byte1;
Byte[1] = raw[i].byte.byte2;
Byte[2] = raw[i].byte.byte3;
Byte[3] = raw[i].byte.byte4;
raw_tx.tx_header.ExtId = id+i+CAN_VESC_CTRL_ID_BASE;
raw_tx.tx_header.IDE = CAN_ID_EXT;
raw_tx.tx_header.RTR = CAN_RTR_DATA;
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
raw_tx.tx_data[0] = Byte[3];
raw_tx.tx_data[1] = Byte[2];
raw_tx.tx_data[2] = Byte[1];
raw_tx.tx_data[3] = Byte[0];
raw_tx.tx_data[4] = 0;
raw_tx.tx_data[5] = 0;
raw_tx.tx_data[6] = 0;
raw_tx.tx_data[7] = 0;
HAL_CAN_AddTxMessage(hcan, &raw_tx.tx_header, raw_tx.tx_data, &send_mail_box);
}
return DEVICE_OK;
}
/**
* @brief 限制vesc电机转速
* @param[in/out] rpm: vesce电机转速
* @retval none
*
* 如果rpm的绝对值大于wtrcfg_VESC_COMMAND_ERPM_MAX,
* 则将rpm的值设置为wtrcfg_VESC_COMMAND_ERPM_MAX或-wtrcfg_VESC_COMMAND_ERPM_MAX
*/
void assert_param_rpm(float *rpm){
if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
*rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
}
/**
* @brief 使vesc电机进入转速模式
* @param[in] controller_id: vesc电机控制器id
* @param[in] RPM: 电机转速
* @retval RPM（1000-50000之间的数）
*/
void CAN_VESC_RPM(uint8_t controller_id, float RPM)
{
uint32_t id;
int32_t data;
uint32_t send_mail_box;
id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
assert_param_rpm(&RPM);
data = (int32_t)(RPM);
vesc_tx_message.ExtId = id;
vesc_tx_message.IDE = CAN_ID_EXT;
vesc_tx_message.RTR = CAN_RTR_DATA;
vesc_tx_message.DLC = 0x04;
vesc_send_data[0] = data >> 24 ;
vesc_send_data[1] = data >> 16 ;
vesc_send_data[2] = data >> 8 ;
vesc_send_data[3] = data ;
HAL_CAN_AddTxMessage(&hcan2, &vesc_tx_message, vesc_send_data, &send_mail_box);
}
/**
* @brief 使vesc电机进入制动模式
* @param[in] controller_id: vesc电机控制器id
* @retval none
*/
void CAN_VESC_HEAD(uint8_t controller_id)
{
uint32_t id;
uint32_t send_mail_box;
id = controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8);
vesc_tx_message.ExtId = id;
vesc_tx_message.IDE = CAN_ID_EXT;
vesc_tx_message.RTR = CAN_RTR_DATA;
vesc_tx_message.DLC = 0x04;
HAL_CAN_AddTxMessage(&hcan2, &vesc_tx_message, vesc_send_data, &send_mail_box);
}
/** /**
* @brief 返回电机数据指针 * @brief 返回电机数据指针
* @param[in] i: 电机编号 * @param[in] i: 电机编号
@ -295,106 +399,7 @@ void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeD
return &motor_chassis[i]; return &motor_chassis[i];
} }
int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan) CAN_MotorFeedback_t *get_vesc_point(uint8_t i)
{ {
return &motor_5065[i];
uint32_t send_mail_box;
int Byte[4];
Vesc_ByteGet raw[4];
//将期望的四个电机输出值分别对应到四个联合体 为了下面的拆分字节
raw[0].as_array = speed;
raw[1].as_array = speed;
for(int i=0 ; i < 2 ; i ++)
{
//将单个电机的期望输出值通过联合体拆分
Byte[0] = raw[i].byte.byte1;
Byte[1] = raw[i].byte.byte2;
Byte[2] = raw[i].byte.byte3;
Byte[3] = raw[i].byte.byte4;
raw_tx.tx_header.ExtId = id+i+CAN_VESC_CTRL_ID_BASE;
raw_tx.tx_header.IDE = CAN_ID_EXT;
raw_tx.tx_header.RTR = CAN_RTR_DATA;
raw_tx.tx_header.DLC = CAN_MOTOR_TX_BUF_SIZE;
raw_tx.tx_data[0] = Byte[3];
raw_tx.tx_data[1] = Byte[2];
raw_tx.tx_data[2] = Byte[1];
raw_tx.tx_data[3] = Byte[0];
raw_tx.tx_data[4] = 0;
raw_tx.tx_data[5] = 0;
raw_tx.tx_data[6] = 0;
raw_tx.tx_data[7] = 0;
HAL_CAN_AddTxMessage(hcan, &raw_tx.tx_header, raw_tx.tx_data, &send_mail_box);
}
return DEVICE_OK;
}
/**
* @brief 限制vesc电机转速
* @param[in/out] rpm: vesce电机转速
* @retval none
*
* 如果rpm的绝对值大于wtrcfg_VESC_COMMAND_ERPM_MAX,
* 则将rpm的值设置为wtrcfg_VESC_COMMAND_ERPM_MAX或-wtrcfg_VESC_COMMAND_ERPM_MAX
*/
void assert_param_rpm(float *rpm){
if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
*rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
}
/**
* @brief 使vesc电机进入转速模式
* @param[in] controller_id: vesc电机控制器id
* @param[in] RPM: 电机转速
* @retval RPM（1000-50000之间的数）
*/
void CAN_VESC_RPM(uint8_t controller_id, float RPM)
{
uint32_t id;
int32_t data;
uint32_t send_mail_box;
id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
assert_param_rpm(&RPM);
data = (int32_t)(RPM);
vesc_tx_message.ExtId = id;
vesc_tx_message.IDE = CAN_ID_EXT;
vesc_tx_message.RTR = CAN_RTR_DATA;
vesc_tx_message.DLC = 0x04;
vesc_send_data[0] = data >> 24 ;
vesc_send_data[1] = data >> 16 ;
vesc_send_data[2] = data >> 8 ;
vesc_send_data[3] = data ;
HAL_CAN_AddTxMessage(&hcan2, &vesc_tx_message, vesc_send_data, &send_mail_box);
}
/**
* @brief 使vesc电机进入制动模式
* @param[in] controller_id: vesc电机控制器id
* @retval none
*/
void CAN_VESC_HEAD(uint8_t controller_id)
{
uint32_t id;
uint32_t send_mail_box;
id = controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8);
vesc_tx_message.ExtId = id;
vesc_tx_message.IDE = CAN_ID_EXT;
vesc_tx_message.RTR = CAN_RTR_DATA;
vesc_tx_message.DLC = 0x04;
HAL_CAN_AddTxMessage(&hcan2, &vesc_tx_message, vesc_send_data, &send_mail_box);
} }

1
User/device/djiMotor.h
View File

@ -152,6 +152,7 @@ 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);
/** /**
* @brief 数据处理函数 * @brief 数据处理函数
* @param[in] none * @param[in] none

14
User/device/nuc.c
View File

@ -2,17 +2,19 @@
#include <string.h> #include <string.h>
#include "crc_ccitt.h" #include "crc_ccitt.h"
static osThreadId_t thread_alert;
uint8_t nucbuf[32]; uint8_t nucbuf[32];
uint8_t packet[32]; // 假设最大数据包长度为 32 字节 uint8_t packet[32]; // 假设最大数据包长度为 32 字节
static void NUC_IdleCallback(void) { static void NUC_IdleCallback(void) {
// osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY); osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
// detect_hook(NUC_TOE);
} }
int8_t NUC_Init(NUC_t *nuc){ int8_t NUC_Init(NUC_t *nuc){
if(nuc == NULL) return DEVICE_ERR_NULL; if(nuc == NULL) return DEVICE_ERR_NULL;
//信号量的东西 // if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL; if((thread_alert = osThreadGetId()) == NULL ) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_AI,BSP_UART_IDLE_LINE_CB, BSP_UART_RegisterCallback(BSP_UART_AI,BSP_UART_IDLE_LINE_CB,
NUC_IdleCallback); NUC_IdleCallback);
@ -35,9 +37,9 @@ int8_t NUC_Restart(void) {
bool_t NUC_WaitDmaCplt(void) { bool_t NUC_WaitDmaCplt(void) {
// return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) == return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) ==
// SIGNAL_NUC_RAW_REDY); SIGNAL_NUC_RAW_REDY);
return 1;
} }
int8_t NUC_SendPacket(void *data, uint16_t length) { int8_t NUC_SendPacket(void *data, uint16_t length) {

1
User/device/nuc.h
View File

@ -4,6 +4,7 @@
extern "C" { extern "C" {
#endif #endif
#include <cmsis_os2.h>
#include "struct_typedef.h" #include "struct_typedef.h"
#include "device.h" #include "device.h"
#include "uart_it.h" #include "uart_it.h"

3
User/device/remote_control.h
View File

@ -22,13 +22,12 @@
extern "C"{ extern "C"{
#endif #endif
#include "TopDefine.h"
#include "struct_typedef.h" #include "struct_typedef.h"
#include "usart.h" #include "usart.h"
#include <string.h> #include <string.h>
#include <stdlib.h> #include <stdlib.h>
#define DT7 0
#if DT7==1 #if DT7==1
#define DBUS_MAX_LEN (50)//通过 UART 串口接收的最大数据长度 #define DBUS_MAX_LEN (50)//通过 UART 串口接收的最大数据长度

88
User/module/ball.cpp
View File

@ -2,24 +2,20 @@
#include "bsp_delay.h" #include "bsp_delay.h"
#include "remote_control.h" #include "remote_control.h"
#include "detect.h" #include "detect.h"
#include "userTask.h"
extern RC_ctrl_t rc_ctrl; extern RC_ctrl_t rc_ctrl;
extern int key; extern int key;
// 定义状态机变量 #if HANDLING3 == 1
BallState_t currentState1 = BALL_IDLE; // 当前状态 //三摩擦轮运球！！！
uint32_t startTime = 0; // 用于记录延时的起始时间 //添加平移3508 得跑位置吧
int speedm=0;
int speedm1=0;
#define MOTOR_SPEED 1000
const fp32 Ball:: M3508_speed_PID[3] = {5, 0.01, 0}; const fp32 Ball:: M3508_speed_PID[3] = {5, 0.01, 0};
int speedm=0;
int speedm1=0;
//PE11 气缸 //PE11 气缸
//三摩擦轮运球！！！
Ball ::Ball() Ball ::Ball()
{ {
detect_init(); detect_init();
@ -34,11 +30,11 @@ Ball ::Ball()
result[i] = 0; result[i] = 0;
speedSet[i] = 0; speedSet[i] = 0;
} }
//状态机状态初始化
currentState1= BALL_IDLE;
} }
#if HANDLING3 == 1
void Ball ::Spin(float speed,float speed2) void Ball ::Spin(float speed,float speed2)
{ {
@ -100,7 +96,6 @@ void Ball::ballHadling(void)
if (ball_state == 0) if (ball_state == 0)
{ {
//flag=2; //抽象的计次
speedm=0; // 停止电机 speedm=0; // 停止电机
speedm1=0; speedm1=0;
currentState1 = BALL_CLOSE; // 切换到完成状态 currentState1 = BALL_CLOSE; // 切换到完成状态
@ -115,15 +110,7 @@ void Ball::ballHadling(void)
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
currentState1 = BALL_FINISH; // 切换到完成状态 currentState1 = BALL_FINISH; // 切换到完成状态
} }
// if (flag == 2)
// {
// if(ball_state == 0)
// {
// HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
// currentState1 = BALL_FINISH; // 切换到完成状态
// }
// }
break; break;
case BALL_FINISH: case BALL_FINISH:
@ -132,6 +119,7 @@ void Ball::ballHadling(void)
flag=0; flag=0;
speedm=0; speedm=0;
speedm1=0; speedm1=0;
osThreadFlagsSet(task_struct.thread.shoot, BALL_OK);
currentState1 = BALL_IDLE; // 回到空闲状态 currentState1 = BALL_IDLE; // 回到空闲状态
break; break;
@ -150,62 +138,8 @@ void Ball::Send_control()
} }
#else #else
int flag =0;
int ball_state = 0;
// void Ball::ballHadling(void)
// {
// ball_state =HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 0 无球 1
// switch (currentState1)
// {
// case BALL_IDLE:
// if (key > 0) // 检测按键是否被按下
// {
// currentState1 = BALL_FORWARD; // 切换到正转状态
// }
// break;
// case BALL_FORWARD: int ball_state = 0;
// HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET);
// osDelay(50);
// HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET);
// currentState1 = BALL_DROP; // 切换到球下落状态
// break;
// case BALL_DROP:
// osDelay(100); //
// HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
// currentState1 = BALL_CLOSE; // 切换到反转状态
// break;
// case BALL_CLOSE:
// osDelay(100);
// if(ball_state == 0)
// {
// HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET);
// currentState1 = BALL_REVERSE; // 切换到反转状态
// }
// break;
// case BALL_REVERSE:
// osDelay(50); // 延时200ms
// HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET);
// HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
// key=0; // 重置按键状态
// currentState1 = BALL_IDLE; // 回到空闲状态
// break;
// default:
// currentState1 = BALL_IDLE; // 默认回到空闲状态
// break;
// }
// }
int triggerCount = 0; // 光电传感器触发计数 int triggerCount = 0; // 光电传感器触发计数
int last_ball_state = 1; // 上一次的光电状态 int last_ball_state = 1; // 上一次的光电状态
void Ball::ballHadling(void) void Ball::ballHadling(void)

21
User/module/ball.hpp
View File

@ -1,14 +1,14 @@
#ifndef BALL_HPP #ifndef BALL_HPP
#define BALL_HPP #define BALL_HPP
#include "main.h" #include "main.h"
#include "TopDefine.h"
#include "cmsis_os.h" #include "cmsis_os.h"
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include <cmsis_os2.h>
#include "bsp_delay.h" #include "bsp_delay.h"
#include "djiMotor.h" #include "djiMotor.h"
#include "pid.h" #include "pid.h"
#define HANDLING3 1
// 定义状态枚举 // 定义状态枚举
typedef enum { typedef enum {
BALL_IDLE, // 空闲状态 BALL_IDLE, // 空闲状态
@ -17,7 +17,11 @@ typedef enum {
BALL_REVERSE, // 反转状态 BALL_REVERSE, // 反转状态
BALL_FLAG, BALL_FLAG,
BALL_CLOSE, // 关闭状态 BALL_CLOSE, // 关闭状态
BALL_FINISH // 完成状态 BALL_FINISH, // 完成状态
//持球状态
BALL_TAKE,
BALL_LOSE
} BallState_t; } BallState_t;
typedef enum typedef enum
@ -39,14 +43,17 @@ public:
void Spin(float speed,float speed2); void Spin(float speed,float speed2);
void ballHadling(void); void ballHadling(void);
void Send_control(void); void Send_control(void);
void ballStateMachine(void);
BallState_t currentState1; // 当前状态
int16_t result[MOTOR_NUM]; int16_t result[MOTOR_NUM];
motor_measure_t *hand_Motor[MOTOR_NUM]; motor_measure_t *hand_Motor[MOTOR_NUM];
private: //==========================公共变量==========================//
//用于传接球,运球后通知
volatile BallState_t ballStatus;//是否有球
volatile uint32_t flag_thread;//接收传回的线程通知
int up_ball_state; //上球状态 0:无球 1:有球 private:
//三个摩擦轮 //三个摩擦轮
static const float M3508_speed_PID[3]; static const float M3508_speed_PID[3];
static const float M3508_angle_PID[3]; static const float M3508_angle_PID[3];

1
User/module/gimbal.hpp
View File

@ -6,7 +6,6 @@
#include "pid.h" #include "pid.h"
#include "nuc.h" #include "nuc.h"
#define GM6020ING 1
class Gimbal class Gimbal
{ {
public: public:

62
User/module/shoot.cpp
View File

@ -7,48 +7,62 @@
#include "calc_lib.h" #include "calc_lib.h"
extern RC_ctrl_t rc_ctrl; extern RC_ctrl_t rc_ctrl;
#define SHOOT_SPEED 40000 #define SHOOT_SPEED 40000
#define SHOOT_SPEED_BACK -4000 #define SHOOT_SPEED_BACK -2000
#define STOP 0 #define STOP 0
#define Trigger 3000
// 定义状态机变量 const fp32 Shoot:: M2006_speed_PID[3] = {5, 0.01, 0};
ShootState_t currentState = SHOOT_IDLE; int t=0;
Shoot::Shoot() Shoot::Shoot()
{ {
// motor_5065[0] = get_5065_motor_point(0);//获取电机数据指针 //扳机初始化
// motor_5065[1] = get_5065_motor_point(1);//获取电机数据指针 trigger_Motor= get_motor_point(4);
CAN_VESC_RPM(1, STOP); trigger_Motor->type=M3508;
CAN_VESC_RPM(2, STOP); PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,16000, 10000);//pid初始化
speedSet = 0;
result = 0;
//发射摩擦轮
motor_5065[0] = get_vesc_point(0);//获取电机数据指针
motor_5065[1] = get_vesc_point(1);//获取电机数据指针
speed_5065=0;
currentState= SHOOT_IDLE;
}
void Shoot::trigger_spin()
{
if(t>0)
{
speedSet=Trigger;
if(trigger_Motor->speed_rpm<5)
{
speedSet=-Trigger;
}
}
} }
void Shoot::shootThree() void Shoot::shootThree()
{ {
if((rc_ctrl.sw[1]>500)) if((rc_ctrl.sw[1]>500))
{ {
speed_5065 = map((float)rc_ctrl.sw[1],500,1694,30000,45000); speed_5065 = SHOOT_SPEED;
// CAN_VESC_RPM(1, speed_5065);
// CAN_VESC_RPM(2, speed_5065);
} }
else else
{ {
speed_5065=STOP; speed_5065=STOP;
// CAN_VESC_Control(1,STOP,&hcan2);
// CAN_VESC_RPM(1, STOP);
// CAN_VESC_RPM(2, STOP);
} }
if(rc_ctrl.sw[5]==1694) if(rc_ctrl.sw[5]==1694)
{ {
speed_5065=SHOOT_SPEED_BACK; speed_5065=SHOOT_SPEED_BACK;
// CAN_VESC_Control(1,SHOOT_SPEED_BACK,&hcan2);
// CAN_VESC_RPM(1, SHOOT_SPEED_BACK);
// CAN_VESC_RPM(2, SHOOT_SPEED_BACK);
} }
//CAN_VESC_Control(1,speed_5065,&hcan2); CAN_VESC_Control(1,speed_5065,&hcan2);
CAN_VESC_RPM(1, speed_5065); // CAN_VESC_RPM(1, speed_5065);
CAN_VESC_RPM(2, speed_5065); // CAN_VESC_RPM(2, speed_5065);
} }
void Shoot::shootBack() void Shoot::shootBack()
@ -69,12 +83,6 @@ void Shoot::shootStop()
} }
void Shoot::shootControl()
{
shootThree();
}
void Shoot::shootStateMachine() { void Shoot::shootStateMachine() {
switch (currentState) { switch (currentState) {
case SHOOT_IDLE: { case SHOOT_IDLE: {

29
User/module/shoot.hpp
View File

@ -1,6 +1,7 @@
#ifndef SHOOT_HPP #ifndef SHOOT_HPP
#define SHOOT_HPP #define SHOOT_HPP
#include "djiMotor.h" #include "djiMotor.h"
#include "pid.h"
// 定义状态枚举 // 定义状态枚举
typedef enum { typedef enum {
@ -10,6 +11,15 @@ typedef enum {
SHOOT_RETURN // 自动返回状态 SHOOT_RETURN // 自动返回状态
} ShootState_t; } ShootState_t;
// 定义状态枚举
typedef enum {
BALL_IDLE, // 空闲状态
BALL_FINISH, // 完成状态
//持球状态
BALL_TAKE,
BALL_LOSE
} BallState_t;
// 光电传感器读取宏 // 光电传感器读取宏
#define IS_PHOTOELECTRIC_TRIGGERED() (HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin) == GPIO_PIN_RESET) #define IS_PHOTOELECTRIC_TRIGGERED() (HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin) == GPIO_PIN_RESET)
@ -21,12 +31,27 @@ public:
void shootThree(void); void shootThree(void);
void shootStop(void); void shootStop(void);
void shootBack(void); void shootBack(void);
void shootControl(void);
void shootStateMachine(void); void shootStateMachine(void);
void trigger_spin(void);
float speed_5065; float speed_5065;
ShootState_t currentState;
//==========================公共变量==========================
volatile BallState_t ballStatus;//是否有球
volatile uint32_t flag_thread;//接收传回的线程通知
private: private:
CAN_MotorFeedback_t *motor_5065[2]; //扳机2006
static const float M2006_speed_PID[3];
//电机速度pid结构体
pid_type_def speed_pid;
motor_measure_t *trigger_Motor;
int16_t result;
float speedSet;
CAN_MotorFeedback_t *motor_5065[2];
}; };

2
User/task/ballTask.cpp
View File

@ -29,7 +29,7 @@ void FunctionBall(void *argument)
abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
//abc=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin); //abc=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);
ball.ballHadling(); // 处理摩擦轮转动 ball.ballHadling(); // 处理摩擦轮转动
ball.Spin(speedm,speedm1); ball.Spin(speedm,speedm1);
ball.Send_control(); ball.Send_control();

8
User/task/shootTask.cpp
View File

@ -25,7 +25,13 @@ while(1)
task_struct.stack_water_mark.shoot = osThreadGetStackSpace(osThreadGetId()); task_struct.stack_water_mark.shoot = osThreadGetStackSpace(osThreadGetId());
#endif #endif
shoot.shootThree(); shoot.flag_thread=osThreadFlagsGet();
if(shoot.flag_thread & BALL_OK)
{
shoot.shootThree();
}
//shoot.shootBack(); //shoot.shootBack();
osDelay(2); osDelay(2);