File content as of revision 0:3d9101dd0061:

#include "mbed.h"
#include "EC.h"

int Ec::defsolution;
double Ec::deftime;
//ピン変化割り込み関数の定義
void Ec::upA()
{
    stateA=1;
    if(stateB==0&&S==0) {
        S=1;
    } else if(stateB==1&&S==3) {
        S=2;
    }
}
void Ec::downA()
{
    stateA=0;
    if(stateB==1&&S==2) {
        S=3;
    } else if(stateB==0&&S==1) {
        S=0;
        count--;
    }
}
void Ec::upB()
{
    stateB=1;
    if(stateA==1&&S==1) {
        S=2;
    } else if(stateA==0&&S==0) {
        S=3;
    }
}
void Ec::downB()
{
    stateB=0;
    if(stateA==0&&S==3) {
        count++;
        S=0;
    } else if(stateA==1&&S==2) {
        S=1;
    }
}

void Ec::upZ()
{
    if(first==false) {
        static int res_count;
        rev++;
        first=true;
        now_time=timer.read();
        RPM=60/(now_time-old_time);
        if((RPM_old-RPM)>RPM_th) {
            if(res_count >= 5) {
                printf("\r\n CAUTION : speed downed drastically\r\n");
                NVIC_SystemReset();
                res_count=0;
            } else {
                res_count++;
            }
        } else {
            res_count=0;
        }
        RPM_old=RPM;
        old_time=timer.read();
    }
}

void Ec::downZ()
{
    first=false;
}


//コンストラクタの定義
//main関数の前に必ず一度宣言する
//第一・第二引数はエンコーダのA・B相のピン名
//第三院数はエンコーダの分解能

Ec::Ec(PinName signalA , PinName signalB , PinName signalZ , int s=defsolution , double t=deftime) : signalA_(signalA),signalB_(signalB),signalZ_(signalZ)
{

    if((signalA!=NC)&&(signalB!=NC)) {
        S=0;
        stateA=0;
        stateB=0;
        count=0;
        pre_count=0.0;
        timer.start();
        signalA_.rise(callback(this,&Ec::upA));
        signalA_.fall(callback(this,&Ec::downA));
        signalB_.rise(callback(this,&Ec::upB));
        signalB_.fall(callback(this,&Ec::downB));
    }
    if(signalZ!=NC) {
        first=false;
        rev=0;
        old_time=0;
        RPM=0;
        RPM_old=0;
        signalZ_.rise(callback(this,&Ec::upZ));
        signalZ_.fall(callback(this,&Ec::downZ));
    }
    dt=t;
    solution=s;
    defsolution=s;
    RPM_th=250;
    count_to_distance_mm_ = 0;
    gear_rate_ = 1;
    is_diameter_set = 0;
}

int Ec::getCount()
{
    return count;
}

void Ec::CalOmega()
{
    omega=(count-pre_count)*2*M_pi/(solution*dt);
    pre_count=count;
}

double Ec::getOmega()
{
    return omega;
}

double Ec::getPreCount()
{
    precount=count+S/4.0;
    return precount;
}
/*reset関数の定義*/
/*エンコーダを初期状態に戻すことができる*/
void Ec::reset()
{
    S=0;
    stateA=0;
    stateB=0;
    count=0;
    pre_count=0.0,omega=0;
    rev=0;
    now_time=0;
    old_time=0;
    RPM=0;
    RPM_old=0;
}
/*setTime関数の定義*/
/*自分で好きなように角速度計算の間隔を決めることができる（デフォルトは0.05秒）*/
void Ec::setTime(double t)
{
    dt=t;
}

double Ec::getRPM()
{
    return RPM;
}

int Ec::getRev()
{
    return rev;
}

void Ec::changeRPM_th(int th)
{
    RPM_th=th;
}

double Ec::getDistance_mm()
{
    if(is_diameter_set == 0) printf("please set diameter\r\n");
    distance_mm_= count * count_to_distance_mm_;
    return distance_mm_;
}
void Ec::setDiameter_mm(double diameter_mm)
{
    diameter_mm_ = diameter_mm;
    setCountToDistance_mm();
    is_diameter_set = 1;
}
void Ec::setGearRate(double gear_rate){
    gear_rate_ = gear_rate;
    setCountToDistance_mm();
}
void Ec::setCountToDistance_mm(){
    count_to_distance_mm_ = 1 / (double)solution * M_pi * diameter_mm_ * gear_rate_;//カウント分解能＊2Pi=角度（rad)。角度*直径/2  距離を整理した式
}