NagaokaRoboticsClub_mbedTeam / R1370MeasuringWheel

measuringwheel関数がついかされています。

R1370.cpp@1:ec87437c2029, 2018-08-27 (annotated)

Committer:
tanabe2000
Date:
Mon Aug 27 04:45:30 2018 +0000
Revision:
1:ec87437c2029
Parent:
0:243364135087
Child:
4:ee51008e03e2
ver2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
UCHITAKE 0:243364135087 1 #include "R1370.h"
UCHITAKE 0:243364135087 2
UCHITAKE 0:243364135087 3 R1370::R1370(PinName tx, PinName rx) : RawSerial(tx, rx, 115200)
UCHITAKE 0:243364135087 4 {
UCHITAKE 0:243364135087 5 attach(callback(this, &R1370::receiveByte));
UCHITAKE 0:243364135087 6 thread.start(callback(this, &R1370::assembleLoop));
UCHITAKE 0:243364135087 7 }
UCHITAKE 0:243364135087 8
UCHITAKE 0:243364135087 9 void R1370::receiveByte()
UCHITAKE 0:243364135087 10 {
UCHITAKE 0:243364135087 11 buf.push_back(getc());
UCHITAKE 0:243364135087 12 }
UCHITAKE 0:243364135087 13
UCHITAKE 0:243364135087 14 void R1370::assemble()
UCHITAKE 0:243364135087 15 {
UCHITAKE 0:243364135087 16 char checksum = 0;
UCHITAKE 0:243364135087 17 for(int i = 2; i <= 13; i++) {
UCHITAKE 0:243364135087 18 checksum += buf[i];
UCHITAKE 0:243364135087 19 }
UCHITAKE 0:243364135087 20 if(checksum == buf[14]) {
UCHITAKE 0:243364135087 21 index = buf[2];
UCHITAKE 0:243364135087 22 angle = (buf[3] & 0xFF) | ((buf[4] << 8) & 0xFF00);
UCHITAKE 0:243364135087 23 rate = (buf[5] & 0xFF) | ((buf[6] << 8) & 0xFF00);
UCHITAKE 0:243364135087 24 acc[0] = (buf[7] & 0xFF) | ((buf[8] << 8) & 0xFF00);
UCHITAKE 0:243364135087 25 acc[1] = (buf[9] & 0xFF) | ((buf[10] << 8) & 0xFF00);
UCHITAKE 0:243364135087 26 acc[2] = (buf[11] & 0xFF) | ((buf[12] << 8) & 0xFF00);
UCHITAKE 0:243364135087 27 reserved = buf[13];
tanabe2000 1:ec87437c2029 28 upbit_ = buf[3];
tanabe2000 1:ec87437c2029 29 downbit_ = buf[4];
UCHITAKE 0:243364135087 30 }
UCHITAKE 0:243364135087 31 }
UCHITAKE 0:243364135087 32
UCHITAKE 0:243364135087 33 void R1370::assembleLoop()
UCHITAKE 0:243364135087 34 {
UCHITAKE 0:243364135087 35 while(true) {
UCHITAKE 0:243364135087 36 if(buf.size() > R1370_BUFFER_SIZE) {
UCHITAKE 0:243364135087 37 if(buf[0] == R1370_HEADER0 && buf[1] == R1370_HEADER1) {
UCHITAKE 0:243364135087 38 assemble();
UCHITAKE 0:243364135087 39 buf.erase(buf.begin(), buf.begin() + (R1370_BUFFER_SIZE - 1));
UCHITAKE 0:243364135087 40 } else {
UCHITAKE 0:243364135087 41 buf.erase(buf.begin());
UCHITAKE 0:243364135087 42 }
UCHITAKE 0:243364135087 43 assemble();
UCHITAKE 0:243364135087 44 }
UCHITAKE 0:243364135087 45 }
UCHITAKE 0:243364135087 46 }
UCHITAKE 0:243364135087 47
UCHITAKE 0:243364135087 48 float R1370::getAngle()
UCHITAKE 0:243364135087 49 {
UCHITAKE 0:243364135087 50 return (float)(angle / 100.0);
UCHITAKE 0:243364135087 51 }
UCHITAKE 0:243364135087 52
UCHITAKE 0:243364135087 53 float R1370::getRate()
UCHITAKE 0:243364135087 54 {
UCHITAKE 0:243364135087 55 return (float)(rate / 100.0);
UCHITAKE 0:243364135087 56 }
UCHITAKE 0:243364135087 57
UCHITAKE 0:243364135087 58 int16_t R1370::getAcc(char l)
UCHITAKE 0:243364135087 59 {
UCHITAKE 0:243364135087 60 if(l == 'x' || l == 'X') {
UCHITAKE 0:243364135087 61 return acc[0];
UCHITAKE 0:243364135087 62 } else if(l == 'y' || l == 'Y') {
UCHITAKE 0:243364135087 63 return acc[1];
UCHITAKE 0:243364135087 64 } else if(l == 'z' || l == 'Z') {
UCHITAKE 0:243364135087 65 return acc[2];
UCHITAKE 0:243364135087 66 } else {
UCHITAKE 0:243364135087 67 return 0;
UCHITAKE 0:243364135087 68 }
UCHITAKE 0:243364135087 69 }
UCHITAKE 0:243364135087 70
UCHITAKE 0:243364135087 71 int16_t R1370::getAcc(int i)
UCHITAKE 0:243364135087 72 {
UCHITAKE 0:243364135087 73 if(i >= 0 && i < 3) {
UCHITAKE 0:243364135087 74 return acc[i];
UCHITAKE 0:243364135087 75 } else {
UCHITAKE 0:243364135087 76 return 0;
UCHITAKE 0:243364135087 77 }
UCHITAKE 0:243364135087 78 }
UCHITAKE 0:243364135087 79
UCHITAKE 0:243364135087 80 int16_t R1370::getAccX()
UCHITAKE 0:243364135087 81 {
UCHITAKE 0:243364135087 82 return acc[0];
UCHITAKE 0:243364135087 83 }
UCHITAKE 0:243364135087 84
UCHITAKE 0:243364135087 85 int16_t R1370::getAccY()
UCHITAKE 0:243364135087 86 {
UCHITAKE 0:243364135087 87 return acc[1];
UCHITAKE 0:243364135087 88 }
UCHITAKE 0:243364135087 89
UCHITAKE 0:243364135087 90 int16_t R1370::getAccZ()
UCHITAKE 0:243364135087 91 {
UCHITAKE 0:243364135087 92 return acc[2];
tanabe2000 1:ec87437c2029 93 }
tanabe2000 1:ec87437c2029 94
tanabe2000 1:ec87437c2029 95 unsigned char R1370::upbit()
tanabe2000 1:ec87437c2029 96 {
tanabe2000 1:ec87437c2029 97 return upbit_;
tanabe2000 1:ec87437c2029 98 }
tanabe2000 1:ec87437c2029 99
tanabe2000 1:ec87437c2029 100 unsigned char R1370::downbit()
tanabe2000 1:ec87437c2029 101 {
tanabe2000 1:ec87437c2029 102 return downbit_;
UCHITAKE 0:243364135087 103 }