NagaokaRoboticsClub_mbedTeam / R1370MeasuringWheel

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

R1370.cpp@4:ee51008e03e2, 2018-12-18 (annotated)

Committer:
UCHITAKE
Date:
Tue Dec 18 18:51:58 2018 +0900
Revision:
4:ee51008e03e2
Parent:
1:ec87437c2029 
ring buffering

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 }
UCHITAKE 0:243364135087 7
UCHITAKE 0:243364135087 8 void R1370::receiveByte()
UCHITAKE 0:243364135087 9 {
UCHITAKE 4:ee51008e03e2 10 buffer[bufferPoint % R1370_BUFFER_SIZE] = getc();
UCHITAKE 4:ee51008e03e2 11
UCHITAKE 4:ee51008e03e2 12 if(bufferPoint != 0xff) {
UCHITAKE 4:ee51008e03e2 13 ++bufferPoint;
UCHITAKE 4:ee51008e03e2 14 } else {
UCHITAKE 4:ee51008e03e2 15 bufferPoint = (255%R1370_BUFFER_SIZE)+1;
UCHITAKE 4:ee51008e03e2 16 }
UCHITAKE 4:ee51008e03e2 17
UCHITAKE 4:ee51008e03e2 18 ++receivedBytes;
UCHITAKE 4:ee51008e03e2 19
UCHITAKE 4:ee51008e03e2 20 if(receivedBytes >= R1370_BUFFER_SIZE) checkData();
UCHITAKE 4:ee51008e03e2 21 }
UCHITAKE 4:ee51008e03e2 22
UCHITAKE 4:ee51008e03e2 23 void R1370::checkData()
UCHITAKE 4:ee51008e03e2 24 {
UCHITAKE 4:ee51008e03e2 25 for(int i = 0; i < R1370_BUFFER_SIZE; i++) {
UCHITAKE 4:ee51008e03e2 26 if(buffer[i % R1370_BUFFER_SIZE] == R1370_HEADER0 && buffer[(i + 1) % R1370_BUFFER_SIZE] == R1370_HEADER1) {
UCHITAKE 4:ee51008e03e2 27
UCHITAKE 4:ee51008e03e2 28 uint8_t checksum = 0x00;
UCHITAKE 4:ee51008e03e2 29 for(int j = 0; j < R1370_BUFFER_SIZE - 3; j++) {
UCHITAKE 4:ee51008e03e2 30 checksum += buffer[(i + 2 + j)% R1370_BUFFER_SIZE];
UCHITAKE 4:ee51008e03e2 31
UCHITAKE 4:ee51008e03e2 32 }
UCHITAKE 4:ee51008e03e2 33 if(checksum == buffer[(i + R1370_BUFFER_SIZE - 1)% R1370_BUFFER_SIZE]) {
UCHITAKE 4:ee51008e03e2 34 for(int j = 0; j < R1370_BUFFER_SIZE - 3; j++) {
UCHITAKE 4:ee51008e03e2 35 data[j] = buffer[(i + 2 + j) % R1370_BUFFER_SIZE];
UCHITAKE 4:ee51008e03e2 36 }
UCHITAKE 4:ee51008e03e2 37 receivedBytes = 0;
UCHITAKE 4:ee51008e03e2 38
UCHITAKE 4:ee51008e03e2 39 assemble();
UCHITAKE 4:ee51008e03e2 40 return;
UCHITAKE 4:ee51008e03e2 41 }
UCHITAKE 4:ee51008e03e2 42 }
UCHITAKE 4:ee51008e03e2 43 }
UCHITAKE 0:243364135087 44 }
UCHITAKE 0:243364135087 45
UCHITAKE 0:243364135087 46 void R1370::assemble()
UCHITAKE 0:243364135087 47 {
UCHITAKE 4:ee51008e03e2 48 index = data[0];
UCHITAKE 4:ee51008e03e2 49 angle = (data[1] & 0xFF) | ((data[2] << 8) & 0xFF00);
UCHITAKE 4:ee51008e03e2 50 rate = (data[3] & 0xFF) | ((data[4] << 8) & 0xFF00);
UCHITAKE 4:ee51008e03e2 51 acc[0] = (data[5] & 0xFF) | ((data[6] << 8) & 0xFF00);
UCHITAKE 4:ee51008e03e2 52 acc[1] = (data[7] & 0xFF) | ((data[8] << 8) & 0xFF00);
UCHITAKE 4:ee51008e03e2 53 acc[2] = (data[9] & 0xFF) | ((data[10] << 8) & 0xFF00);
UCHITAKE 4:ee51008e03e2 54 reserved = data[11];
UCHITAKE 4:ee51008e03e2 55 upbit_ = data[1];
UCHITAKE 4:ee51008e03e2 56 downbit_ = data[2];
UCHITAKE 0:243364135087 57 }
UCHITAKE 0:243364135087 58
UCHITAKE 0:243364135087 59 float R1370::getAngle()
UCHITAKE 0:243364135087 60 {
UCHITAKE 0:243364135087 61 return (float)(angle / 100.0);
UCHITAKE 0:243364135087 62 }
UCHITAKE 0:243364135087 63
UCHITAKE 0:243364135087 64 float R1370::getRate()
UCHITAKE 0:243364135087 65 {
UCHITAKE 0:243364135087 66 return (float)(rate / 100.0);
UCHITAKE 0:243364135087 67 }
UCHITAKE 0:243364135087 68
UCHITAKE 0:243364135087 69 int16_t R1370::getAcc(char l)
UCHITAKE 0:243364135087 70 {
UCHITAKE 0:243364135087 71 if(l == 'x' || l == 'X') {
UCHITAKE 0:243364135087 72 return acc[0];
UCHITAKE 0:243364135087 73 } else if(l == 'y' || l == 'Y') {
UCHITAKE 0:243364135087 74 return acc[1];
UCHITAKE 0:243364135087 75 } else if(l == 'z' || l == 'Z') {
UCHITAKE 0:243364135087 76 return acc[2];
UCHITAKE 0:243364135087 77 } else {
UCHITAKE 0:243364135087 78 return 0;
UCHITAKE 0:243364135087 79 }
UCHITAKE 0:243364135087 80 }
UCHITAKE 0:243364135087 81
UCHITAKE 0:243364135087 82 int16_t R1370::getAcc(int i)
UCHITAKE 0:243364135087 83 {
UCHITAKE 0:243364135087 84 if(i >= 0 && i < 3) {
UCHITAKE 0:243364135087 85 return acc[i];
UCHITAKE 0:243364135087 86 } else {
UCHITAKE 0:243364135087 87 return 0;
UCHITAKE 0:243364135087 88 }
UCHITAKE 0:243364135087 89 }
UCHITAKE 0:243364135087 90
UCHITAKE 0:243364135087 91 int16_t R1370::getAccX()
UCHITAKE 0:243364135087 92 {
UCHITAKE 0:243364135087 93 return acc[0];
UCHITAKE 0:243364135087 94 }
UCHITAKE 0:243364135087 95
UCHITAKE 0:243364135087 96 int16_t R1370::getAccY()
UCHITAKE 0:243364135087 97 {
UCHITAKE 0:243364135087 98 return acc[1];
UCHITAKE 0:243364135087 99 }
UCHITAKE 0:243364135087 100
UCHITAKE 0:243364135087 101 int16_t R1370::getAccZ()
UCHITAKE 0:243364135087 102 {
UCHITAKE 0:243364135087 103 return acc[2];
tanabe2000 1:ec87437c2029 104 }
tanabe2000 1:ec87437c2029 105
tanabe2000 1:ec87437c2029 106 unsigned char R1370::upbit()
tanabe2000 1:ec87437c2029 107 {
tanabe2000 1:ec87437c2029 108 return upbit_;
tanabe2000 1:ec87437c2029 109 }
tanabe2000 1:ec87437c2029 110
tanabe2000 1:ec87437c2029 111 unsigned char R1370::downbit()
tanabe2000 1:ec87437c2029 112 {
tanabe2000 1:ec87437c2029 113 return downbit_;
UCHITAKE 4:ee51008e03e2 114 }