金牌会员
 
- 积分
- 1498
- 威望
- 730
- 贡献
- 434
- 兑换币
- 456
- 注册时间
- 2015-10-23
- 在线时间
- 167 小时
|
void CalSpeedError(void)
{
static float fSpeedErrorTemp[5] = {0};
static int8 Flag_Exchange = 0;
switch(Flag_Exchange)
{
case 0:
{
g_nLeftSpeed = (g_nLeftPWM >= 0? tpm_pulse_get(TPM2) -tpm_pulse_get(TPM2)));
tpm_pulse_clean (TPM2);
tpm_pulse_init(TPM2,TPM_CLKIN1,TPM_PS_1); //初始化 TPM2 (PE30)为脉冲累加,右编码器
};break;
case 1:
{
g_nRighSpeed = (g_nRighPWM >= 0? tpm_pulse_get(TPM2):(-tpm_pulse_get(TPM2)));
tpm_pulse_clean (TPM2);
tpm_pulse_init(TPM2,TPM_CLKIN0,TPM_PS_1); //初始化 TPM2 (PB16)为脉冲累加,左编码器
fSpeedErrorTemp[4] = fSpeedErrorTemp[3];
fSpeedErrorTemp[3] = fSpeedErrorTemp[2];
fSpeedErrorTemp[2] = fSpeedErrorTemp[1];
fSpeedErrorTemp[1] = fSpeedErrorTemp[0];
fSpeedErrorTemp[0] = -(g_fExpectSpeed - ((g_nLeftSpeed + g_nRighSpeed)/2.0)*Ratio_Encoder);
g_fSpeedError = 0.5*fSpeedErrorTemp[0] + 0.2*fSpeedErrorTemp[1] + 0*fSpeedErrorTemp[2] + 0*fSpeedErrorTemp[3];
};break;
}
Flag_Exchange++; if(Flag_Exchange >= 2)Flag_Exchange = 0;
}
|
|