可用
This commit is contained in:
parent
b2ac3ccd4d
commit
e531eb8d47
36
MDK-ARM/.vscode/c_cpp_properties.json
vendored
36
MDK-ARM/.vscode/c_cpp_properties.json
vendored
@ -3,26 +3,26 @@
|
||||
{
|
||||
"name": "R1",
|
||||
"includePath": [
|
||||
"d:\\Desktop\\r1\\r1_up\\Core\\Inc",
|
||||
"d:\\Desktop\\r1\\r1_up\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
|
||||
"d:\\Desktop\\r1\\r1_up\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
|
||||
"d:\\Desktop\\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_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_up\\Drivers\\CMSIS\\Include",
|
||||
"d:\\Desktop\\r1\\r1_up\\User\\bsp",
|
||||
"d:\\Desktop\\r1\\r1_up\\User\\module",
|
||||
"d:\\Desktop\\r1\\r1_up\\User\\task",
|
||||
"d:\\Desktop\\r1\\r1_up\\User\\lib",
|
||||
"d:\\Desktop\\r1\\r1_up\\User\\device",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\Core\\Inc",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\Drivers\\CMSIS\\Include",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\User\\bsp",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\User\\module",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\User\\task",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\User\\lib",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\User\\device",
|
||||
"D:\\keil\\ARM\\ARMCC\\include",
|
||||
"D:\\keil\\ARM\\ARMCC\\include\\rw",
|
||||
"d:\\Desktop\\r1\\r1_up\\MDK-ARM",
|
||||
"d:\\Desktop\\r1\\r1_up\\Core\\Src",
|
||||
"d:\\Desktop\\r1\\r1_up\\Drivers\\STM32F4xx_HAL_Driver\\Src",
|
||||
"d:\\Desktop\\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\\MDK-ARM",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\Core\\Src",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\Drivers\\STM32F4xx_HAL_Driver\\Src",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source",
|
||||
"d:\\Desktop\\r1\\R1\\R1_up\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
|
||||
],
|
||||
"defines": [
|
||||
"USE_HAL_DRIVER",
|
||||
|
||||
12
MDK-ARM/.vscode/keil-assistant.log
vendored
12
MDK-ARM/.vscode/keil-assistant.log
vendored
@ -8,3 +8,15 @@
|
||||
|
||||
[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
6
MDK-ARM/.vscode/uv4.log
vendored
@ -1,4 +1,8 @@
|
||||
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
|
||||
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).
|
||||
Build Time Elapsed: 00:00:02
|
||||
Build Time Elapsed: 00:00:06
|
||||
|
||||
2
MDK-ARM/.vscode/uv4.log.lock
vendored
2
MDK-ARM/.vscode/uv4.log.lock
vendored
@ -1 +1 @@
|
||||
2025/6/6 20:49:07
|
||||
2025/6/9 20:40:44
|
||||
@ -160,6 +160,21 @@
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>shoot,0x0A</ItemText>
|
||||
</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>
|
||||
<Tracepoint>
|
||||
<THDelay>0</THDelay>
|
||||
@ -919,7 +934,7 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>User/device</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1046,18 +1061,6 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<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>
|
||||
<FilenameWithoutPath>shoot.cpp</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
@ -1073,7 +1076,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>66</FileNumber>
|
||||
<FileNumber>65</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1085,7 +1088,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>67</FileNumber>
|
||||
<FileNumber>66</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1097,7 +1100,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>68</FileNumber>
|
||||
<FileNumber>67</FileNumber>
|
||||
<FileType>8</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1109,7 +1112,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>69</FileNumber>
|
||||
<FileNumber>68</FileNumber>
|
||||
<FileType>8</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1121,7 +1124,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>70</FileNumber>
|
||||
<FileNumber>69</FileNumber>
|
||||
<FileType>8</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1133,7 +1136,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>71</FileNumber>
|
||||
<FileNumber>70</FileNumber>
|
||||
<FileType>8</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
@ -1145,7 +1148,7 @@
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>10</GroupNumber>
|
||||
<FileNumber>72</FileNumber>
|
||||
<FileNumber>71</FileNumber>
|
||||
<FileType>8</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
|
||||
@ -738,11 +738,6 @@
|
||||
<FileType>8</FileType>
|
||||
<FilePath>..\User\module\gimbal.cpp</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>motor.cpp</FileName>
|
||||
<FileType>8</FileType>
|
||||
<FilePath>..\User\module\motor.cpp</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>shoot.cpp</FileName>
|
||||
<FileType>8</FileType>
|
||||
|
||||
2
R1.ioc
2
R1.ioc
@ -448,7 +448,7 @@ TIM10.Period=4999
|
||||
TIM4.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
|
||||
TIM4.IPParameters=Channel-PWM Generation3 CH3,Period
|
||||
TIM4.Period=20999
|
||||
USART1.BaudRate=115200
|
||||
USART1.BaudRate=4000000
|
||||
USART1.IPParameters=VirtualMode,BaudRate,Mode
|
||||
USART1.Mode=MODE_TX_RX
|
||||
USART1.VirtualMode=VM_ASYNC
|
||||
|
||||
20
README.md
20
README.md
@ -1,3 +1,23 @@
|
||||
# R1_up
|
||||
|
||||
r1上层
|
||||
|
||||
## 外设
|
||||
|
||||
+ CAN1
|
||||
- 扳机2006 id:0x205
|
||||
- 三摩擦 id:123
|
||||
+ UART
|
||||
- uart1 波特率4000000 id：2
|
||||
- uart6 nuc
|
||||
- uart3 遥控器接收
|
||||
+ GPIO
|
||||
- PI6运球光电
|
||||
- PE11 运球气缸
|
||||
|
||||
|
||||
|
||||
## 遥控器
|
||||
|
||||
|
||||
|
||||
|
||||
@ -93,12 +93,12 @@ int8_t NUC_RawParse(NUC_t *n) {
|
||||
z fp32
|
||||
0xFE TAIL
|
||||
*/
|
||||
if (nucbuf[15] != TAIL) goto error;
|
||||
if (nucbuf[7] != TAIL) goto error;
|
||||
|
||||
instance.data[3] = nucbuf[3];
|
||||
instance.data[2] = nucbuf[4];
|
||||
instance.data[1] = nucbuf[5];
|
||||
instance.data[0] = nucbuf[6];
|
||||
instance.data[3] = nucbuf[6];
|
||||
instance.data[2] = nucbuf[5];
|
||||
instance.data[1] = nucbuf[4];
|
||||
instance.data[0] = nucbuf[3];
|
||||
n->vision.x = instance.x[0];
|
||||
|
||||
instance.data[7] = nucbuf[7];
|
||||
|
||||
@ -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->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[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)
|
||||
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[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;
|
||||
|
||||
remote_ready = 1;
|
||||
|
||||
@ -65,19 +65,21 @@ Ball ::Ball()
|
||||
|
||||
}
|
||||
|
||||
void Ball ::Extend_mcontrol(int angle)
|
||||
void Ball ::Extend_mcontrol(int angle1,int angle2)
|
||||
{
|
||||
int16_t delta;
|
||||
angleSet[0] = angle/2;
|
||||
int16_t delta[2];
|
||||
angleSet[0] = angle1;
|
||||
angleSet[1] = angle2;
|
||||
|
||||
fp32 angle_get;
|
||||
angle_get=motor_angle_change(Extern_Motor[0]->real_round, angleSet[0]);
|
||||
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 = 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);
|
||||
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 ::Extend_control(int angle)
|
||||
{
|
||||
int16_t delta[2];
|
||||
|
||||
@ -55,7 +55,7 @@ public:
|
||||
void ballStop(void);
|
||||
void ballTake(void);
|
||||
void Extend_control(int angle);
|
||||
void Extend_mcontrol(int angle);
|
||||
void Extend_mcontrol(int angle1,int angle2);
|
||||
|
||||
BallState_t currentState1; // 当前状态
|
||||
int flag;//暂时还没用到
|
||||
|
||||
@ -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]);
|
||||
|
||||
|
||||
}
|
||||
@ -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
|
||||
@ -20,13 +20,13 @@ NUC_t nuc_v;
|
||||
//B键 sw[3]👆 200 开 👇1800 关
|
||||
//左旋钮 sw[4] 👈 200 👉1800
|
||||
|
||||
const fp32 Shoot:: M2006_speed_PID[3] = {10, 0, 0};
|
||||
const fp32 Shoot:: M2006_angle_PID[3] = { 10, 0, 0.1};
|
||||
const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0};
|
||||
const fp32 Shoot:: M2006_angle_PID[3] = { 25, 0, 0.1};
|
||||
|
||||
#define INIT_POS 50
|
||||
#define TOP_POS 100
|
||||
#define Tigger_DO 300
|
||||
#define Tigger_ZERO 0
|
||||
#define INIT_POS -100
|
||||
#define TOP_POS -210
|
||||
#define Tigger_DO -5
|
||||
#define Tigger_ZERO 130
|
||||
|
||||
float knob_increment;
|
||||
|
||||
@ -35,8 +35,8 @@ Shoot::Shoot()
|
||||
//扳机初始化
|
||||
trigger_Motor= get_motor_point(4);//id 205
|
||||
trigger_Motor->type=M2006;
|
||||
PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,8000, 1000);//pid初始化
|
||||
PID_init(&angle_pid,PID_POSITION,M2006_angle_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,3000, 2000);//pid初始化
|
||||
t_speedSet = 0;
|
||||
result = 0;
|
||||
|
||||
@ -45,10 +45,12 @@ Shoot::Shoot()
|
||||
go1.mode = 1,
|
||||
go1.K_P = 1.0f,
|
||||
go1.K_W = 0.05,
|
||||
go1.Pos = 25,//上电先到一个舒服的位置
|
||||
go1.Pos = 0,//上电先到一个舒服的位置
|
||||
go1.W = 0,
|
||||
go1.T = 0,
|
||||
|
||||
fire_pos = INIT_POS; // 发射位置
|
||||
|
||||
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART1_RxCompleteCallback);//注册串口回调函数，bsp层
|
||||
|
||||
go1_data = get_GO_measure_point();//go1数据
|
||||
@ -133,6 +135,8 @@ void Shoot::rc_mode()
|
||||
trigger_key=SHOOT;
|
||||
}
|
||||
|
||||
//knob_increment=rc_ctrl.ch[2]/150;
|
||||
|
||||
//旋钮增量
|
||||
static int last_knob_value = 0; // 记录旋钮的上一次值
|
||||
int current_knob_value = rc_ctrl.sw[4]; // 获取当前旋钮值
|
||||
@ -149,8 +153,8 @@ void Shoot::rc_mode()
|
||||
void Shoot::shoot_control() {
|
||||
|
||||
//方便调试
|
||||
fd_gopos = go1_data->Pos;
|
||||
fd_tpos = trigger_Motor->total_angle;
|
||||
feedback.fd_gopos = go1_data->Pos;
|
||||
feedback.fd_tpos = trigger_Motor->total_angle;
|
||||
|
||||
switch (rc_key) {
|
||||
case DOWN1:
|
||||
@ -158,12 +162,13 @@ void Shoot::shoot_control() {
|
||||
control_pos = INIT_POS; // 默认中间挡位位置
|
||||
//fire_pos = control_pos + 10; // 发射位置（可调节）
|
||||
fire_pos = control_pos + knob_increment; // 根据旋钮值调整发射位置
|
||||
//fire_pos +=knob_increment;
|
||||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||||
|
||||
if (currentState == SHOOT_READY) {
|
||||
// 如果当前状态是准备发射，松开钩子发射
|
||||
t_posSet = Tigger_ZERO; // 扳机扣动
|
||||
if (t_posSet <= 50) { // 假设250度为发射完成角度
|
||||
if (t_posSet >= 120) { // 假设120度为发射完成角度
|
||||
currentState = SHOOT_IDLE; // 切换到空闲状态
|
||||
is_hooked = false; // 重置钩子状态
|
||||
}
|
||||
@ -200,14 +205,15 @@ void Shoot :: shoot_Current()
|
||||
control_pos = TOP_POS; // 顶部位置
|
||||
go1.Pos = control_pos;
|
||||
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; // 切换到准备发射状态
|
||||
}
|
||||
break;
|
||||
|
||||
case SHOOT_TOP:
|
||||
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; // 标记钩子已钩住
|
||||
@ -219,7 +225,7 @@ void Shoot :: shoot_Current()
|
||||
if (is_hooked)
|
||||
{
|
||||
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; // 等待发射信号
|
||||
//在拨杆切换时触发了
|
||||
}
|
||||
@ -251,7 +257,7 @@ void Shoot::RemoveError() {
|
||||
currentState = SHOOT_IDLE;
|
||||
control_pos = TOP_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;
|
||||
is_hooked = false;
|
||||
|
||||
@ -58,8 +58,14 @@ public:
|
||||
GO_MotorCmd_t go1;
|
||||
GO_MotorData_t *go1_data;//存放go电机数据
|
||||
float control_pos; //控制位置
|
||||
float fire_pos; //发射位置
|
||||
float fd_gopos;
|
||||
float fire_pos; //发射位置
|
||||
struct feedback
|
||||
{
|
||||
float fd_gopos;
|
||||
float fd_tpos;
|
||||
}feedback;
|
||||
|
||||
|
||||
|
||||
//扳机
|
||||
float speed_trigger;
|
||||
|
||||
@ -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]);
|
||||
}
|
||||
@ -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
|
||||
@ -18,16 +18,11 @@ Ball ball;
|
||||
int angle1=34;
|
||||
int angle2=23;
|
||||
|
||||
//检查光电
|
||||
int abc=0;
|
||||
|
||||
int a1=0;
|
||||
|
||||
int speed_set=0;
|
||||
int speed_feedback=0;
|
||||
|
||||
extern int speedm;
|
||||
|
||||
|
||||
void FunctionBall(void *argument)
|
||||
{
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
@ -4,7 +4,6 @@
|
||||
#include <cmsis_os2.h>
|
||||
#include "shootTask.hpp"
|
||||
#include "shoot.hpp"
|
||||
#include "motor.hpp"
|
||||
#include "remote_control.h"
|
||||
#include "nuc.h"
|
||||
extern RC_ctrl_t rc_ctrl;
|
||||
@ -53,7 +52,8 @@ while(1)
|
||||
|
||||
//shoot.GO_SendData(goangle,5);
|
||||
shoot.shoot_control();
|
||||
|
||||
// shoot.t_posSet=goangle;
|
||||
// shoot.trigger_control();
|
||||
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
osDelayUntil(tick);
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
@ -1,5 +0,0 @@
|
||||
#ifndef TAKETASK_HPP
|
||||
#define TAKETASK_HPP
|
||||
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue
Block a user