File content as of revision 11:df2acf22123e:

#include "mbed.h"
#include "AMT21.h"

Amt21::Amt21(PinName tx,PinName rx,PinName mode):serial_(tx,rx),rs485_mode(mode)
{
    receive_mode=1;
    serial_.baud(115200);
    serial_.format(8,Serial::None,1);
    rs485_mode=RECEIVE;
    serial_.attach(callback(this,&Amt21::receiveMessage),Serial::RxIrq);
    flow_count=0;
    th_min=DETECT_FLOW;
    th_max=RESOLUTION_AMT21-DETECT_FLOW;
    reset();
    timer_.reset();
    timer_.start();
}

void Amt21::sendMessage()
{
    rs485_mode=SEND;
    wait_us(3);
    serial_.putc(instruction);
    wait_us(96);//mbed LPC1768の場合
    //wait_us(85);Nucleo303の場合
    rs485_mode=RECEIVE;
}
void Amt21::receiveMessage()
{
    if(receive_mode==0) {//count受信モード（下８ビット）
        low_count=serial_.getc();
        receive_mode++;
    } else if(receive_mode==1) {//count受信モード（上８ビット）
        high_count=serial_.getc();
    } else if(receive_mode==2) {//turn受信モード（下８ビット）
        low_turn=serial_.getc();
        receive_mode++;
    } else if(receive_mode==3) {//turn受信モード（上８ビット）
        high_turn=serial_.getc();
    }
    wait_us(2);
}
int Amt21::getAbCount()
{
    instruction=GET_COUNT;
    receive_mode=RECEIVE_COUNT;
    sendMessage();
    wait_us(190);
    int count;
    count=((high_count&0b00111111)<<8)+low_count;
    return count;
}
int Amt21::getTurn()
{
    instruction=GET_TURN;
    receive_mode=RECEIVE_TURN;
    sendMessage();
    wait_us(190);
    int turn,receive_;
    receive_=((high_turn&0b00111111)<<8)+low_turn;
    if(receive_>th_max&&receive_old<th_min) {//ターン数がアンダーフローしたとき
        flow_count--;
    } else if(receive_<th_min&&receive_old>th_max) { //ターン数がオーバーフローした場合
        flow_count++;
    }
    turn=receive_+flow_count*RESOLUTION_AMT21;
    receive_old=receive_;
    return turn;
}
void Amt21::rewriteCount()
{
    int count,turn;
    count=getAbCount();
    wait_us(200);
    turn=getTurn();
    count_=count+RESOLUTION_AMT21*turn;
}
int Amt21::getCount()
{
    return count_;
}
double Amt21::getDeg()
{
    double angle_;
    angle_=(count_-COUNT_OFFSET)*360.0/RESOLUTION_AMT21;
    angle_/=GEER_STEER;
    return angle_;
}
double Amt21::getRad()
{
    double angle_;
    angle_=(count_-COUNT_OFFSET)*2.0f*M_PI/RESOLUTION_AMT21;
    angle_/=GEER_STEER;
    return angle_;
}
void Amt21::reset()
{
    instruction=RESET;
    sendMessage();
    instruction=0x75;
    sendMessage();
    wait_us(200);
    getTurn();//リセット前のバッファに残っているらしき値の消化
    getTurn();//
    flow_count=0;
    receive_old=0;
//    printf("reset Amt21\r\n");
}
void Amt21::calOmega()
{
    double time_=timer_.read();
    omega_=(count_-pre_count)*2.0f*M_PI/(RESOLUTION_AMT21*(time_-pre_time));
    omega_/=GEER_STEER;
    pre_count=count_;
    pre_time=time_;
}
double Amt21::getOmega()
{
    return omega_;
}