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-12 17:07:28 +08:00
parent b2ac3ccd4d
commit e531eb8d47
22 changed files with 133 additions and 487 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
MDK-ARM/.vscode/c_cpp_properties.json vendored
View File

@ -3,26 +3,26 @@
{ {
"name": "R1", "name": "R1",
"includePath": [ "includePath": [
"d:\\Desktop\\r1\\r1_up\\Core\\Inc", "d:\\Desktop\\r1\\R1\\R1_up\\Core\\Inc",
"d:\\Desktop\\r1\\r1_up\\Drivers\\STM32F4xx_HAL_Driver\\Inc", "d:\\Desktop\\r1\\R1\\R1_up\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
"d:\\Desktop\\r1\\r1_up\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy", "d:\\Desktop\\r1\\R1\\R1_up\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
"d:\\Desktop\\r1\\r1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include", "d:\\Desktop\\r1\\R1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
"d:\\Desktop\\r1\\r1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2", "d:\\Desktop\\r1\\R1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
"d:\\Desktop\\r1\\r1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F", "d:\\Desktop\\r1\\R1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
"d:\\Desktop\\r1\\r1_up\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include", "d:\\Desktop\\r1\\R1\\R1_up\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
"d:\\Desktop\\r1\\r1_up\\Drivers\\CMSIS\\Include", "d:\\Desktop\\r1\\R1\\R1_up\\Drivers\\CMSIS\\Include",
"d:\\Desktop\\r1\\r1_up\\User\\bsp", "d:\\Desktop\\r1\\R1\\R1_up\\User\\bsp",
"d:\\Desktop\\r1\\r1_up\\User\\module", "d:\\Desktop\\r1\\R1\\R1_up\\User\\module",
"d:\\Desktop\\r1\\r1_up\\User\\task", "d:\\Desktop\\r1\\R1\\R1_up\\User\\task",
"d:\\Desktop\\r1\\r1_up\\User\\lib", "d:\\Desktop\\r1\\R1\\R1_up\\User\\lib",
"d:\\Desktop\\r1\\r1_up\\User\\device", "d:\\Desktop\\r1\\R1\\R1_up\\User\\device",
"D:\\keil\\ARM\\ARMCC\\include", "D:\\keil\\ARM\\ARMCC\\include",
"D:\\keil\\ARM\\ARMCC\\include\\rw", "D:\\keil\\ARM\\ARMCC\\include\\rw",
"d:\\Desktop\\r1\\r1_up\\MDK-ARM", "d:\\Desktop\\r1\\R1\\R1_up\\MDK-ARM",
"d:\\Desktop\\r1\\r1_up\\Core\\Src", "d:\\Desktop\\r1\\R1\\R1_up\\Core\\Src",
"d:\\Desktop\\r1\\r1_up\\Drivers\\STM32F4xx_HAL_Driver\\Src", "d:\\Desktop\\r1\\R1\\R1_up\\Drivers\\STM32F4xx_HAL_Driver\\Src",
"d:\\Desktop\\r1\\r1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source", "d:\\Desktop\\r1\\R1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source",
"d:\\Desktop\\r1\\r1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang" "d:\\Desktop\\r1\\R1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
], ],
"defines": [ "defines": [
"USE_HAL_DRIVER", "USE_HAL_DRIVER",

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

@ -8,3 +8,15 @@
[info] Log at : 2025/6/6|16:29:08|GMT+0800 [info] Log at : 2025/6/6|16:29:08|GMT+0800
[info] Log at : 2025/6/7|15:29:24|GMT+0800
[info] Log at : 2025/6/8|21:27:17|GMT+0800
[info] Log at : 2025/6/9|20:02:13|GMT+0800
[info] Log at : 2025/6/10|17:40:24|GMT+0800
[info] Log at : 2025/6/11|13:45:42|GMT+0800
[info] Log at : 2025/6/11|17:37:52|GMT+0800

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

@ -1,4 +1,8 @@
*** 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'
compiling ballTask.cpp...
linking...
Program Size: Code=29728 RO-data=1884 RW-data=284 ZI-data=33268
FromELF: creating hex file...
"R1\R1.axf" - 0 Error(s), 0 Warning(s). "R1\R1.axf" - 0 Error(s), 0 Warning(s).
Build Time Elapsed: 00:00:02 Build Time Elapsed: 00:00:06

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

@ -1 +1 @@
2025/6/6 20:49:07 2025/6/9 20:40:44

43
MDK-ARM/R1.uvoptx
View File

@ -160,6 +160,21 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>shoot,0x0A</ItemText> <ItemText>shoot,0x0A</ItemText>
</Ww> </Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_fromnuc</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>nucbuf</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>goangle,0x0A</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -919,7 +934,7 @@
<Group> <Group>
<GroupName>User/device</GroupName> <GroupName>User/device</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>
@ -1046,18 +1061,6 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\User\module\motor.cpp</PathWithFileName>
<FilenameWithoutPath>motor.cpp</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>65</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>
@ -1073,7 +1076,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<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>
@ -1085,7 +1088,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>67</FileNumber> <FileNumber>66</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1097,7 +1100,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>
@ -1109,7 +1112,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>69</FileNumber> <FileNumber>68</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1121,7 +1124,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>70</FileNumber> <FileNumber>69</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1133,7 +1136,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>71</FileNumber> <FileNumber>70</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1145,7 +1148,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>72</FileNumber> <FileNumber>71</FileNumber>
<FileType>8</FileType> <FileType>8</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>

5
MDK-ARM/R1.uvprojx
View File

@ -738,11 +738,6 @@
<FileType>8</FileType> <FileType>8</FileType>
<FilePath>..\User\module\gimbal.cpp</FilePath> <FilePath>..\User\module\gimbal.cpp</FilePath>
</File> </File>
<File>
<FileName>motor.cpp</FileName>
<FileType>8</FileType>
<FilePath>..\User\module\motor.cpp</FilePath>
</File>
<File> <File>
<FileName>shoot.cpp</FileName> <FileName>shoot.cpp</FileName>
<FileType>8</FileType> <FileType>8</FileType>

2
R1.ioc
View File

@ -448,7 +448,7 @@ TIM10.Period=4999
TIM4.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3 TIM4.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
TIM4.IPParameters=Channel-PWM Generation3 CH3,Period TIM4.IPParameters=Channel-PWM Generation3 CH3,Period
TIM4.Period=20999 TIM4.Period=20999
USART1.BaudRate=115200 USART1.BaudRate=4000000
USART1.IPParameters=VirtualMode,BaudRate,Mode USART1.IPParameters=VirtualMode,BaudRate,Mode
USART1.Mode=MODE_TX_RX USART1.Mode=MODE_TX_RX
USART1.VirtualMode=VM_ASYNC USART1.VirtualMode=VM_ASYNC

22
README.md
View File

@ -1,3 +1,23 @@
# R1_up # R1_up
r1上层 r1上层
## 外设
+ CAN1
- 扳机2006 id:0x205
- 三摩擦 id:123
+ UART
- uart1 波特率4000000 id：2
- uart6 nuc
- uart3 遥控器接收
+ GPIO
- PI6运球光电
- PE11 运球气缸
## 遥控器

10
User/device/nuc.c
View File

@ -93,12 +93,12 @@ int8_t NUC_RawParse(NUC_t *n) {
z fp32 z fp32
0xFE TAIL 0xFE TAIL
*/ */
if (nucbuf[15] != TAIL) goto error; if (nucbuf[7] != TAIL) goto error;
instance.data[3] = nucbuf[3]; instance.data[3] = nucbuf[6];
instance.data[2] = nucbuf[4]; instance.data[2] = nucbuf[5];
instance.data[1] = nucbuf[5]; instance.data[1] = nucbuf[4];
instance.data[0] = nucbuf[6]; instance.data[0] = nucbuf[3];
n->vision.x = instance.x[0]; n->vision.x = instance.x[0];
instance.data[7] = nucbuf[7]; instance.data[7] = nucbuf[7];

4
User/device/remote_control.c
View File

@ -287,11 +287,13 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
rc_ctrl->sw[3] = map(rc_ctrl->sw[3],306,1694,1694,306); rc_ctrl->sw[3] = map(rc_ctrl->sw[3],306,1694,1694,306);
rc_ctrl->ch[1] = map(rc_ctrl->ch[1],-693,694,-700,700); //x rc_ctrl->ch[1] = map(rc_ctrl->ch[1],-693,694,-700,700); //x
rc_ctrl->ch[0] = map(rc_ctrl->ch[0],-694,693,-700,700); //y rc_ctrl->ch[0] = map(rc_ctrl->ch[0],-694,693,-700,700); //y
rc_ctrl->ch[2] = map(rc_ctrl->ch[2],200,1800,-700,700); //x
rc_ctrl->ch[3] = map(rc_ctrl->ch[3],-694,693,-700,700); //y
//死区(-30,30) //死区(-30,30)
if(rc_ctrl->ch[0]>-14&&rc_ctrl->ch[0]<6) rc_ctrl->ch[0]=0; if(rc_ctrl->ch[0]>-14&&rc_ctrl->ch[0]<6) rc_ctrl->ch[0]=0;
if(rc_ctrl->ch[1]>-30&&rc_ctrl->ch[1]<-10) rc_ctrl->ch[1]=0; if(rc_ctrl->ch[1]>-30&&rc_ctrl->ch[1]<-10) rc_ctrl->ch[1]=0;
if(rc_ctrl->ch[2]>=0&&rc_ctrl->ch[2]<=7) rc_ctrl->ch[2]=7; if(rc_ctrl->ch[2]>=0&&rc_ctrl->ch[2]<=7) rc_ctrl->ch[2]=0;
if(rc_ctrl->ch[3]>-22&&rc_ctrl->ch[3]<-2) rc_ctrl->ch[3]=0; if(rc_ctrl->ch[3]>-22&&rc_ctrl->ch[3]<-2) rc_ctrl->ch[3]=0;
remote_ready = 1; remote_ready = 1;

18
User/module/ball.cpp
View File

@ -65,19 +65,21 @@ Ball ::Ball()
} }
void Ball ::Extend_mcontrol(int angle) void Ball ::Extend_mcontrol(int angle1,int angle2)
{ {
int16_t delta; int16_t delta[2];
angleSet[0] = angle/2; angleSet[0] = angle1;
angleSet[1] = angle2;
fp32 angle_get; delta[0] = PID_calc(&extend_angle_pid[0], Extern_Motor[0]->total_angle , angleSet[0]);
angle_get=motor_angle_change(Extern_Motor[0]->real_round, 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]);
delta = PID_calc(&extend_angle_pid[0], angle_get, angleSet[0]);
e_result[0] = PID_calc(&extend_speed_pid[0], Extern_Motor[0]->speed_rpm, delta);
} }
void Ball ::Extend_control(int angle) void Ball ::Extend_control(int angle)
{ {
int16_t delta[2]; int16_t delta[2];

2
User/module/ball.hpp
View File

@ -55,7 +55,7 @@ public:
void ballStop(void); void ballStop(void);
void ballTake(void); void ballTake(void);
void Extend_control(int angle); void Extend_control(int angle);
void Extend_mcontrol(int angle); void Extend_mcontrol(int angle1,int angle2);
BallState_t currentState1; // 当前状态 BallState_t currentState1; // 当前状态
int flag;//暂时还没用到 int flag;//暂时还没用到

42
User/module/motor.cpp
View File

@ -1,42 +0,0 @@
#include "TopDefine.h"
#include "motor.hpp"
#include "remote_control.h"
#include "calc_lib.h"
extern RC_ctrl_t rc_ctrl;
const float Trigger::Trigger_speed_PID[3]={10, 0 ,0}; //3508P,I,D(速度环)
const float Trigger::Trigger_angle_PID[3]={10.0, 0 ,0}; //3508P,I,D(角度环)
Trigger::Trigger()
{
T_Motor = get_motor_point(1);//获取电机数据指针
T_Motor->type = M2006;//设置电机类型
PID_init(&speed_pid,PID_POSITION,Trigger_speed_PID,7000,2000);//pid初始化
PID_init(&angle_pid,PID_POSITION,Trigger_angle_PID,700,0);//pid初始化
result = 0;
angleSet = 0;
}
void Trigger::triggerZero()
{
int16_t delta[1];
angleSet = ZER0;
delta[0] = PID_calc(&angle_pid,T_Motor->total_angle,angleSet);
result = PID_calc(&speed_pid, T_Motor->speed_rpm, delta[0]);
}
void Trigger::triggerFlow(float angle)
{
int16_t delta[1];
//下降
angleSet = angle;
delta[0] = PID_calc(&angle_pid,T_Motor->total_angle,angleSet);
result = PID_calc(&speed_pid, T_Motor->speed_rpm, delta[0]);
}

37
User/module/motor.hpp
View File

@ -1,37 +0,0 @@
#ifndef MOTOR_HPP
#define MOTOR_HPP
#include "djiMotor.h"
#include "pid.h"
typedef enum
{
ZER0 = 0,
// POINT_TOP = 360,
}Trigger_state;//存放位置宏定义
class Trigger
{
public:
Trigger();//构造函数声明
void triggerZero();
void triggerFlow(float angle);
//暂存要发送的电流
int16_t result;
//电机状态
private:
motor_measure_t* T_Motor;
//扳机3508pid
static const float Trigger_speed_PID[3];
static const float Trigger_angle_PID[3];
//电机速度pid结构体
pid_type_def speed_pid;
//位置环pid
pid_type_def angle_pid;
float angleSet;
};
#endif

38
User/module/shoot.cpp
View File

@ -20,13 +20,13 @@ NUC_t nuc_v;
//B键 sw[3]👆 200 开 👇1800 关 //B键 sw[3]👆 200 开 👇1800 关
//左旋钮 sw[4] 👈 200 👉1800 //左旋钮 sw[4] 👈 200 👉1800
const fp32 Shoot:: M2006_speed_PID[3] = {10, 0, 0}; const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0};
const fp32 Shoot:: M2006_angle_PID[3] = { 10, 0, 0.1}; const fp32 Shoot:: M2006_angle_PID[3] = { 25, 0, 0.1};
#define INIT_POS 50 #define INIT_POS -100
#define TOP_POS 100 #define TOP_POS -210
#define Tigger_DO 300 #define Tigger_DO -5
#define Tigger_ZERO 0 #define Tigger_ZERO 130
float knob_increment; float knob_increment;
@ -35,8 +35,8 @@ Shoot::Shoot()
//扳机初始化 //扳机初始化
trigger_Motor= get_motor_point(4);//id 205 trigger_Motor= get_motor_point(4);//id 205
trigger_Motor->type=M2006; trigger_Motor->type=M2006;
PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,8000, 1000);//pid初始化 PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,3000, 2000);//pid初始化
PID_init(&angle_pid,PID_POSITION,M2006_angle_PID,8000, 1000);//pid初始化 PID_init(&angle_pid,PID_POSITION,M2006_angle_PID,3000, 2000);//pid初始化
t_speedSet = 0; t_speedSet = 0;
result = 0; result = 0;
@ -45,10 +45,12 @@ Shoot::Shoot()
go1.mode = 1, go1.mode = 1,
go1.K_P = 1.0f, go1.K_P = 1.0f,
go1.K_W = 0.05, go1.K_W = 0.05,
go1.Pos = 25,//上电先到一个舒服的位置 go1.Pos = 0,//上电先到一个舒服的位置
go1.W = 0, go1.W = 0,
go1.T = 0, go1.T = 0,
fire_pos = INIT_POS; // 发射位置
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART1_RxCompleteCallback);//注册串口回调函数，bsp层 BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART1_RxCompleteCallback);//注册串口回调函数，bsp层
go1_data = get_GO_measure_point();//go1数据 go1_data = get_GO_measure_point();//go1数据
@ -133,6 +135,8 @@ void Shoot::rc_mode()
trigger_key=SHOOT; trigger_key=SHOOT;
} }
//knob_increment=rc_ctrl.ch[2]/150;
//旋钮增量 //旋钮增量
static int last_knob_value = 0; // 记录旋钮的上一次值 static int last_knob_value = 0; // 记录旋钮的上一次值
int current_knob_value = rc_ctrl.sw[4]; // 获取当前旋钮值 int current_knob_value = rc_ctrl.sw[4]; // 获取当前旋钮值
@ -149,8 +153,8 @@ void Shoot::rc_mode()
void Shoot::shoot_control() { void Shoot::shoot_control() {
//方便调试 //方便调试
fd_gopos = go1_data->Pos; feedback.fd_gopos = go1_data->Pos;
fd_tpos = trigger_Motor->total_angle; feedback.fd_tpos = trigger_Motor->total_angle;
switch (rc_key) { switch (rc_key) {
case DOWN1: case DOWN1:
@ -158,12 +162,13 @@ void Shoot::shoot_control() {
control_pos = INIT_POS; // 默认中间挡位位置 control_pos = INIT_POS; // 默认中间挡位位置
//fire_pos = control_pos + 10; // 发射位置（可调节） //fire_pos = control_pos + 10; // 发射位置（可调节）
fire_pos = control_pos + knob_increment; // 根据旋钮值调整发射位置 fire_pos = control_pos + knob_increment; // 根据旋钮值调整发射位置
//fire_pos +=knob_increment;
go1.Pos = fire_pos; // 设置蓄力电机位置 go1.Pos = fire_pos; // 设置蓄力电机位置
if (currentState == SHOOT_READY) { if (currentState == SHOOT_READY) {
// 如果当前状态是准备发射，松开钩子发射 // 如果当前状态是准备发射，松开钩子发射
t_posSet = Tigger_ZERO; // 扳机扣动 t_posSet = Tigger_ZERO; // 扳机扣动
if (t_posSet <= 50) { // 假设250度为发射完成角度 if (t_posSet >= 120) { // 假设120度为发射完成角度
currentState = SHOOT_IDLE; // 切换到空闲状态 currentState = SHOOT_IDLE; // 切换到空闲状态
is_hooked = false; // 重置钩子状态 is_hooked = false; // 重置钩子状态
} }
@ -200,14 +205,15 @@ void Shoot :: shoot_Current()
control_pos = TOP_POS; // 顶部位置 control_pos = TOP_POS; // 顶部位置
go1.Pos = control_pos; go1.Pos = control_pos;
t_posSet = Tigger_ZERO; // 扳机松开 t_posSet = Tigger_ZERO; // 扳机松开
if (go1_data->Pos >= control_pos - 0.5f && go1_data->Pos <= control_pos + 0.5f) { if (feedback.fd_gopos <-209) {
currentState = SHOOT_TOP; // 切换到准备发射状态 currentState = SHOOT_TOP; // 切换到准备发射状态
} }
break; break;
case SHOOT_TOP: case SHOOT_TOP:
t_posSet = Tigger_DO; // 扳机扣动钩住 t_posSet = Tigger_DO; // 扳机扣动钩住
if (fd_tpos >= Tigger_DO- 5.0f&&fd_tpos <= Tigger_DO+ 5.0f) osDelay(500); // 等待一段时间，确保扳机动作完成
if (fd_tpos <1)
{ {
//判定为钩住 //判定为钩住
is_hooked = true; // 标记钩子已钩住 is_hooked = true; // 标记钩子已钩住
@ -219,7 +225,7 @@ void Shoot :: shoot_Current()
if (is_hooked) if (is_hooked)
{ {
go1.Pos = fire_pos; // 下拉到中间挡位位置 go1.Pos = fire_pos; // 下拉到中间挡位位置
if (go1_data->Pos >= fire_pos - 0.5f && go1_data->Pos <= fire_pos + 0.5f) { if (feedback.fd_gopos >= fire_pos - 0.3f && feedback.fd_gopos <= fire_pos + 0.3f) {
//currentState = SHOOT_WAIT; // 等待发射信号 //currentState = SHOOT_WAIT; // 等待发射信号
//在拨杆切换时触发了 //在拨杆切换时触发了
} }
@ -251,7 +257,7 @@ void Shoot::RemoveError() {
currentState = SHOOT_IDLE; currentState = SHOOT_IDLE;
control_pos = TOP_POS; control_pos = TOP_POS;
go1.Pos = control_pos; go1.Pos = control_pos;
if(fd_gopos >= control_pos - 0.5f && fd_gopos <= control_pos + 0.5f) if(feedback.fd_gopos >= control_pos - 0.5f && feedback.fd_gopos <= control_pos + 0.5f)
{ {
t_posSet = Tigger_ZERO; t_posSet = Tigger_ZERO;
is_hooked = false; is_hooked = false;

10
User/module/shoot.hpp
View File

@ -58,8 +58,14 @@ public:
GO_MotorCmd_t go1; GO_MotorCmd_t go1;
GO_MotorData_t *go1_data;//存放go电机数据 GO_MotorData_t *go1_data;//存放go电机数据
float control_pos; //控制位置 float control_pos; //控制位置
float fire_pos; //发射位置 float fire_pos; //发射位置
float fd_gopos; struct feedback
{
float fd_gopos;
float fd_tpos;
}feedback;
//扳机 //扳机
float speed_trigger; float speed_trigger;

198
User/module/take.cpp
View File

@ -1,198 +0,0 @@
#include "TopDefine.h"
#include "take.hpp"
#include "remote_control.h"
#include "calc_lib.h"
extern RC_ctrl_t rc_ctrl;
const float Take::TakeRing_speed_PID[3]={10, 0 ,0}; //3508P,I,D(速度环)
const float Take::TakeRing_angle_PID[3]={10.0, 0 ,0}; //3508P,I,D(角度环)
const float Take::M3508_speed_PID[3]={12.0,0.01 ,0}; //两边上下pid
const float Take::M3508_angle_PID[3]={16, 0.0 ,0}; //3508P,I,D(角度环)
const float Take::Turn_speed_PID[3]={10,0.0,0}; //2006P,I,D(速度环)
const float Take::Turn_angle_PID[3]={14,0.0,0}; //2006P,I,D(角度环)
#define CURRENT_UP 1600
#define CURRENT_DOWN 700
#define CURRENT_TOP 2200
#define CURRENT_FALL 30
#define DEBUG_TAKE 0
int aaa=0;
Take::Take()
{
for(int i = 0;i < MOTOR_NUM;i ++)
{
putMotor[i] = get_motor_point(i);//获取电机数据指针
if(0 == i)
{
putMotor[i]->type = M3508;//设置电机类型
PID_init(&angle_pid[i],PID_POSITION,TakeRing_angle_PID,3000,1000);//pid初始化
PID_init(&speed_pid[i],PID_POSITION,TakeRing_speed_PID,7000,2000);//pid初始化
}
else if(1 == i)
{
putMotor[i]->type = M3508;//设置电机类型
PID_init(&angle_pid[i],PID_POSITION,Turn_angle_PID,700,0);//pid初始化
PID_init(&speed_pid[i],PID_POSITION,Turn_speed_PID,4000,1000);//pid初始化
}
else
{
putMotor[i]->type = M3508;//设置电机类型
PID_init(&angle_pid[i],PID_POSITION,M3508_angle_PID,800,0);//pid初始化
PID_init(&speed_pid[i],PID_POSITION,M3508_speed_PID,6000,1000);//pid初始化
}
result[i] = 0;
angleSet[i] = 0;
}
}
#if DEBUG_TAKE == 1
//int16_t current_fall = -30;
//void Take::fall()
//{
// if(putMotor[MOTOR_UP]->total_angle < -10)
// {result[MOTOR_UP] = current_fall;}
// else{
// int16_t delta[1];
// //下降
// angleSet[MOTOR_UP] = POINT_BUTTOM;
//
// delta[MOTOR_UP] = PID_calc(&angle_pid[MOTOR_UP],putMotor[MOTOR_UP]->total_angle,angleSet[MOTOR_UP]);
// result[MOTOR_UP] = PID_calc(&speed_pid[MOTOR_UP], putMotor[MOTOR_UP]->speed_rpm, delta[MOTOR_UP]);
// }
// // result[MOTOR_UP] = current_fall;
//}
#else
void Take::fall()
{
// if(putMotor[MOTOR_UP]->total_angle > -10)
// {result[MOTOR_UP] = -CURRENT_FALL;}
// else{
int16_t delta[1];
//下降
angleSet[MOTOR_UP] = aaa;
delta[MOTOR_UP] = PID_calc(&angle_pid[MOTOR_UP],putMotor[MOTOR_UP]->total_angle,angleSet[MOTOR_UP]);
result[MOTOR_UP] = PID_calc(&speed_pid[MOTOR_UP], putMotor[MOTOR_UP]->speed_rpm, delta[MOTOR_UP]);
// }
}
#endif
void Take::mid()
{
int16_t delta[1];
//下降
angleSet[MOTOR_UP] = POINT_MID;
delta[MOTOR_UP] = PID_calc(&angle_pid[MOTOR_UP],putMotor[MOTOR_UP]->total_angle,angleSet[MOTOR_UP]);
result[MOTOR_UP] = PID_calc(&speed_pid[MOTOR_UP], putMotor[MOTOR_UP]->speed_rpm, delta[MOTOR_UP]);
}
#if DEBUG_TAKE == 1
int16_t current_top = 1600;
void Take::top()
{
//int16_t delta[1];
//下降
angleSet[MOTOR_UP] = POINT_TOP;
// delta[MOTOR_UP] = PID_calc(&angle_pid[MOTOR_UP],putMotor[MOTOR_UP]->total_angle,angleSet[MOTOR_UP]);
// result[MOTOR_UP] = PID_calc(&speed_pid[MOTOR_UP], putMotor[MOTOR_UP]->speed_rpm, delta[MOTOR_UP]);
result[MOTOR_UP] = -current_top;
}
#else
void Take::top()
{
//int16_t delta[1];
//下降
angleSet[MOTOR_UP] = POINT_TOP;
// delta[MOTOR_UP] = PID_calc(&angle_pid[MOTOR_UP],putMotor[MOTOR_UP]->total_angle,angleSet[MOTOR_UP]);
// result[MOTOR_UP] = PID_calc(&speed_pid[MOTOR_UP], putMotor[MOTOR_UP]->speed_rpm, delta[MOTOR_UP]);
result[MOTOR_UP] = -CURRENT_TOP;
}
#endif
void Take::putRight()
{
// int16_t delta[1];
//下降
angleSet[MOTOR_RING_RIGHT] = POINT_PUT;
// delta[MOTOR_RING_RIGHT] = PID_calc(&angle_pid[MOTOR_RING_RIGHT],putMotor[MOTOR_RING_RIGHT]->total_angle,angleSet[MOTOR_RING_RIGHT]);
// result[MOTOR_RING_RIGHT] = PID_calc(&speed_pid[MOTOR_RING_RIGHT], putMotor[MOTOR_RING_RIGHT]->speed_rpm, delta[MOTOR_RING_RIGHT]);
result[MOTOR_RING_RIGHT] = CURRENT_DOWN-100;
}
void Take::putLeft()
{
// int16_t delta[1];
//下降
angleSet[MOTOR_RING_LEFT] = -POINT_PUT;
// delta[MOTOR_RING_LEFT] = PID_calc(&angle_pid[MOTOR_RING_LEFT],putMotor[MOTOR_RING_LEFT]->total_angle,angleSet[MOTOR_RING_LEFT]);
// result[MOTOR_RING_LEFT] = PID_calc(&speed_pid[MOTOR_RING_LEFT], putMotor[MOTOR_RING_LEFT]->speed_rpm, delta[MOTOR_RING_LEFT]);
result[MOTOR_RING_LEFT] = -CURRENT_DOWN;
}
void Take::takeRight()
{
// int16_t delta[1];
//下降
angleSet[MOTOR_RING_RIGHT] = POINT_TAKE;
// delta[MOTOR_RING_RIGHT] = PID_calc(&angle_pid[MOTOR_RING_RIGHT],putMotor[MOTOR_RING_RIGHT]->total_angle,angleSet[MOTOR_RING_RIGHT]);
// result[MOTOR_RING_RIGHT] = PID_calc(&speed_pid[MOTOR_RING_RIGHT], putMotor[MOTOR_RING_RIGHT]->speed_rpm, delta[MOTOR_RING_RIGHT]);
result[MOTOR_RING_RIGHT] = -CURRENT_UP-150;
}
void Take::takeLeft()
{
// int16_t delta[1];
//下降
angleSet[MOTOR_RING_LEFT] = -POINT_TAKE;
// delta[MOTOR_RING_LEFT] = PID_calc(&angle_pid[MOTOR_RING_LEFT],putMotor[MOTOR_RING_LEFT]->total_angle,angleSet[MOTOR_RING_LEFT]);
// result[MOTOR_RING_LEFT] = PID_calc(&speed_pid[MOTOR_RING_LEFT], putMotor[MOTOR_RING_LEFT]->speed_rpm, delta[MOTOR_RING_LEFT]);
result[MOTOR_RING_LEFT] = CURRENT_UP;
}
void Take::right()
{
int16_t delta[1];
//下降
angleSet[MOTOR_TURN] = POINT_TURN_RIGHT;
delta[MOTOR_TURN] = PID_calc(&angle_pid[MOTOR_TURN],putMotor[MOTOR_TURN]->total_angle,angleSet[MOTOR_TURN]);
result[MOTOR_TURN] = PID_calc(&speed_pid[MOTOR_TURN], putMotor[MOTOR_TURN]->speed_rpm, delta[MOTOR_TURN]);
}
void Take::left()
{
int16_t delta[1];
//下降
angleSet[MOTOR_TURN] = POINT_TURN_LEFT;
delta[MOTOR_TURN] = PID_calc(&angle_pid[MOTOR_TURN],putMotor[MOTOR_TURN]->total_angle,angleSet[MOTOR_TURN]);
result[MOTOR_TURN] = PID_calc(&speed_pid[MOTOR_TURN], putMotor[MOTOR_TURN]->speed_rpm, delta[MOTOR_TURN]);
}
void Take::turnMid()
{
int16_t delta[1];
//下降
angleSet[MOTOR_TURN] = POINT_TURN_ZERO;
delta[MOTOR_TURN] = PID_calc(&angle_pid[MOTOR_TURN],putMotor[MOTOR_TURN]->total_angle,angleSet[MOTOR_TURN]);
result[MOTOR_TURN] = PID_calc(&speed_pid[MOTOR_TURN], putMotor[MOTOR_TURN]->speed_rpm, delta[MOTOR_TURN]);
}
void Take::turnFlow()
{
int16_t delta[1];
//下降
angleSet[MOTOR_TURN] = map(rc_ctrl.sw[6],308,1694,POINT_TURN_LEFT-200,POINT_TURN_RIGHT+200);
delta[MOTOR_TURN] = PID_calc(&angle_pid[MOTOR_TURN],putMotor[MOTOR_TURN]->total_angle,angleSet[MOTOR_TURN]);
result[MOTOR_TURN] = PID_calc(&speed_pid[MOTOR_TURN], putMotor[MOTOR_TURN]->speed_rpm, delta[MOTOR_TURN]);
}

77
User/module/take.hpp
View File

@ -1,77 +0,0 @@
#ifndef TAKE_HPP
#define TAKE_HPP
#include "djiMotor.h"
#include "pid.h"
typedef enum
{
//上下3508顶端
POINT_TOP = -2280,
//上下3508暂停点
POINT_MID = -1720,
//上下3508取球点
POINT_BUTTOM = 0,
//前后2006取球点
POINT_TAKE = -730,
//前后2006放球点
POINT_PUT = 0,
//翻转3508放球点
POINT_TURN_LEFT = -350,
POINT_TURN_RIGHT = 350,
POINT_TURN_ZERO = 0,
//归零死区，没有柔性控制，防止堵转
POINT_DEAD = 5,
}point_e;//存放位置宏定义
typedef enum
{
MOTOR_UP = 0,
MOTOR_TURN = 1,
MOTOR_RING_RIGHT = 2,
MOTOR_RING_LEFT = 3,
MOTOR_NUM
}motorput_e;
class Take
{
public:
Take();
//下降到取球点
void fall();
void mid();//平常停在中间
void top();
void putRight();
void putLeft();
void takeRight();
void takeLeft();
void left();
void right();
void turnMid();
void turnFlow();
//暂存要发送的电流
int16_t result[MOTOR_NUM];
//电机状态
private:
motor_measure_t* putMotor[MOTOR_NUM];
//上下3508pid
static const float TakeRing_speed_PID[3];
static const float TakeRing_angle_PID[3];
//翻转3508pid
static const float M3508_speed_PID[3];
static const float M3508_angle_PID[3];
//前后与夹球2006pid
static const float Turn_speed_PID[3];
static const float Turn_angle_PID[3];
//电机速度pid结构体
pid_type_def speed_pid[MOTOR_NUM];
//位置环pid
pid_type_def angle_pid[MOTOR_NUM];
//暂存目标位置
//0上下，1翻转，2前后，3夹球
float angleSet[MOTOR_NUM];
};
#endif

7
User/task/ballTask.cpp
View File

@ -18,16 +18,11 @@ Ball ball;
int angle1=34; int angle1=34;
int angle2=23; int angle2=23;
//检查光电
int abc=0; int abc=0;
int a1=0;
int speed_set=0;
int speed_feedback=0;
extern int speedm; extern int speedm;
void FunctionBall(void *argument) void FunctionBall(void *argument)
{ {
(void)argument; /* 未使用argument，消除警告 */ (void)argument; /* 未使用argument，消除警告 */

4
User/task/shootTask.cpp
View File

@ -4,7 +4,6 @@
#include <cmsis_os2.h> #include <cmsis_os2.h>
#include "shootTask.hpp" #include "shootTask.hpp"
#include "shoot.hpp" #include "shoot.hpp"
#include "motor.hpp"
#include "remote_control.h" #include "remote_control.h"
#include "nuc.h" #include "nuc.h"
extern RC_ctrl_t rc_ctrl; extern RC_ctrl_t rc_ctrl;
@ -53,7 +52,8 @@ while(1)
//shoot.GO_SendData(goangle,5); //shoot.GO_SendData(goangle,5);
shoot.shoot_control(); shoot.shoot_control();
// shoot.t_posSet=goangle;
// shoot.trigger_control();
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
osDelayUntil(tick); osDelayUntil(tick);

40
User/task/takeTask.cpp
View File

@ -1,40 +0,0 @@
#include "TopDefine.h"
#include "FreeRTOS.h"
#include "userTask.h"
#include <cmsis_os2.h>
#include "takeTask.hpp"
#include "remote_control.h"
#include "take.hpp"
#include "motor.hpp"
extern RC_ctrl_t rc_ctrl;
Take take;
Trigger trigger;
int pos=1600;
void FunctionTake(void *argument)
{
//osDelay(2000);
while(1)
{
if(rc_ctrl.sw[4]==306)
{
trigger.triggerFlow(pos);
}
else if(rc_ctrl.sw[4]==1694)
{
//trigger.triggerZero();
trigger.result=0;
}
else if(rc_ctrl.sw[4]==0)
{
//trigger.triggerZero();
trigger.result=0;
}
CAN_cmd_200(0,trigger.result,0,0,&hcan1);
osDelay(1);
}
}

5
User/task/takeTask.hpp
View File

@ -1,5 +0,0 @@
#ifndef TAKETASK_HPP
#define TAKETASK_HPP
#endif