kinect用

Files at this revision

API Documentation at this revision

Comitter:
tanabe2000
Date:
Fri Oct 12 04:39:28 2018 +0000
Commit message:
kinect?

Changed in this revision

FEP.cpp Show annotated file Show diff for this revision Revisions of this file
FEP.h Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 68470e808ae2 FEP.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FEP.cpp	Fri Oct 12 04:39:28 2018 +0000
@@ -0,0 +1,72 @@
+#include "FEP.h"
+
+FEP::FEP(PinName tx, PinName rx, uint8_t addr, int baud) :
+    RawSerial(tx, rx, baud)
+{
+    _address = addr;
+}
+
+void FEP::startReceive(uint16_t dataSize)
+{
+    bufferSize = dataSize + 15;
+    bufferPoint = 0;
+    receivedBytes = 0;
+    buffer = new uint8_t[bufferSize];
+    data  = new uint8_t[dataSize];
+    attach(callback(this, &FEP::receiveByte));
+}
+
+void FEP::getData(uint8_t data_[])
+{
+    for(int i = 0; i < bufferSize - 15; i++) {
+        data_[i] = data[i];
+    }
+}
+
+void FEP::sendData(uint8_t data_[], uint8_t length)
+{
+    uint8_t checksum = 0x00;
+
+    printf("@TBN%03d%03d", _address, length + 1);
+    for (int j = 0; j < length; j++) {
+        checksum += data_[j];
+        putc(data_[j]);
+    }
+    putc(checksum);
+    putc('\r');
+    putc('\n');
+}
+
+void FEP::receiveByte()
+{
+    buffer[bufferPoint % bufferSize] = getc();
+
+    if(bufferPoint != 0xff) {
+        ++bufferPoint;
+    } else {
+        bufferPoint = (255%bufferSize)+1;
+    }
+
+    ++receivedBytes;
+
+    if(receivedBytes >= bufferSize) checkData();
+}
+
+void FEP::checkData()
+{
+    for(int i = 0; i < bufferSize; i++) {
+        if(buffer[i % bufferSize] == FEP_HEADER0 && buffer[(i + 1) % bufferSize] == FEP_HEADER1 && buffer[(i + 2) % bufferSize] == FEP_HEADER2) {
+            uint8_t checksum = 0x00;
+            for(int j = 9; j < bufferSize - 6; j++) {
+                checksum += buffer[(i + j)% bufferSize];
+            }
+            if(checksum == buffer[(i + bufferSize - 6)% bufferSize]) {
+                for(int j = 9; j < bufferSize - 6; j++) {
+                    data[j - 9] = buffer[(i + j) % bufferSize];
+                }
+                receivedBytes = 0;
+                return;
+            }
+        }
+    }
+}
diff -r 000000000000 -r 68470e808ae2 FEP.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FEP.h	Fri Oct 12 04:39:28 2018 +0000
@@ -0,0 +1,34 @@
+#ifndef FEP_H
+#define FEP_H
+
+#include "mbed.h"
+
+#define FEP_BUFFER_SIZE 22
+#define FEP_HEADER0 'R'
+#define FEP_HEADER1 'B'
+#define FEP_HEADER2 'N'
+
+class FEP : public RawSerial
+{
+public :
+    FEP(PinName tx, PinName rx, uint8_t addr, int baud=115200);
+
+    void startReceive(uint16_t dataSize);
+    void getData(uint8_t data_[]);
+    void sendData(uint8_t data_[], uint8_t length);
+
+private :
+    void receiveByte();
+    void checkData();
+
+    uint8_t *buffer;
+    uint8_t bufferPoint;
+    uint8_t receivedBytes;
+
+    uint16_t bufferSize;
+    uint8_t* data;
+
+    uint8_t _address;
+};
+
+#endif
\ No newline at end of file