NagaokaRoboticsClub_mbedTeam / R1370

受信割り込みバージョン

Dependents:   a_team_R1370 ashimawari_sample_test 2022NHK_A_ashimawari 2022A_NHK_kikou

R1370.cpp@4:ffc2a55269f9, 2019-05-18 (annotated)

Committer:
highfieldsnj
Date:
Sat May 18 08:42:05 2019 +0000
Revision:
4:ffc2a55269f9
Parent:
3:2f00371553c1
Child:
5:553c02009ad0
add function getRadian()

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