diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index 28b9eeb..17444bc 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -330,6 +330,21 @@
1
bmi088
+
+ 35
+ 1
+ up,0x0A
+
+
+ 36
+ 1
+ low,0x0A
+
+
+ 37
+ 1
+ NUC_send
+
0
diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf
index 437d937..5971779 100644
Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ
diff --git a/User/Algorithm/user_math.c b/User/Algorithm/user_math.c
index ccce2e5..b0eb3ad 100644
--- a/User/Algorithm/user_math.c
+++ b/User/Algorithm/user_math.c
@@ -216,7 +216,21 @@ void abs_limit_fp(fp32 *num, fp32 Limit)
*num = -Limit;
}
}
-
+//改原始值，限制最大最小
+fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max)
+{
+ if (*num > Limit_max)
+ {
+ *num=Limit_max;
+ return Limit_max;
+ }
+ else if (*num < Limit_min)
+ {
+ *num=Limit_min;
+ return Limit_min;
+
+ }
+}
/* 移动向量 */
MoveVector_t *mv;
diff --git a/User/Algorithm/user_math.h b/User/Algorithm/user_math.h
index dd4d8ab..0dad6dd 100644
--- a/User/Algorithm/user_math.h
+++ b/User/Algorithm/user_math.h
@@ -164,5 +164,6 @@ int read_flag_state(uint8_t flag) ;
void normalize_vector(double x, double y, double *out_x, double *out_y) ;
bool is_reached(float current, float target, float mistake) ;
bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) ;
+fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max);
#endif
diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c
index aa24b3d..9d3fea9 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -63,7 +63,7 @@ void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw; // 左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw; // 左前
}
-
+float aaaaa=0;
int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
if (c == NULL) return CHASSIS_ERR_NULL;
if (ctrl == NULL) return CHASSIS_ERR_NULL;
@@ -86,9 +86,22 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
switch (c->chassis_ctrl.mode) {
case Normal:
+
+ if(ctrl->Vw){
c->move_vec.Vw = ctrl->Vw * 6000;
c->move_vec.Vx = ctrl->Vy * 6000;
c->move_vec.Vy = ctrl->Vx * 6000;
+ }
+ else
+ {
+ c->move_vec.Vw = ctrl->Vw * 6000 + c->pos088 .bmi088 .gyro .z * aaaaa;
+ c->move_vec.Vx = ctrl->Vy * 6000 + c->pos088 .bmi088 .gyro .z * aaaaa ;
+ c->move_vec.Vy = ctrl->Vx * 6000 + c->pos088 .bmi088 .gyro .z * aaaaa ;
+
+
+
+ }
+
break;
case Pitch:
c->move_vec.Vw = 0;
@@ -117,9 +130,9 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
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, 2000.0f);
- abs_limit_fp(&c->move_vec.Vy, 2000.0f);
- abs_limit_fp(&c->move_vec.Vw, 2000.0f);
+ abs_limit_fp(&c->move_vec.Vx, 5000.0f);
+ abs_limit_fp(&c->move_vec.Vy, 5000.0f);
+ abs_limit_fp(&c->move_vec.Vw, 5000.0f);
c->NUC_send .send [0]=0;
break;
diff --git a/User/Module/config.c b/User/Module/config.c
index 93311b2..bbe561f 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -117,6 +117,10 @@ static const ConfigParam_t param ={
.go1_release_threshold =-210,
.m2006_Screw_init=0,
.Pitch_angle =58,
+
+ .pull_speed =5,
+
+
},
diff --git a/User/Module/up.c b/User/Module/up.c
index 50969eb..57e024d 100644
--- a/User/Module/up.c
+++ b/User/Module/up.c
@@ -230,8 +230,10 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
(PID_calc (&(u->pid .Pitch_M2006_angle ),
u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle))
);
-
- GO_SendData(u,u->motor_target .go_shoot,5 );
+
+ GO_SendData(u,
+ u->motor_target .go_shoot,
+ u->PitchContext .PitchConfig .pull_speed );//对应的拉动速度
for(int i=0;i<4;i++){
@@ -259,12 +261,14 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
if(u ==NULL) return 0;
static int is_pitch=1;
-
+ /*config值限位*/
+ abs_limit_min_max_fp(&u->PitchContext.PitchConfig.go1_Receive_ball ,-208.0f,2.0f);
+
switch (c->CMD_CtrlType )
{
case RCcontrol: //在手动模式下
-
+
switch (c-> CMD_mode )
{
@@ -306,7 +310,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
break ;
case UP_Adjust: //测试用
-
+ u->PitchContext .PitchConfig .pull_speed=5;
u->PitchContext.PitchConfig.go1_Receive_ball += c->Vx ;
u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100;
@@ -372,8 +376,6 @@ return 0;
-
-
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
{
@@ -381,12 +383,12 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
case PITCH_START:
// u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
-
+ u->PitchContext .PitchConfig .pull_speed=10;
u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
u->motor_target .Shoot_M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
-
+ u->PitchContext .PitchConfig .pull_speed=6;
u->PitchContext .PitchState=PITCH_PULL_TRIGGER;
}//更改标志位
diff --git a/User/Module/up.h b/User/Module/up.h
index d915ebf..5b5711e 100644
--- a/User/Module/up.h
+++ b/User/Module/up.h
@@ -69,6 +69,8 @@ typedef struct {
fp32 m2006_Screw_init;
fp32 Pitch_angle;
+ fp32 pull_speed;
+
} PitchConfig_t;
/* 投球控制上下文 */