シンイチ 田中 / AMT21_right_arm

9/10

AMT21.cpp@5:b78f031b66f7, 2021-09-10 (annotated)

Committer:
oshin1030
Date:
Fri Sep 10 08:55:09 2021 +0000
Revision:
5:b78f031b66f7
Parent:
2:e0164b9fe93a 
9/10;

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);
oshin1030 5:b78f031b66f7 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 }
oshin1030 5:b78f031b66f7 66 turn_=receive_+flow_count*RESOLUTION_AMT21;
yuki0108 0:f3d3f744dbd9 67 receive_old=receive_;
oshin1030 5:b78f031b66f7 68 return turn_;
yuki0108 0:f3d3f744dbd9 69 }
oshin1030 5:b78f031b66f7 70 void Amt21::rewriteCount_1()
yuki0108 0:f3d3f744dbd9 71 {
oshin1030 5:b78f031b66f7 72 int count,turn_;
yuki0108 0:f3d3f744dbd9 73 count=getAbCount();
yuki0108 2:e0164b9fe93a 74 wait_us(200);
oshin1030 5:b78f031b66f7 75 turn_=getTurn();
oshin1030 5:b78f031b66f7 76 count_=-(count+RESOLUTION_AMT21*turn_);
yuki0108 2:e0164b9fe93a 77 }
oshin1030 5:b78f031b66f7 78 void Amt21::rewriteCount_2()
oshin1030 5:b78f031b66f7 79 {
oshin1030 5:b78f031b66f7 80 int count,turn_;
oshin1030 5:b78f031b66f7 81 count=getAbCount();
oshin1030 5:b78f031b66f7 82 wait_us(200);
oshin1030 5:b78f031b66f7 83 turn_=getTurn();
oshin1030 5:b78f031b66f7 84 count_=count+RESOLUTION_AMT21*turn_;
oshin1030 5:b78f031b66f7 85 }
oshin1030 5:b78f031b66f7 86
yuki0108 2:e0164b9fe93a 87 int Amt21::getCount()
yuki0108 2:e0164b9fe93a 88 {
yuki0108 0:f3d3f744dbd9 89 return count_;
yuki0108 0:f3d3f744dbd9 90 }
yuki0108 1:3838b5e3966c 91 double Amt21::getDeg()
yuki0108 0:f3d3f744dbd9 92 {
yuki0108 0:f3d3f744dbd9 93 double angle_;
yuki0108 0:f3d3f744dbd9 94 angle_=(count_-COUNT_OFFSET)*360.0/RESOLUTION_AMT21;
yuki0108 0:f3d3f744dbd9 95 angle_/=GEER_STEER;
yuki0108 0:f3d3f744dbd9 96 return angle_;
yuki0108 0:f3d3f744dbd9 97 }
yuki0108 1:3838b5e3966c 98 double Amt21::getRad()
yuki0108 1:3838b5e3966c 99 {
yuki0108 1:3838b5e3966c 100 double angle_;
yuki0108 1:3838b5e3966c 101 angle_=(count_-COUNT_OFFSET)*2.0f*M_PI/RESOLUTION_AMT21;
yuki0108 1:3838b5e3966c 102 angle_/=GEER_STEER;
yuki0108 1:3838b5e3966c 103 return angle_;
yuki0108 1:3838b5e3966c 104 }
yuki0108 0:f3d3f744dbd9 105 void Amt21::reset()
yuki0108 0:f3d3f744dbd9 106 {
yuki0108 0:f3d3f744dbd9 107 instruction=RESET;
yuki0108 0:f3d3f744dbd9 108 sendMessage();
yuki0108 0:f3d3f744dbd9 109 instruction=0x75;
yuki0108 0:f3d3f744dbd9 110 sendMessage();
yuki0108 0:f3d3f744dbd9 111 wait_us(200);
yuki0108 0:f3d3f744dbd9 112 getTurn();//リセット前のバッファに残っているらしき値の消化
yuki0108 0:f3d3f744dbd9 113 getTurn();//
yuki0108 0:f3d3f744dbd9 114 flow_count=0;
yuki0108 0:f3d3f744dbd9 115 receive_old=0;
yuki0108 2:e0164b9fe93a 116 // printf("reset Amt21\r\n");
yuki0108 0:f3d3f744dbd9 117 }
yuki0108 0:f3d3f744dbd9 118 void Amt21::calOmega()
yuki0108 0:f3d3f744dbd9 119 {
yuki0108 0:f3d3f744dbd9 120 double time_=timer_.read();
yuki0108 0:f3d3f744dbd9 121 omega_=(count_-pre_count)*2.0f*M_PI/(RESOLUTION_AMT21*(time_-pre_time));
yuki0108 0:f3d3f744dbd9 122 omega_/=GEER_STEER;
yuki0108 0:f3d3f744dbd9 123 pre_count=count_;
yuki0108 0:f3d3f744dbd9 124 pre_time=time_;
yuki0108 0:f3d3f744dbd9 125 }
yuki0108 0:f3d3f744dbd9 126 double Amt21::getOmega()
yuki0108 0:f3d3f744dbd9 127 {
yuki0108 0:f3d3f744dbd9 128 return omega_;
yuki0108 0:f3d3f744dbd9 129 }