NagaokaRoboticsClub_mbedTeam / R1370MeasuringWheel

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

R1370.cpp@0:243364135087, 2018-08-24 (annotated)

Committer:
UCHITAKE
Date:
Fri Aug 24 04:38:13 2018 +0000
Revision:
0:243364135087
Child:
1:ec87437c2029
Child:
3:08ff7c46665f
initial commit

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];
UCHITAKE 0:243364135087 28 }
UCHITAKE 0:243364135087 29 }
UCHITAKE 0:243364135087 30
UCHITAKE 0:243364135087 31 void R1370::assembleLoop()
UCHITAKE 0:243364135087 32 {
UCHITAKE 0:243364135087 33 while(true) {
UCHITAKE 0:243364135087 34 if(buf.size() > R1370_BUFFER_SIZE) {
UCHITAKE 0:243364135087 35 if(buf[0] == R1370_HEADER0 && buf[1] == R1370_HEADER1) {
UCHITAKE 0:243364135087 36 assemble();
UCHITAKE 0:243364135087 37 buf.erase(buf.begin(), buf.begin() + (R1370_BUFFER_SIZE - 1));
UCHITAKE 0:243364135087 38 } else {
UCHITAKE 0:243364135087 39 buf.erase(buf.begin());
UCHITAKE 0:243364135087 40 }
UCHITAKE 0:243364135087 41 assemble();
UCHITAKE 0:243364135087 42 }
UCHITAKE 0:243364135087 43 }
UCHITAKE 0:243364135087 44 }
UCHITAKE 0:243364135087 45
UCHITAKE 0:243364135087 46 float R1370::getAngle()
UCHITAKE 0:243364135087 47 {
UCHITAKE 0:243364135087 48 return (float)(angle / 100.0);
UCHITAKE 0:243364135087 49 }
UCHITAKE 0:243364135087 50
UCHITAKE 0:243364135087 51 float R1370::getRate()
UCHITAKE 0:243364135087 52 {
UCHITAKE 0:243364135087 53 return (float)(rate / 100.0);
UCHITAKE 0:243364135087 54 }
UCHITAKE 0:243364135087 55
UCHITAKE 0:243364135087 56 int16_t R1370::getAcc(char l)
UCHITAKE 0:243364135087 57 {
UCHITAKE 0:243364135087 58 if(l == 'x' || l == 'X') {
UCHITAKE 0:243364135087 59 return acc[0];
UCHITAKE 0:243364135087 60 } else if(l == 'y' || l == 'Y') {
UCHITAKE 0:243364135087 61 return acc[1];
UCHITAKE 0:243364135087 62 } else if(l == 'z' || l == 'Z') {
UCHITAKE 0:243364135087 63 return acc[2];
UCHITAKE 0:243364135087 64 } else {
UCHITAKE 0:243364135087 65 return 0;
UCHITAKE 0:243364135087 66 }
UCHITAKE 0:243364135087 67 }
UCHITAKE 0:243364135087 68
UCHITAKE 0:243364135087 69 int16_t R1370::getAcc(int i)
UCHITAKE 0:243364135087 70 {
UCHITAKE 0:243364135087 71 if(i >= 0 && i < 3) {
UCHITAKE 0:243364135087 72 return acc[i];
UCHITAKE 0:243364135087 73 } else {
UCHITAKE 0:243364135087 74 return 0;
UCHITAKE 0:243364135087 75 }
UCHITAKE 0:243364135087 76 }
UCHITAKE 0:243364135087 77
UCHITAKE 0:243364135087 78 int16_t R1370::getAccX()
UCHITAKE 0:243364135087 79 {
UCHITAKE 0:243364135087 80 return acc[0];
UCHITAKE 0:243364135087 81 }
UCHITAKE 0:243364135087 82
UCHITAKE 0:243364135087 83 int16_t R1370::getAccY()
UCHITAKE 0:243364135087 84 {
UCHITAKE 0:243364135087 85 return acc[1];
UCHITAKE 0:243364135087 86 }
UCHITAKE 0:243364135087 87
UCHITAKE 0:243364135087 88 int16_t R1370::getAccZ()
UCHITAKE 0:243364135087 89 {
UCHITAKE 0:243364135087 90 return acc[2];
UCHITAKE 0:243364135087 91 }