File content as of revision 5:32ee588c735d:

#include "encoder.h"

Encoder::Encoder(PinName a_phase , PinName b_phase ) : aPhase(a_phase) , bPhase(b_phase) {}

void Encoder::calculate(){
    //パルスの差分を更新
    count[DELTA][ONE_BEFORE] = count[NORMAL][ONE_BEFORE] - count[NORMAL][TWO_BEFORE];
    count[DELTA][CURRENT]    = count[NORMAL][CURRENT] - count[NORMAL][ONE_BEFORE];
    
    distance = count[NORMAL][CURRENT] * circle / ppr;
    rpm = (count[DELTA][CURRENT]/ppr) * 60 / dt;
    velocity = (count[DELTA][CURRENT]/ppr)*circle/dt;
    //acceleration = (count[DELTA][CURRENT]-count[DELTA][ONE_BEFORE])/(dt_square*ppr) * circle;
    //omega = (2*PI*count[DELTA][CURRENT])/(ppr*dt);
    delta_distance = (count[DELTA][CURRENT]/ppr)*circle;
    //1つ前・2つ前のデータを更新
    count[NORMAL][TWO_BEFORE] = count[NORMAL][ONE_BEFORE];
    count[NORMAL][ONE_BEFORE] = count[NORMAL][CURRENT];
}

float Encoder::getState(int ch){
    float output;
    switch(ch){
        case DISTANCE:
            output = distance;
            break;
        case RPM:
            output = rpm;
            break;
        case VELOCITY:
            output = velocity;
            break;
        case ACCELERATION:
            output = acceleration;
            break;
        case OMEGA:
            output = omega;
            break;
        case DELTA_DISTANCE:
            output = delta_distance;
            break;
    }
    return output;
}

void Encoder::reset(){
    for(int i=CURRENT;i<BEFORE_NUMBER;i++){
        count[NORMAL][i] = 0;
        count[DELTA][i]  = 0;   
    }
}

void Encoder::readAphaseRise(){
    if(bPhase){
        count[NORMAL][CURRENT]--; 
    } else {
        count[NORMAL][CURRENT]++; 
    }
}

void Encoder::readAphaseFall(){
    if(bPhase){
        count[NORMAL][CURRENT]++; 
    } else {
        count[NORMAL][CURRENT]--; 
    }
}

void Encoder::readBphaseRise(){
    if(aPhase){
        count[NORMAL][CURRENT]++; 
    } else {
        count[NORMAL][CURRENT]--; 
    }
}

void Encoder::readBphaseFall(){
    if(aPhase){
        count[NORMAL][CURRENT]--; 
    } else {
        count[NORMAL][CURRENT]++; 
    }
}


void Encoder::init(float _ppr, float _radius,float _dt,int mode = X1_ENCODE){
    ppr     = _ppr;
    dt      = _dt;
    dt_square = dt*dt;
    circle  = PI*2.0f*_radius;
    radius = _radius;
    
    switch(mode){
        case X1_ENCODE:
            aPhase.rise(this,&Encoder::readAphaseRise);
            ppr = _ppr;
            break;
        case X2_ENCODE:              
            aPhase.rise(this,&Encoder::readAphaseRise);
            aPhase.fall(this,&Encoder::readAphaseFall);
            ppr = _ppr*2;
            break;
        case X4_ENCODE:          
            aPhase.rise(this,&Encoder::readAphaseRise);
            aPhase.fall(this,&Encoder::readAphaseFall);
            bPhase.rise(this,&Encoder::readBphaseRise);
            bPhase.fall(this,&Encoder::readBphaseFall);
            ppr = _ppr*4;
            break;
    }
}