From e531eb8d4792b98aa3b2ca58c1eee29f7a69f041 Mon Sep 17 00:00:00 2001 From: ws <1621320660@qq.com> Date: Thu, 12 Jun 2025 17:07:28 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=AF=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MDK-ARM/.vscode/c_cpp_properties.json | 36 ++--- MDK-ARM/.vscode/keil-assistant.log | 12 ++ MDK-ARM/.vscode/uv4.log | 6 +- MDK-ARM/.vscode/uv4.log.lock | 2 +- MDK-ARM/R1.uvoptx | 43 +++--- MDK-ARM/R1.uvprojx | 5 - R1.ioc | 2 +- README.md | 22 ++- User/device/nuc.c | 10 +- User/device/remote_control.c | 4 +- User/module/ball.cpp | 18 +-- User/module/ball.hpp | 2 +- User/module/motor.cpp | 42 ------ User/module/motor.hpp | 37 ----- User/module/shoot.cpp | 38 ++--- User/module/shoot.hpp | 10 +- User/module/take.cpp | 198 -------------------------- User/module/take.hpp | 77 ---------- User/task/ballTask.cpp | 7 +- User/task/shootTask.cpp | 4 +- User/task/takeTask.cpp | 40 ------ User/task/takeTask.hpp | 5 - 22 files changed, 133 insertions(+), 487 deletions(-) delete mode 100644 User/module/motor.cpp delete mode 100644 User/module/motor.hpp delete mode 100644 User/module/take.cpp delete mode 100644 User/module/take.hpp delete mode 100644 User/task/takeTask.cpp delete mode 100644 User/task/takeTask.hpp diff --git a/MDK-ARM/.vscode/c_cpp_properties.json b/MDK-ARM/.vscode/c_cpp_properties.json index 827c279..a07aace 100644 --- a/MDK-ARM/.vscode/c_cpp_properties.json +++ b/MDK-ARM/.vscode/c_cpp_properties.json @@ -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", diff --git a/MDK-ARM/.vscode/keil-assistant.log b/MDK-ARM/.vscode/keil-assistant.log index 25b195d..8e93176 100644 --- a/MDK-ARM/.vscode/keil-assistant.log +++ b/MDK-ARM/.vscode/keil-assistant.log @@ -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 + diff --git a/MDK-ARM/.vscode/uv4.log b/MDK-ARM/.vscode/uv4.log index ab2a269..2d011b3 100644 --- a/MDK-ARM/.vscode/uv4.log +++ b/MDK-ARM/.vscode/uv4.log @@ -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 diff --git a/MDK-ARM/.vscode/uv4.log.lock b/MDK-ARM/.vscode/uv4.log.lock index c91f820..5b7b866 100644 --- a/MDK-ARM/.vscode/uv4.log.lock +++ b/MDK-ARM/.vscode/uv4.log.lock @@ -1 +1 @@ -2025/6/6 20:49:07 \ No newline at end of file +2025/6/9 20:40:44 \ No newline at end of file diff --git a/MDK-ARM/R1.uvoptx b/MDK-ARM/R1.uvoptx index ccd422e..40f82a0 100644 --- a/MDK-ARM/R1.uvoptx +++ b/MDK-ARM/R1.uvoptx @@ -160,6 +160,21 @@ 1 shoot,0x0A + + 2 + 1 + cmd_fromnuc + + + 3 + 1 + nucbuf + + + 4 + 1 + goangle,0x0A + 0 @@ -919,7 +934,7 @@ User/device - 0 + 1 0 0 0 @@ -1046,18 +1061,6 @@ 0 0 0 - ..\User\module\motor.cpp - motor.cpp - 0 - 0 - - - 9 - 65 - 8 - 0 - 0 - 0 ..\User\module\shoot.cpp shoot.cpp 0 @@ -1073,7 +1076,7 @@ 0 10 - 66 + 65 1 0 0 @@ -1085,7 +1088,7 @@ 10 - 67 + 66 1 0 0 @@ -1097,7 +1100,7 @@ 10 - 68 + 67 8 0 0 @@ -1109,7 +1112,7 @@ 10 - 69 + 68 8 0 0 @@ -1121,7 +1124,7 @@ 10 - 70 + 69 8 0 0 @@ -1133,7 +1136,7 @@ 10 - 71 + 70 8 0 0 @@ -1145,7 +1148,7 @@ 10 - 72 + 71 8 0 0 diff --git a/MDK-ARM/R1.uvprojx b/MDK-ARM/R1.uvprojx index 911935d..7754386 100644 --- a/MDK-ARM/R1.uvprojx +++ b/MDK-ARM/R1.uvprojx @@ -738,11 +738,6 @@ 8 ..\User\module\gimbal.cpp - - motor.cpp - 8 - ..\User\module\motor.cpp - shoot.cpp 8 diff --git a/R1.ioc b/R1.ioc index 6109e5c..a000341 100644 --- a/R1.ioc +++ b/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 diff --git a/README.md b/README.md index 23d6a76..b0f5193 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,23 @@ # R1_up -r1上层 \ No newline at end of file +r1上层 + +## 外设 + ++ CAN1 + - 扳机2006 id:0x205 + - 三摩擦 id:123 ++ UART + - uart1 波特率4000000 id：2 + - uart6 nuc + - uart3 遥控器接收 ++ GPIO + - PI6运球光电 + - PE11 运球气缸 + + + +## 遥控器 + + + diff --git a/User/device/nuc.c b/User/device/nuc.c index 00db11a..4ef84b7 100644 --- a/User/device/nuc.c +++ b/User/device/nuc.c @@ -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]; diff --git a/User/device/remote_control.c b/User/device/remote_control.c index d9da51e..c035507 100644 --- a/User/device/remote_control.c +++ b/User/device/remote_control.c @@ -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; diff --git a/User/module/ball.cpp b/User/module/ball.cpp index bcc993b..8999f8f 100644 --- a/User/module/ball.cpp +++ b/User/module/ball.cpp @@ -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[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) { int16_t delta[2]; diff --git a/User/module/ball.hpp b/User/module/ball.hpp index 682cf54..2a2c756 100644 --- a/User/module/ball.hpp +++ b/User/module/ball.hpp @@ -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;//暂时还没用到 diff --git a/User/module/motor.cpp b/User/module/motor.cpp deleted file mode 100644 index 7f08f56..0000000 --- a/User/module/motor.cpp +++ /dev/null @@ -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]); - - -} diff --git a/User/module/motor.hpp b/User/module/motor.hpp deleted file mode 100644 index 5c122ec..0000000 --- a/User/module/motor.hpp +++ /dev/null @@ -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 diff --git a/User/module/shoot.cpp b/User/module/shoot.cpp index db9c8f3..942a070 100644 --- a/User/module/shoot.cpp +++ b/User/module/shoot.cpp @@ -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; diff --git a/User/module/shoot.hpp b/User/module/shoot.hpp index 7974010..54b4945 100644 --- a/User/module/shoot.hpp +++ b/User/module/shoot.hpp @@ -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; diff --git a/User/module/take.cpp b/User/module/take.cpp deleted file mode 100644 index 70435ca..0000000 --- a/User/module/take.cpp +++ /dev/null @@ -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]); -} diff --git a/User/module/take.hpp b/User/module/take.hpp deleted file mode 100644 index 616da3b..0000000 --- a/User/module/take.hpp +++ /dev/null @@ -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 diff --git a/User/task/ballTask.cpp b/User/task/ballTask.cpp index c3db914..1c30de0 100644 --- a/User/task/ballTask.cpp +++ b/User/task/ballTask.cpp @@ -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，消除警告 */ diff --git a/User/task/shootTask.cpp b/User/task/shootTask.cpp index 6ef9aa4..dd96ebf 100644 --- a/User/task/shootTask.cpp +++ b/User/task/shootTask.cpp @@ -4,7 +4,6 @@ #include #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); diff --git a/User/task/takeTask.cpp b/User/task/takeTask.cpp deleted file mode 100644 index fb6e92e..0000000 --- a/User/task/takeTask.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include "TopDefine.h" -#include "FreeRTOS.h" -#include "userTask.h" -#include -#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); - } -} diff --git a/User/task/takeTask.hpp b/User/task/takeTask.hpp deleted file mode 100644 index d103937..0000000 --- a/User/task/takeTask.hpp +++ /dev/null @@ -1,5 +0,0 @@ -#ifndef TAKETASK_HPP -#define TAKETASK_HPP - - -#endif