NagaokaRoboticsClub_mbedTeam / R1370MeasuringWheel

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

R1370.cpp@3:08ff7c46665f, 2018-08-24 (annotated)

Committer:
UCHITAKE
Date:
Fri Aug 24 05:39:24 2018 +0000
Revision:
3:08ff7c46665f
Parent:
0:243364135087 
tinyfix

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 }
UCHITAKE 0:243364135087 42 }
UCHITAKE 0:243364135087 43 }
UCHITAKE 0:243364135087 44
UCHITAKE 0:243364135087 45 float R1370::getAngle()
UCHITAKE 0:243364135087 46 {
UCHITAKE 0:243364135087 47 return (float)(angle / 100.0);
UCHITAKE 0:243364135087 48 }
UCHITAKE 0:243364135087 49
UCHITAKE 0:243364135087 50 float R1370::getRate()
UCHITAKE 0:243364135087 51 {
UCHITAKE 0:243364135087 52 return (float)(rate / 100.0);
UCHITAKE 0:243364135087 53 }
UCHITAKE 0:243364135087 54
UCHITAKE 0:243364135087 55 int16_t R1370::getAcc(char l)
UCHITAKE 0:243364135087 56 {
UCHITAKE 0:243364135087 57 if(l == 'x' || l == 'X') {
UCHITAKE 0:243364135087 58 return acc[0];
UCHITAKE 0:243364135087 59 } else if(l == 'y' || l == 'Y') {
UCHITAKE 0:243364135087 60 return acc[1];
UCHITAKE 0:243364135087 61 } else if(l == 'z' || l == 'Z') {
UCHITAKE 0:243364135087 62 return acc[2];
UCHITAKE 0:243364135087 63 } else {
UCHITAKE 0:243364135087 64 return 0;
UCHITAKE 0:243364135087 65 }
UCHITAKE 0:243364135087 66 }
UCHITAKE 0:243364135087 67
UCHITAKE 0:243364135087 68 int16_t R1370::getAcc(int i)
UCHITAKE 0:243364135087 69 {
UCHITAKE 0:243364135087 70 if(i >= 0 && i < 3) {
UCHITAKE 0:243364135087 71 return acc[i];
UCHITAKE 0:243364135087 72 } else {
UCHITAKE 0:243364135087 73 return 0;
UCHITAKE 0:243364135087 74 }
UCHITAKE 0:243364135087 75 }
UCHITAKE 0:243364135087 76
UCHITAKE 0:243364135087 77 int16_t R1370::getAccX()
UCHITAKE 0:243364135087 78 {
UCHITAKE 0:243364135087 79 return acc[0];
UCHITAKE 0:243364135087 80 }
UCHITAKE 0:243364135087 81
UCHITAKE 0:243364135087 82 int16_t R1370::getAccY()
UCHITAKE 0:243364135087 83 {
UCHITAKE 0:243364135087 84 return acc[1];
UCHITAKE 0:243364135087 85 }
UCHITAKE 0:243364135087 86
UCHITAKE 0:243364135087 87 int16_t R1370::getAccZ()
UCHITAKE 0:243364135087 88 {
UCHITAKE 0:243364135087 89 return acc[2];
UCHITAKE 0:243364135087 90 }