シンイチ 田中 / AMT21_right_arm

9/10

AMT21.cpp@2:e0164b9fe93a, 2021-04-13 (annotated)

Committer:
yuki0108
Date:
Tue Apr 13 16:50:54 2021 +0000
Revision:
2:e0164b9fe93a
Parent:
1:3838b5e3966c
Child:
5:b78f031b66f7
4.14

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 2:e0164b9fe93a 41 wait_us(2);
yuki0108 0:f3d3f744dbd9 42 }
yuki0108 0:f3d3f744dbd9 43 int Amt21::getAbCount()
yuki0108 0:f3d3f744dbd9 44 {
yuki0108 0:f3d3f744dbd9 45 instruction=GET_COUNT;
yuki0108 0:f3d3f744dbd9 46 receive_mode=RECEIVE_COUNT;
yuki0108 0:f3d3f744dbd9 47 sendMessage();
yuki0108 0:f3d3f744dbd9 48 wait_us(190);
yuki0108 0:f3d3f744dbd9 49 int count;
yuki0108 0:f3d3f744dbd9 50 count=((high_count&0b00111111)<<8)+low_count;
yuki0108 0:f3d3f744dbd9 51 return count;
yuki0108 0:f3d3f744dbd9 52 }
yuki0108 0:f3d3f744dbd9 53 int Amt21::getTurn()
yuki0108 0:f3d3f744dbd9 54 {
yuki0108 0:f3d3f744dbd9 55 instruction=GET_TURN;
yuki0108 0:f3d3f744dbd9 56 receive_mode=RECEIVE_TURN;
yuki0108 0:f3d3f744dbd9 57 sendMessage();
yuki0108 0:f3d3f744dbd9 58 wait_us(190);
yuki0108 0:f3d3f744dbd9 59 int turn,receive_;
yuki0108 0:f3d3f744dbd9 60 receive_=((high_turn&0b00111111)<<8)+low_turn;
yuki0108 0:f3d3f744dbd9 61 if(receive_>th_max&&receive_old<th_min) {//ターン数がアンダーフローしたとき
yuki0108 0:f3d3f744dbd9 62 flow_count--;
yuki0108 0:f3d3f744dbd9 63 } else if(receive_<th_min&&receive_old>th_max) { //ターン数がオーバーフローした場合
yuki0108 0:f3d3f744dbd9 64 flow_count++;
yuki0108 0:f3d3f744dbd9 65 }
yuki0108 0:f3d3f744dbd9 66 turn=receive_+flow_count*RESOLUTION_AMT21;
yuki0108 0:f3d3f744dbd9 67 receive_old=receive_;
yuki0108 0:f3d3f744dbd9 68 return turn;
yuki0108 0:f3d3f744dbd9 69 }
yuki0108 2:e0164b9fe93a 70 void Amt21::rewriteCount()
yuki0108 0:f3d3f744dbd9 71 {
yuki0108 0:f3d3f744dbd9 72 int count,turn;
yuki0108 0:f3d3f744dbd9 73 count=getAbCount();
yuki0108 2:e0164b9fe93a 74 wait_us(200);
yuki0108 0:f3d3f744dbd9 75 turn=getTurn();
yuki0108 0:f3d3f744dbd9 76 count_=count+RESOLUTION_AMT21*turn;
yuki0108 2:e0164b9fe93a 77 }
yuki0108 2:e0164b9fe93a 78 int Amt21::getCount()
yuki0108 2:e0164b9fe93a 79 {
yuki0108 0:f3d3f744dbd9 80 return count_;
yuki0108 0:f3d3f744dbd9 81 }
yuki0108 1:3838b5e3966c 82 double Amt21::getDeg()
yuki0108 0:f3d3f744dbd9 83 {
yuki0108 0:f3d3f744dbd9 84 double angle_;
yuki0108 0:f3d3f744dbd9 85 angle_=(count_-COUNT_OFFSET)*360.0/RESOLUTION_AMT21;
yuki0108 0:f3d3f744dbd9 86 angle_/=GEER_STEER;
yuki0108 0:f3d3f744dbd9 87 return angle_;
yuki0108 0:f3d3f744dbd9 88 }
yuki0108 1:3838b5e3966c 89 double Amt21::getRad()
yuki0108 1:3838b5e3966c 90 {
yuki0108 1:3838b5e3966c 91 double angle_;
yuki0108 1:3838b5e3966c 92 angle_=(count_-COUNT_OFFSET)*2.0f*M_PI/RESOLUTION_AMT21;
yuki0108 1:3838b5e3966c 93 angle_/=GEER_STEER;
yuki0108 1:3838b5e3966c 94 return angle_;
yuki0108 1:3838b5e3966c 95 }
yuki0108 0:f3d3f744dbd9 96 void Amt21::reset()
yuki0108 0:f3d3f744dbd9 97 {
yuki0108 0:f3d3f744dbd9 98 instruction=RESET;
yuki0108 0:f3d3f744dbd9 99 sendMessage();
yuki0108 0:f3d3f744dbd9 100 instruction=0x75;
yuki0108 0:f3d3f744dbd9 101 sendMessage();
yuki0108 0:f3d3f744dbd9 102 wait_us(200);
yuki0108 0:f3d3f744dbd9 103 getTurn();//リセット前のバッファに残っているらしき値の消化
yuki0108 0:f3d3f744dbd9 104 getTurn();//
yuki0108 0:f3d3f744dbd9 105 flow_count=0;
yuki0108 0:f3d3f744dbd9 106 receive_old=0;
yuki0108 2:e0164b9fe93a 107 // printf("reset Amt21\r\n");
yuki0108 0:f3d3f744dbd9 108 }
yuki0108 0:f3d3f744dbd9 109 void Amt21::calOmega()
yuki0108 0:f3d3f744dbd9 110 {
yuki0108 0:f3d3f744dbd9 111 double time_=timer_.read();
yuki0108 0:f3d3f744dbd9 112 omega_=(count_-pre_count)*2.0f*M_PI/(RESOLUTION_AMT21*(time_-pre_time));
yuki0108 0:f3d3f744dbd9 113 omega_/=GEER_STEER;
yuki0108 0:f3d3f744dbd9 114 pre_count=count_;
yuki0108 0:f3d3f744dbd9 115 pre_time=time_;
yuki0108 0:f3d3f744dbd9 116 }
yuki0108 0:f3d3f744dbd9 117 double Amt21::getOmega()
yuki0108 0:f3d3f744dbd9 118 {
yuki0108 0:f3d3f744dbd9 119 return omega_;
yuki0108 0:f3d3f744dbd9 120 }