Diff: AMT21/AMT21.cpp

Revision:

0:83b2c6a67cce

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AMT21/AMT21.cpp	Thu Jul 08 03:46:29 2021 +0000
@@ -0,0 +1,120 @@
+#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_;
+}
\ No newline at end of file