File content as of revision 0:4b99060621fd:

#include "encoder.h"

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

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

float Encoder::getState(int ch){
    float output;
    switch(ch){
        case DISTANCE:
            output = distance;
            break;
        case RPM:
            distance = 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=NOW;i<BEFORE_NUMBER;i++){
        count[NORMAL][i] = 0;
        count[DELTA][i]  = 0;   
    }
}

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

void Encoder::init(float _ppr, float _radius,float _dt){
    aPhase.rise(this,&Encoder::read);
    ppr     = _ppr;
    dt      = _dt;
    dt_square = dt*dt;
    circle  = PI*2.0f*_radius;
    radius = _radius;
}