File content as of revision 2:4bb7c8730d91:

#include "encoder.h"

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

void Encoder::calState(){
    //パルスの差分を更新
    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:
            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=CURRENT;i<BEFORE_NUMBER;i++){
        count[NORMAL][i] = 0;
        count[DELTA][i]  = 0;   
    }
}

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

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;
}