シンイチ 田中 / AMT21_right_arm

9/10

AMT21.cpp@1:3838b5e3966c, 2021-04-12 (annotated)

Committer:
yuki0108
Date:
Mon Apr 12 06:08:22 2021 +0000
Revision:
1:3838b5e3966c
Parent:
0:f3d3f744dbd9
Child:
2:e0164b9fe93a
4.12

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 1:3838b5e3966c 79 double Amt21::getDeg()
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 1:3838b5e3966c 87 double Amt21::getRad()
yuki0108 1:3838b5e3966c 88 {
yuki0108 1:3838b5e3966c 89 getCount();
yuki0108 1:3838b5e3966c 90 double angle_;
yuki0108 1:3838b5e3966c 91 angle_=(count_-COUNT_OFFSET)*2.0f*M_PI/RESOLUTION_AMT21;
yuki0108 1:3838b5e3966c 92 angle_/=GEER_STEER;
yuki0108 1:3838b5e3966c 93 return angle_;
yuki0108 1:3838b5e3966c 94 }
yuki0108 0:f3d3f744dbd9 95 void Amt21::reset()
yuki0108 0:f3d3f744dbd9 96 {
yuki0108 0:f3d3f744dbd9 97 instruction=RESET;
yuki0108 0:f3d3f744dbd9 98 sendMessage();
yuki0108 0:f3d3f744dbd9 99 instruction=0x75;
yuki0108 0:f3d3f744dbd9 100 sendMessage();
yuki0108 0:f3d3f744dbd9 101 wait_us(200);
yuki0108 0:f3d3f744dbd9 102 getTurn();//リセット前のバッファに残っているらしき値の消化
yuki0108 0:f3d3f744dbd9 103 getTurn();//
yuki0108 0:f3d3f744dbd9 104 flow_count=0;
yuki0108 0:f3d3f744dbd9 105 receive_old=0;
yuki0108 0:f3d3f744dbd9 106 printf("reset Amt21\r\n");
yuki0108 0:f3d3f744dbd9 107 }
yuki0108 0:f3d3f744dbd9 108 void Amt21::calOmega()
yuki0108 0:f3d3f744dbd9 109 {
yuki0108 0:f3d3f744dbd9 110 double time_=timer_.read();
yuki0108 0:f3d3f744dbd9 111 omega_=(count_-pre_count)*2.0f*M_PI/(RESOLUTION_AMT21*(time_-pre_time));
yuki0108 0:f3d3f744dbd9 112 omega_/=GEER_STEER;
yuki0108 0:f3d3f744dbd9 113 pre_count=count_;
yuki0108 0:f3d3f744dbd9 114 pre_time=time_;
yuki0108 0:f3d3f744dbd9 115 }
yuki0108 0:f3d3f744dbd9 116 double Amt21::getOmega()
yuki0108 0:f3d3f744dbd9 117 {
yuki0108 0:f3d3f744dbd9 118 return omega_;
yuki0108 0:f3d3f744dbd9 119 }