diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index 71503df..929c293 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -26,9 +26,9 @@
0x4
ARM-ADS
- 12000000
+ 25000000
- 1
+ 0
1
0
1
@@ -77,7 +77,7 @@
0
1
- 18
+ 0
0
1
@@ -103,7 +103,7 @@
1
0
0
- 3
+ 0
@@ -114,39 +114,9 @@
- BIN\CMSIS_AGDI.dll
+ BIN\UL2CM3.DLL
-
- 0
- ST-LINKIII-KEIL_SWO
- -U00160029510000164E574E32 -O206 -SF1000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)
-
-
- 0
- ARMRTXEVENTFLAGS
- -L70 -Z18 -C0 -M0 -T1
-
-
- 0
- DLGTARM
- (1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0)
-
-
- 0
- ARMDBGFLAGS
-
-
-
- 0
- DLGUARM
-
-
-
- 0
- CMSIS_AGDI
- -X"Any" -UAny -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)
-
0
UL2CM3
@@ -206,13 +176,13 @@
0
- 1
- 1
+ 0
+ 0
0
0
0
0
- 1
+ 0
0
0
0
@@ -245,10 +215,10 @@
1
- 0
+ 1
0
2
- 1000000
+ 10000000
@@ -287,7 +257,7 @@
Application/User/Core
- 1
+ 0
0
0
0
@@ -427,7 +397,7 @@
Drivers/STM32F4xx_HAL_Driver
- 1
+ 0
0
0
0
@@ -699,7 +669,7 @@
Drivers/CMSIS
- 1
+ 0
0
0
0
@@ -719,7 +689,7 @@
Middlewares/FreeRTOS
- 1
+ 0
0
0
0
@@ -847,7 +817,7 @@
User/Module
- 1
+ 0
0
0
0
@@ -903,7 +873,7 @@
User/bsp
- 1
+ 0
0
0
0
@@ -1055,7 +1025,7 @@
User/Task
- 1
+ 0
0
0
0
@@ -1183,7 +1153,7 @@
User/Algorithm
- 1
+ 0
0
0
0
@@ -1263,7 +1233,7 @@
User/Device
- 1
+ 0
0
0
0
@@ -1355,7 +1325,7 @@
Application/User/USB_DEVICE/App
- 1
+ 0
0
0
0
@@ -1399,7 +1369,7 @@
Application/User/USB_DEVICE/Target
- 1
+ 0
0
0
0
@@ -1419,7 +1389,7 @@
Middlewares/USB_Device_Library
- 1
+ 0
0
0
0
diff --git a/MDK-ARM/R2.uvprojx b/MDK-ARM/R2.uvprojx
index 893edd4..64c3412 100644
--- a/MDK-ARM/R2.uvprojx
+++ b/MDK-ARM/R2.uvprojx
@@ -17,8 +17,8 @@
STM32F407IGHx
STMicroelectronics
- Keil.STM32F4xx_DFP.3.0.0
- https://www.keil.com/pack/
+ Keil.STM32F4xx_DFP.2.15.0
+ http://www.keil.com/pack/
IRAM(0x20000000-0x2001BFFF) IRAM2(0x2001C000-0x2001FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4") TZ
diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c
index 3469010..7ce6beb 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -72,19 +72,19 @@ int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_fre
c->sick_cali .sickparam=c->param ->sickparam ;
-
+
+ c->ang_cail.is_open=1;
return CHASSIS_OK;
}
-fp32 pian_yaw;
void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
fp64 Nor_Vx, Nor_Vy;
normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
- c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+pian_yaw; // 右前
- c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+pian_yaw; // 右后
- c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +pian_yaw; // 左后
- c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +pian_yaw; // 左前
+ c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+c->ang_cail.out; // 右前
+ c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+c->ang_cail.out; // 右后
+ c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +c->ang_cail.out; // 左后
+ c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +c->ang_cail.out; // 左前
Chassis_AngleCompensate(c);
@@ -148,8 +148,8 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
break;
case AUTO_MID360_Pitch:
- c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000;
- c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000;
+ c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
+ c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
@@ -174,8 +174,8 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
break ;
case PB_MID:
case PB_DOWN:
- c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000;
- c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000;
+ c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000;
+ c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
@@ -292,17 +292,21 @@ int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
int8_t Chassis_AngleCompensate(Chassis_t *c)
{
if (c == NULL) return CHASSIS_ERR_NULL;
+ if(c->ang_cail.is_open==0) return 0;
+
static fp32 pian_angel,cur_angle;
+
if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0))
{
- pian_angel=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
+ c->ang_cail.ang_error=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
+
}
else {
- cur_angle=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
- pian_angel=0;
+ c->ang_cail.ang_cur=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);
+ c->ang_cail.ang_error=0;
}
- pian_yaw = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0);
+ c->ang_cail.out = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0);
}
diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h
index 8a5c3ec..a5e410b 100644
--- a/User/Module/Chassis.h
+++ b/User/Module/Chassis.h
@@ -171,6 +171,16 @@ typedef struct{
extKalman_t extKalman[3];
LowPassFilter2p_t filled[8]; /* 输出滤波器滤波器数组 */
+ /*角度偏转修正 */
+ struct{
+
+ int is_open;
+ fp32 ang_error;
+ fp32 ang_cur;
+ fp32 out;
+
+
+ }ang_cail;
struct {
fp32 sick_dis[4]; //获取到的sick激光值
diff --git a/User/Module/config.c b/User/Module/config.c
index 2d56cc7..beea43a 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -80,7 +80,7 @@ static const ConfigParam_t param ={
.go_pull_pos = -214.0f, // go上升去合并扳机,扳机位置
.go_up_speed = 12.0f, // 上升速度
.go_down_speed = 6.0f, // 下降速度
- .Pitch_angle = 66, //俯仰角
+ .Pitch_angle = 58, //俯仰角
.go_init = -50
},
.PitchCfg = {
diff --git a/User/Module/up.c b/User/Module/up.c
index 752a80f..1120ddf 100644
--- a/User/Module/up.c
+++ b/User/Module/up.c
@@ -182,15 +182,10 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
/*部分数据更新*/
static int is_pitch=1;
posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3.4,4.2,&u->MID360Context.Curve);
-// if (u->PitchContext.Curve == CURVE_58) {
-// target->Pitch_angle = 58;
-// } else {
-// target->Pitch_angle = 66;
-// }
- u->vofa_send [2] = c->pos;
- u->vofa_send [3] = LowPassFilter2p_Apply(&u->filled[0],c->pos);
- u->vofa_send [4] =1;
-
+ u->vofa_send[0] = u->MID360Context.MID360Cfg.go_release_pos;
+ u->vofa_send[1] = u->motorfeedback.go_data.Pos;
+ u->vofa_send[2] = c->pos;
+
switch (c->CMD_CtrlType )
{
case RCcontrol: //在手动模式下
@@ -263,13 +258,23 @@ return 0;
//复用发射,
int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){
+ /*电机位置到达判断*/
+
+ bool is_GoStartReach=is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f); //go开始位置
+ bool is_GoSpeedReach=is_reached(u->motorfeedback.go_data.W,0,1.0f) ; //go速度归零判断
+ bool is_GoEndReach =(u->motorfeedback .go_data .Pos < -209); //go去上拉钩子位置,判断到达
+ bool is_HookDone=(u->motorfeedback .DJmotor_feedback [4].total_angle>-10); //2006钩子放下判断
+ bool is_Shoot=(u->motorfeedback.DJmotor_feedback[4].total_angle<-10);//是否发射判断
+
+
+
+
switch(LaunchContext->LaunchState){
case Launch_Stop: break;
case Launch_PREPARE:
u->motor_target.go_shoot = StartPos;
- if(is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f)&&
- is_reached(u->motorfeedback.go_data.W,0,1.0f)){
+ if(is_GoStartReach&& is_GoSpeedReach){
//根据位置和速度判断是否到达初始位置
LaunchContext->LaunchState = Launch_START;
@@ -278,7 +283,7 @@ int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartP
case Launch_START:
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_up_speed;
u->motor_target.go_shoot = u->LaunchContext.LaunchCfg.go_pull_pos;
- if(u->motorfeedback .go_data .Pos < -209){ //检测go位置到达最上面,这里的检测条件可以更改
+ if(is_GoEndReach){ //检测go位置到达最上面,这里的检测条件可以更改
u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_trig ;//设置2006角度,关闭
LaunchContext->LaunchState = Launch_TRIGGER;
@@ -286,7 +291,7 @@ int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartP
case Launch_TRIGGER:
- if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1){ //当2006的总角度小于1,可以认为已经勾上,误差为1
+ if( is_HookDone ){ //当2006的总角度小于1,可以认为已经勾上,误差为1
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_down_speed;
u->motor_target.go_shoot = EndPos ;
// if(is_reached(u->motorfeedback.go_data.Pos,EndPos,1.0f))
@@ -294,11 +299,10 @@ int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartP
} break;
case Launch_SHOOT_WAIT:
- if(u->motorfeedback.DJmotor_feedback[4].total_angle<-1) //认为发射
+ if(is_Shoot) //认为发射
LaunchContext->LaunchState = Launch_DONE;
break;
- case Launch_DONE:
- break ;
+ default:break;
}
}
@@ -360,7 +364,7 @@ int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
PassCfg ->go_release_pos =
- CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)+0.15,3,4,&u->PassContext.Curve);
+ CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PassContext.Curve);
switch (*state) { //遥控器按键进行状态切换
case PASS_STOP:
@@ -399,7 +403,7 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){
LaunchContext_t *LaunchContext = &u->LaunchContext;
MID360Context_t *MID360Context=&u->MID360Context;
MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg;
- MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)+0.15,3.2,4.3,&u->MID360Context.Curve);
+ MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.05,3.2,4.3,&u->MID360Context.Curve);
if (u->MID360Context.Curve == CURVE_58) {
target->Pitch_angle = 58;
} else {
diff --git a/User/Module/up_utils.c b/User/Module/up_utils.c
index b493bb4..a1e48f0 100644
--- a/User/Module/up_utils.c
+++ b/User/Module/up_utils.c
@@ -1,29 +1,32 @@
#include "up_utils.h"
#include "up.h"
-int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle)
+int8_t DJ_processdata(DJmotor_feedback_t *f, fp32 ecd_to_angle)
{
- int8_t cnt=0;
- fp32 angle ,delta;
-
- angle = f->ecd;
+ fp32 angle, delta;
+
+ angle = f->ecd;
+
+ // 初始化阶段,记录 offset
if (f->init_cnt < 50) {
- f->orig_angle= angle;
- f->last_angle = angle;
- f->init_cnt++;
- return 0;
+ f->offset_ecd = (uint16_t)angle; // 记录初始偏移
+ f->orig_angle = angle - f->offset_ecd; // orig_angle 归零
+ f->last_angle = angle - f->offset_ecd;
+ f->init_cnt++;
+ return 0;
}
-
-
- delta = angle - f->last_angle;
+
+ // 使用 offset 修正
+ angle = angle - f->offset_ecd;
+
+ delta = angle - f->last_angle;
if (delta > 4096) {
f->round_cnt--;
} else if (delta < -4096) {
f->round_cnt++;
}
- f->last_angle = angle;
- f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle;
-
+ f->last_angle = angle;
+ f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle;
}
/*go电机控制*/
@@ -65,13 +68,13 @@ int8_t GO_SendData( GO_MotorData_t *go_data,GO_MotorCmd_t *go_cmd,float pos, flo
// 计算66度曲线(偏上)
// 计算66度曲线(偏上)
static float curve_66(float d) {
- return 4.0310f * d * d + 8.9026f * d -139.5156;
-}
+ return 3.7028f * d * d + 11.2126f * d -142.9446f;
+ }
// 计算58度曲线(偏下)
// 计算58度曲线(偏下)
static float curve_58(float d) {
- return -1.9776f * d * d + 42.8499f * d - 204.2442f;
+ return 0.9242f * d * d + 19.4246f * d - 154.9055f;
}
/*
diff --git a/User/Module/up_utils.h b/User/Module/up_utils.h
index c29ab78..436e403 100644
--- a/User/Module/up_utils.h
+++ b/User/Module/up_utils.h
@@ -33,6 +33,8 @@ typedef struct
int32_t round_cnt;
int init_cnt;
fp32 total_angle;
+ uint16_t offset_ecd;
+ uint32_t msg_cnt;
}DJmotor_feedback_t;
diff --git a/User/device/cmd.h b/User/device/cmd.h
index a7835da..3b41b4f 100644
--- a/User/device/cmd.h
+++ b/User/device/cmd.h
@@ -43,7 +43,7 @@ typedef enum{
}CMD_mode_t;
typedef struct {
uint8_t status_fromnuc;
- uint8_t ctrl_status; //取其中每一个二进制位用作通信
+ uint8_t ctrl_status;
struct
{
fp32 vx;
@@ -52,6 +52,7 @@ typedef struct {
fp32 pos;
fp32 angle;
+
char flag;
}MID360;
diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c
index 3167ddb..5c0aa74 100644
--- a/User/task/nuc_task.c
+++ b/User/task/nuc_task.c
@@ -23,6 +23,8 @@ fp32 send[4]={11.0f,45.0,1.f,4.0f};
int a=0;
+
+
void Task_nuc(void *argument){
(void)argument; /**/
@@ -70,6 +72,9 @@ void Task_nuc(void *argument){
}
+
+ // NUC_StartSend(&nuc_raw, cmd_update);
+
tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
osDelayUntil(tick);
}