给go加位置限位

This commit is contained in:
ZHAISHUI04 2025-06-02 22:01:25 +08:00
parent 4c8a3945c4
commit bcfaac985f
8 changed files with 65 additions and 14 deletions

View File

@ -330,6 +330,21 @@
<WinNumber>1</WinNumber>
<ItemText>bmi088</ItemText>
</Ww>
<Ww>
<count>35</count>
<WinNumber>1</WinNumber>
<ItemText>up,0x0A</ItemText>
</Ww>
<Ww>
<count>36</count>
<WinNumber>1</WinNumber>
<ItemText>low,0x0A</ItemText>
</Ww>
<Ww>
<count>37</count>
<WinNumber>1</WinNumber>
<ItemText>NUC_send</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>

Binary file not shown.

View File

@ -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;

View File

@ -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

View File

@ -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;

View File

@ -117,6 +117,10 @@ static const ConfigParam_t param ={
.go1_release_threshold =-210,
.m2006_Screw_init=0,
.Pitch_angle =58,
.pull_speed =5,
},

View File

@ -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;
}//更改标志位

View File

@ -69,6 +69,8 @@ typedef struct {
fp32 m2006_Screw_init;
fp32 Pitch_angle;
fp32 pull_speed;
} PitchConfig_t;
/* 投球控制上下文 */