Ejemplo n.º 1
0
void calcSpeedPID()
{
    static int leftCount,rightCount,aveCount;
    static int leftCountLast,rightCountLast,leftCountNew,rightCountNew;
    
    leftCountNew=FTM_QUAD_get(FTM1);
    leftCount=leftCountNew*1+leftCountLast*0;//0.6  0.4
    leftCountLast=leftCountNew;


    rightCountNew=-FTM_QUAD_get(FTM2);
    rightCount=rightCountNew*1+rightCountLast*0;//0.6  0.4
    rightCountLast=rightCountNew;

    aveCount=(leftCount+rightCount)/20;
    leftSpeedPID.err=runSpeed-aveCount;
    leftSpeedPID.adjust=leftSpeedPID.P*leftSpeedPID.err+leftSpeedPID.D*(leftSpeedPID.err-leftSpeedPID.err1);
    leftSpeedPID.err2=leftSpeedPID.err1;
    leftSpeedPID.err1=leftSpeedPID.err;

    rightSpeedPID.err=runSpeed-aveCount;
    rightSpeedPID.adjust=rightSpeedPID.P*rightSpeedPID.err+rightSpeedPID.D*(rightSpeedPID.err-rightSpeedPID.err1);
    rightSpeedPID.err2=rightSpeedPID.err1;
    rightSpeedPID.err1=rightSpeedPID.err;
    
    FTM_QUAD_clean(FTM1);
    FTM_QUAD_clean(FTM2);
}
Ejemplo n.º 2
0
void Quad_count()
{
    guad_val =  FTM_QUAD_get(FTM2);
    FTM_QUAD_clean(FTM2);

}