kohei matsumoto / Mbed 2 deprecated NHK_CenterArm

DRのPS3での操作用

Dependencies:   mbed CalPID motorout ros_lib_melodic MotorController_AbsEC

AMT21/AMT21.cpp@11:df2acf22123e, 2021-07-25 (annotated)

Committer:
koheim
Date:
Sun Jul 25 13:57:43 2021 +0000
Revision:
11:df2acf22123e
Parent:
0:83b2c6a67cce 
a; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ayu13 0:83b2c6a67cce 1 #include "mbed.h"
ayu13 0:83b2c6a67cce 2 #include "AMT21.h"
ayu13 0:83b2c6a67cce 3
ayu13 0:83b2c6a67cce 4 Amt21::Amt21(PinName tx,PinName rx,PinName mode):serial_(tx,rx),rs485_mode(mode)
ayu13 0:83b2c6a67cce 5 {
ayu13 0:83b2c6a67cce 6 receive_mode=1;
ayu13 0:83b2c6a67cce 7 serial_.baud(115200);
ayu13 0:83b2c6a67cce 8 serial_.format(8,Serial::None,1);
ayu13 0:83b2c6a67cce 9 rs485_mode=RECEIVE;
ayu13 0:83b2c6a67cce 10 serial_.attach(callback(this,&Amt21::receiveMessage),Serial::RxIrq);
ayu13 0:83b2c6a67cce 11 flow_count=0;
ayu13 0:83b2c6a67cce 12 th_min=DETECT_FLOW;
ayu13 0:83b2c6a67cce 13 th_max=RESOLUTION_AMT21-DETECT_FLOW;
ayu13 0:83b2c6a67cce 14 reset();
ayu13 0:83b2c6a67cce 15 timer_.reset();
ayu13 0:83b2c6a67cce 16 timer_.start();
ayu13 0:83b2c6a67cce 17 }
ayu13 0:83b2c6a67cce 18
ayu13 0:83b2c6a67cce 19 void Amt21::sendMessage()
ayu13 0:83b2c6a67cce 20 {
ayu13 0:83b2c6a67cce 21 rs485_mode=SEND;
ayu13 0:83b2c6a67cce 22 wait_us(3);
ayu13 0:83b2c6a67cce 23 serial_.putc(instruction);
ayu13 0:83b2c6a67cce 24 wait_us(96);//mbed LPC1768の場合
ayu13 0:83b2c6a67cce 25 //wait_us(85);Nucleo303の場合
ayu13 0:83b2c6a67cce 26 rs485_mode=RECEIVE;
ayu13 0:83b2c6a67cce 27 }
ayu13 0:83b2c6a67cce 28 void Amt21::receiveMessage()
ayu13 0:83b2c6a67cce 29 {
ayu13 0:83b2c6a67cce 30 if(receive_mode==0) {//count受信モード（下８ビット）
ayu13 0:83b2c6a67cce 31 low_count=serial_.getc();
ayu13 0:83b2c6a67cce 32 receive_mode++;
ayu13 0:83b2c6a67cce 33 } else if(receive_mode==1) {//count受信モード（上８ビット）
ayu13 0:83b2c6a67cce 34 high_count=serial_.getc();
ayu13 0:83b2c6a67cce 35 } else if(receive_mode==2) {//turn受信モード（下８ビット）
ayu13 0:83b2c6a67cce 36 low_turn=serial_.getc();
ayu13 0:83b2c6a67cce 37 receive_mode++;
ayu13 0:83b2c6a67cce 38 } else if(receive_mode==3) {//turn受信モード（上８ビット）
ayu13 0:83b2c6a67cce 39 high_turn=serial_.getc();
ayu13 0:83b2c6a67cce 40 }
ayu13 0:83b2c6a67cce 41 wait_us(2);
ayu13 0:83b2c6a67cce 42 }
ayu13 0:83b2c6a67cce 43 int Amt21::getAbCount()
ayu13 0:83b2c6a67cce 44 {
ayu13 0:83b2c6a67cce 45 instruction=GET_COUNT;
ayu13 0:83b2c6a67cce 46 receive_mode=RECEIVE_COUNT;
ayu13 0:83b2c6a67cce 47 sendMessage();
ayu13 0:83b2c6a67cce 48 wait_us(190);
ayu13 0:83b2c6a67cce 49 int count;
ayu13 0:83b2c6a67cce 50 count=((high_count&0b00111111)<<8)+low_count;
ayu13 0:83b2c6a67cce 51 return count;
ayu13 0:83b2c6a67cce 52 }
ayu13 0:83b2c6a67cce 53 int Amt21::getTurn()
ayu13 0:83b2c6a67cce 54 {
ayu13 0:83b2c6a67cce 55 instruction=GET_TURN;
ayu13 0:83b2c6a67cce 56 receive_mode=RECEIVE_TURN;
ayu13 0:83b2c6a67cce 57 sendMessage();
ayu13 0:83b2c6a67cce 58 wait_us(190);
ayu13 0:83b2c6a67cce 59 int turn,receive_;
ayu13 0:83b2c6a67cce 60 receive_=((high_turn&0b00111111)<<8)+low_turn;
ayu13 0:83b2c6a67cce 61 if(receive_>th_max&&receive_old<th_min) {//ターン数がアンダーフローしたとき
ayu13 0:83b2c6a67cce 62 flow_count--;
ayu13 0:83b2c6a67cce 63 } else if(receive_<th_min&&receive_old>th_max) { //ターン数がオーバーフローした場合
ayu13 0:83b2c6a67cce 64 flow_count++;
ayu13 0:83b2c6a67cce 65 }
ayu13 0:83b2c6a67cce 66 turn=receive_+flow_count*RESOLUTION_AMT21;
ayu13 0:83b2c6a67cce 67 receive_old=receive_;
ayu13 0:83b2c6a67cce 68 return turn;
ayu13 0:83b2c6a67cce 69 }
ayu13 0:83b2c6a67cce 70 void Amt21::rewriteCount()
ayu13 0:83b2c6a67cce 71 {
ayu13 0:83b2c6a67cce 72 int count,turn;
ayu13 0:83b2c6a67cce 73 count=getAbCount();
ayu13 0:83b2c6a67cce 74 wait_us(200);
ayu13 0:83b2c6a67cce 75 turn=getTurn();
ayu13 0:83b2c6a67cce 76 count_=count+RESOLUTION_AMT21*turn;
ayu13 0:83b2c6a67cce 77 }
ayu13 0:83b2c6a67cce 78 int Amt21::getCount()
ayu13 0:83b2c6a67cce 79 {
ayu13 0:83b2c6a67cce 80 return count_;
ayu13 0:83b2c6a67cce 81 }
ayu13 0:83b2c6a67cce 82 double Amt21::getDeg()
ayu13 0:83b2c6a67cce 83 {
ayu13 0:83b2c6a67cce 84 double angle_;
ayu13 0:83b2c6a67cce 85 angle_=(count_-COUNT_OFFSET)*360.0/RESOLUTION_AMT21;
ayu13 0:83b2c6a67cce 86 angle_/=GEER_STEER;
ayu13 0:83b2c6a67cce 87 return angle_;
ayu13 0:83b2c6a67cce 88 }
ayu13 0:83b2c6a67cce 89 double Amt21::getRad()
ayu13 0:83b2c6a67cce 90 {
ayu13 0:83b2c6a67cce 91 double angle_;
ayu13 0:83b2c6a67cce 92 angle_=(count_-COUNT_OFFSET)*2.0f*M_PI/RESOLUTION_AMT21;
ayu13 0:83b2c6a67cce 93 angle_/=GEER_STEER;
ayu13 0:83b2c6a67cce 94 return angle_;
ayu13 0:83b2c6a67cce 95 }
ayu13 0:83b2c6a67cce 96 void Amt21::reset()
ayu13 0:83b2c6a67cce 97 {
ayu13 0:83b2c6a67cce 98 instruction=RESET;
ayu13 0:83b2c6a67cce 99 sendMessage();
ayu13 0:83b2c6a67cce 100 instruction=0x75;
ayu13 0:83b2c6a67cce 101 sendMessage();
ayu13 0:83b2c6a67cce 102 wait_us(200);
ayu13 0:83b2c6a67cce 103 getTurn();//リセット前のバッファに残っているらしき値の消化
ayu13 0:83b2c6a67cce 104 getTurn();//
ayu13 0:83b2c6a67cce 105 flow_count=0;
ayu13 0:83b2c6a67cce 106 receive_old=0;
ayu13 0:83b2c6a67cce 107 // printf("reset Amt21\r\n");
ayu13 0:83b2c6a67cce 108 }
ayu13 0:83b2c6a67cce 109 void Amt21::calOmega()
ayu13 0:83b2c6a67cce 110 {
ayu13 0:83b2c6a67cce 111 double time_=timer_.read();
ayu13 0:83b2c6a67cce 112 omega_=(count_-pre_count)*2.0f*M_PI/(RESOLUTION_AMT21*(time_-pre_time));
ayu13 0:83b2c6a67cce 113 omega_/=GEER_STEER;
ayu13 0:83b2c6a67cce 114 pre_count=count_;
ayu13 0:83b2c6a67cce 115 pre_time=time_;
ayu13 0:83b2c6a67cce 116 }
ayu13 0:83b2c6a67cce 117 double Amt21::getOmega()
ayu13 0:83b2c6a67cce 118 {
ayu13 0:83b2c6a67cce 119 return omega_;
ayu13 0:83b2c6a67cce 120 }