encoder.c

dspic30f6010a的学习事例程序

 /**********************************************************************
 *                                                                     *
 *                        Software License Agreement                   *
 *                                                                     *
 *    The software supplied herewith by Microchip Technology           *
 *    Incorporated (the "Company") for its dsPIC controller            *
 *    is intended and supplied to you, the Company's customer,         *
 *    for use solely and exclusively on Microchip dsPIC                *
 *    products. The software is owned by the Company and/or its        *
 *    supplier, and is protected under applicable copyright laws. All  *
 *    rights are reserved. Any use in violation of the foregoing       *
 *    restrictions may subject the user to criminal sanctions under    *
 *    applicable laws, as well as to civil liability for the breach of *
 *    the terms and conditions of this license.                        *
 *                                                                     *
 *    THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION.  NO           *
 *    WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING,    *
 *    BUT NOT LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND    *
 *    FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE     *
 *    COMPANY SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL,  *
 *    INCIDENTAL OR CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.  *
 *                                                                     *
  **********************************************************************/
// Scaling for encoder routines

#include "general.h"
#include "Parms.h"
#include "Encoder.h"

/**********************************************************
InitEncoderScaling

Initialize scaling constants for encoder rotuines.

    Arguments:
        CntsPerRev: Encoder counts per revolution from quadrature
        ScalingSpeedInRPS: Rev per sec used for basic velocity scaling
        IrpPerCalc: Number of CalcVelIrp interrupts per velocity calculation
        VelIrpPeriod: Period between VelCalcIrp interrupts (in Sec)

For CalcAng:
    Runtime equation:
    qMechAng = qKang * (POSCNT*4) / 2^Nang

    Scaling equations:
        qKang = (2^15)*(2^Nang)/CntsPerRev.

For CalcVelIrp, CalcVel:
    Runtime equation:
        qMechVel = qKvel * (2^15 * Delta / 2^Nvel)

    Scaling equations:
        fVelCalcPeriod = fVelIrpPeriod * iIrpPerCalc
        MaxCntRate = CntsPerRev * ScaleMechRPS
        MaxDeltaCnt = fVelCalcPeriod * MaxCntRate
        qKvel = (2^15)*(2^Nvel)/MaxDeltaCnt
**********************************************************/

bool InitEncoderScaling( void )
{
    float fVelCalcPeriod, fMaxCntRate;
    long  MaxDeltaCnt;
    long  K;

    EncoderParm.iCntsPerRev = MotorParm.iCntsPerRev;

    K = 32768;
    K *= 1 << Nang;
    EncoderParm.qKang = K/EncoderParm.iCntsPerRev;


    EncoderParm.iIrpPerCalc = MotorParm.iIrpPerCalc;
    fVelCalcPeriod = MotorParm.fVelIrpPeriod * MotorParm.iIrpPerCalc;
    fMaxCntRate = EncoderParm.iCntsPerRev * MotorParm.fScaleMechRPS;
    MaxDeltaCnt = fVelCalcPeriod * fMaxCntRate;

    // qKvel = (2^15)*(2^Nvel)/MaxDeltaCnt
    K = 32768;
    K *= 1 << Nvel;
    K /= MaxDeltaCnt;
    if( K >= 32768 )
        // Error
        return True;
    EncoderParm.qKvel = K;

    // Initialize private variables used by CalcVelIrp.
    InitCalcVel();
    return False;
}