シンイチ 田中 / AMT21_right_arm

9/10

AMT21.cpp@0:f3d3f744dbd9, 2020-12-10 (annotated)

Committer:
yuki0108
Date:
Thu Dec 10 06:41:13 2020 +0000
Revision:
0:f3d3f744dbd9
Child:
1:3838b5e3966c
AMT21 series

Who changed what in which revision?

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