#define prd 10000
#define Vbreak 8000
u16 cnt2;
s32 CNT2;
s32 V2;
extern s32 rcnt2,rcnt3,rcnt4,rcnt5;
void get_encoder_ONE(void)//*******************计算当前实际速度
{
s32 CNT2_temp,CNT2_last;
cnt2 = TIM2 -> CNT;
CNT2_last = CNT2;
CNT2_temp = rcnt2 * prd + cnt2; //为什么要乘prd
V2 = CNT2_temp - CNT2_last;
while (V2>Vbreak)
{
rcnt2--;
CNT2_temp = rcnt2 * prd + cnt2;
V2 = CNT2_temp - CNT2_last;
}
while (V2<-Vbreak)
{
rcnt2++;
CNT2_temp = rcnt2 * prd + cnt2;
V2 = CNT2_temp - CNT2_last;
}
CNT2 = CNT2_temp;
}
|