给go加位置限位
This commit is contained in:
parent
4c8a3945c4
commit
bcfaac985f
@ -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.
@ -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;
|
||||
|
@ -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
|
||||
|
@ -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;
|
||||
|
@ -117,6 +117,10 @@ static const ConfigParam_t param ={
|
||||
.go1_release_threshold =-210,
|
||||
.m2006_Screw_init=0,
|
||||
.Pitch_angle =58,
|
||||
|
||||
.pull_speed =5,
|
||||
|
||||
|
||||
},
|
||||
|
||||
|
||||
|
@ -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;
|
||||
}//更改标志位
|
||||
|
||||
|
@ -69,6 +69,8 @@ typedef struct {
|
||||
fp32 m2006_Screw_init;
|
||||
fp32 Pitch_angle;
|
||||
|
||||
fp32 pull_speed;
|
||||
|
||||
} PitchConfig_t;
|
||||
|
||||
/* 投球控制上下文 */
|
||||
|
Loading…
Reference in New Issue
Block a user