NagaokaRoboticsClub_mbedTeam / R1370

受信割り込みバージョン

Dependents:   a_team_R1370 ashimawari_sample_test 2022NHK_A_ashimawari 2022A_NHK_kikou

R1370.cpp@5:553c02009ad0, 2020-12-16 (annotated)

Committer:
highfieldsnj
Date:
Wed Dec 16 08:31:38 2020 +0000
Revision:
5:553c02009ad0
Parent:
4:ffc2a55269f9 
add functions reset

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 {
highfieldsnj 5:553c02009ad0 5 zeroPoint = 0;
UCHITAKE 0:243364135087 6 attach(callback(this, &R1370::receiveByte));
UCHITAKE 0:243364135087 7 }
UCHITAKE 0:243364135087 8
UCHITAKE 0:243364135087 9 void R1370::receiveByte()
UCHITAKE 0:243364135087 10 {
highfieldsnj 3:2f00371553c1 11 buffer[bufferPoint % R1370_BUFFER_SIZE] = getc();
highfieldsnj 3:2f00371553c1 12
highfieldsnj 3:2f00371553c1 13 if(bufferPoint != 0xff) {
highfieldsnj 3:2f00371553c1 14 ++bufferPoint;
highfieldsnj 3:2f00371553c1 15 } else {
highfieldsnj 3:2f00371553c1 16 bufferPoint = (255%R1370_BUFFER_SIZE)+1;
highfieldsnj 3:2f00371553c1 17 }
highfieldsnj 3:2f00371553c1 18
highfieldsnj 3:2f00371553c1 19 ++receivedBytes;
highfieldsnj 3:2f00371553c1 20
highfieldsnj 3:2f00371553c1 21 if(receivedBytes >= R1370_BUFFER_SIZE) checkData();
highfieldsnj 3:2f00371553c1 22 }
highfieldsnj 3:2f00371553c1 23
highfieldsnj 3:2f00371553c1 24 void R1370::checkData()
highfieldsnj 3:2f00371553c1 25 {
highfieldsnj 3:2f00371553c1 26 for(int i = 0; i < R1370_BUFFER_SIZE; i++) {
highfieldsnj 3:2f00371553c1 27 if(buffer[i % R1370_BUFFER_SIZE] == R1370_HEADER0 && buffer[(i + 1) % R1370_BUFFER_SIZE] == R1370_HEADER1) {
highfieldsnj 3:2f00371553c1 28
highfieldsnj 3:2f00371553c1 29 uint8_t checksum = 0x00;
highfieldsnj 3:2f00371553c1 30 for(int j = 0; j < R1370_BUFFER_SIZE - 3; j++) {
highfieldsnj 3:2f00371553c1 31 checksum += buffer[(i + 2 + j)% R1370_BUFFER_SIZE];
highfieldsnj 3:2f00371553c1 32
highfieldsnj 3:2f00371553c1 33 }
highfieldsnj 3:2f00371553c1 34 if(checksum == buffer[(i + R1370_BUFFER_SIZE - 1)% R1370_BUFFER_SIZE]) {
highfieldsnj 3:2f00371553c1 35 for(int j = 0; j < R1370_BUFFER_SIZE - 3; j++) {
highfieldsnj 3:2f00371553c1 36 data[j] = buffer[(i + 2 + j) % R1370_BUFFER_SIZE];
highfieldsnj 3:2f00371553c1 37 }
highfieldsnj 3:2f00371553c1 38 receivedBytes = 0;
highfieldsnj 3:2f00371553c1 39
highfieldsnj 3:2f00371553c1 40 assemble();
highfieldsnj 3:2f00371553c1 41 return;
highfieldsnj 3:2f00371553c1 42 }
highfieldsnj 3:2f00371553c1 43 }
highfieldsnj 3:2f00371553c1 44 }
UCHITAKE 0:243364135087 45 }
UCHITAKE 0:243364135087 46
UCHITAKE 0:243364135087 47 void R1370::assemble()
UCHITAKE 0:243364135087 48 {
highfieldsnj 3:2f00371553c1 49 index = data[0];
highfieldsnj 3:2f00371553c1 50 angle = (data[1] & 0xFF) | ((data[2] << 8) & 0xFF00);
highfieldsnj 3:2f00371553c1 51 rate = (data[3] & 0xFF) | ((data[4] << 8) & 0xFF00);
highfieldsnj 3:2f00371553c1 52 acc[0] = (data[5] & 0xFF) | ((data[6] << 8) & 0xFF00);
highfieldsnj 3:2f00371553c1 53 acc[1] = (data[7] & 0xFF) | ((data[8] << 8) & 0xFF00);
highfieldsnj 3:2f00371553c1 54 acc[2] = (data[9] & 0xFF) | ((data[10] << 8) & 0xFF00);
highfieldsnj 3:2f00371553c1 55 reserved = data[11];
highfieldsnj 3:2f00371553c1 56 upbit_ = data[1];
highfieldsnj 3:2f00371553c1 57 downbit_ = data[2];
UCHITAKE 0:243364135087 58 }
UCHITAKE 0:243364135087 59
UCHITAKE 0:243364135087 60 float R1370::getAngle()
UCHITAKE 0:243364135087 61 {
UCHITAKE 0:243364135087 62 return (float)(angle / 100.0);
UCHITAKE 0:243364135087 63 }
UCHITAKE 0:243364135087 64
highfieldsnj 5:553c02009ad0 65 float R1370::getRelativeAngle()
highfieldsnj 5:553c02009ad0 66 {
highfieldsnj 5:553c02009ad0 67 relativeAngle = (float)((angle - zeroPoint) / 100.0);
highfieldsnj 5:553c02009ad0 68 if(relativeAngle > 180.0){
highfieldsnj 5:553c02009ad0 69 return relativeAngle - 360.0;
highfieldsnj 5:553c02009ad0 70 }else if(relativeAngle < -180.0){
highfieldsnj 5:553c02009ad0 71 return relativeAngle + 360.0;
highfieldsnj 5:553c02009ad0 72 }
highfieldsnj 5:553c02009ad0 73 return relativeAngle;
highfieldsnj 5:553c02009ad0 74 }
highfieldsnj 5:553c02009ad0 75
UCHITAKE 0:243364135087 76 float R1370::getRate()
UCHITAKE 0:243364135087 77 {
UCHITAKE 0:243364135087 78 return (float)(rate / 100.0);
UCHITAKE 0:243364135087 79 }
UCHITAKE 0:243364135087 80
highfieldsnj 4:ffc2a55269f9 81 float R1370::getRadian()
highfieldsnj 4:ffc2a55269f9 82 {
highfieldsnj 4:ffc2a55269f9 83 return (float)(angle / 100.0 / 180.0 * 3.141592653589793);
highfieldsnj 4:ffc2a55269f9 84 }
highfieldsnj 4:ffc2a55269f9 85
highfieldsnj 5:553c02009ad0 86 float R1370::getRelativeRadian()
highfieldsnj 5:553c02009ad0 87 {
highfieldsnj 5:553c02009ad0 88 return getRelativeAngle() / 180.0 * 3.141592653589793;
highfieldsnj 5:553c02009ad0 89 }
highfieldsnj 5:553c02009ad0 90
UCHITAKE 0:243364135087 91 int16_t R1370::getAcc(char l)
UCHITAKE 0:243364135087 92 {
UCHITAKE 0:243364135087 93 if(l == 'x' || l == 'X') {
UCHITAKE 0:243364135087 94 return acc[0];
UCHITAKE 0:243364135087 95 } else if(l == 'y' || l == 'Y') {
UCHITAKE 0:243364135087 96 return acc[1];
UCHITAKE 0:243364135087 97 } else if(l == 'z' || l == 'Z') {
UCHITAKE 0:243364135087 98 return acc[2];
UCHITAKE 0:243364135087 99 } else {
UCHITAKE 0:243364135087 100 return 0;
UCHITAKE 0:243364135087 101 }
UCHITAKE 0:243364135087 102 }
UCHITAKE 0:243364135087 103
UCHITAKE 0:243364135087 104 int16_t R1370::getAcc(int i)
UCHITAKE 0:243364135087 105 {
UCHITAKE 0:243364135087 106 if(i >= 0 && i < 3) {
UCHITAKE 0:243364135087 107 return acc[i];
UCHITAKE 0:243364135087 108 } else {
UCHITAKE 0:243364135087 109 return 0;
UCHITAKE 0:243364135087 110 }
UCHITAKE 0:243364135087 111 }
UCHITAKE 0:243364135087 112
UCHITAKE 0:243364135087 113 int16_t R1370::getAccX()
UCHITAKE 0:243364135087 114 {
UCHITAKE 0:243364135087 115 return acc[0];
UCHITAKE 0:243364135087 116 }
UCHITAKE 0:243364135087 117
UCHITAKE 0:243364135087 118 int16_t R1370::getAccY()
UCHITAKE 0:243364135087 119 {
UCHITAKE 0:243364135087 120 return acc[1];
UCHITAKE 0:243364135087 121 }
UCHITAKE 0:243364135087 122
UCHITAKE 0:243364135087 123 int16_t R1370::getAccZ()
UCHITAKE 0:243364135087 124 {
UCHITAKE 0:243364135087 125 return acc[2];
highfieldsnj 3:2f00371553c1 126 }
highfieldsnj 3:2f00371553c1 127
highfieldsnj 3:2f00371553c1 128 unsigned char R1370::upbit()
highfieldsnj 3:2f00371553c1 129 {
highfieldsnj 3:2f00371553c1 130 return upbit_;
highfieldsnj 3:2f00371553c1 131 }
highfieldsnj 3:2f00371553c1 132
highfieldsnj 3:2f00371553c1 133 unsigned char R1370::downbit()
highfieldsnj 3:2f00371553c1 134 {
highfieldsnj 3:2f00371553c1 135 return downbit_;
highfieldsnj 5:553c02009ad0 136 }
highfieldsnj 5:553c02009ad0 137
highfieldsnj 5:553c02009ad0 138 void R1370::reset()
highfieldsnj 5:553c02009ad0 139 {
highfieldsnj 5:553c02009ad0 140 zeroPoint = angle;
UCHITAKE 0:243364135087 141 }