Shih-Ho Hsieh / Mbed 2 deprecated pololu5mag_with_platform

Dependencies:   mbed

Fork of Motor_XYZ_UI_SPI_I2C_5mag by Shih-Ho Hsieh

Revision:
4:5cde69a36d74
Child:
7:ee0569d49c52
diff -r 9ffe7ef07124 -r 5cde69a36d74 parse_array/ParseArray.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/parse_array/ParseArray.cpp	Wed Nov 08 07:49:05 2017 +0000
@@ -0,0 +1,258 @@
+#include "ParseArray.h"
+#include <cstdlib>
+// constructor
+ParseArray::ParseArray()
+{
+    hasCheckXOR = false;
+    hasCRC = false;
+    hasFooter = false;
+    hasHeader = false;
+
+    headerIndex=0;
+    headerLength = 0;
+    headerValueTmp="";
+
+    footerIndex = 0;
+    footerLength = 0;
+    footerValueTmp="";
+
+    checkCRCIndex = 0;
+    checkCRCLength = 0 ;
+    checkXORIndex = 0;
+}
+// string to byte function
+byte* ParseArray::stringToByte(string hex, int length)
+{
+    int NumberChars = hex.length();
+    byte* bytes = new byte[length];
+    long value;
+    string subS;
+    char arr[2];
+    //strtol( &hexstring[1], NULL, 16);
+    for (int i = 0; i < length; i += 1) {
+        subS = hex.substr(i * 3, 2);
+        value = strtol(subS.c_str() , NULL, 16);
+        bytes[i] = (byte)value;
+    }
+    return bytes;
+}
+// buffer setting
+void ParseArray::setBufferValue(byte* buf, int lengthTmp, int checkSumStart)
+{
+    if (hasCheckXOR) {
+        bufferLength = checkXORIndex + 1;
+    } else if (hasFooter) {
+        bufferLength = footerIndex + footerLength + 1;
+    } else if (hasHeader)
+        bufferLength = headerIndex + headerLength + 1;
+    else
+        bufferLength = lengthTmp;
+
+
+    bufferArray = new byte[bufferLength];
+
+    // header values
+    if (hasHeader) {
+        for (int i = headerIndex, j = 0; i < headerLength; i++, j++)
+            bufferArray[i] = headerValue[j];
+        for (int i = headerIndex + headerLength, j = 0; j < lengthTmp; i++, j++)
+            bufferArray[i] = buf[j];
+    } else {
+        for (int j = 0; j < lengthTmp; j++)
+            bufferArray[j] = buf[j];
+    }
+    // footer values
+    if (hasFooter) {
+        for (int i = footerIndex, j = 0; j < footerLength; i++, j++)
+            bufferArray[i] = footerValue[j];
+    }
+    // compute xorValue
+    if (hasCheckXOR) {
+        byte xorValue = 0;
+        for (int i = checkSumStart; i < bufferLength - 1; i++) {
+            xorValue ^= bufferArray[i];
+        }
+        bufferArray[bufferLength - 1] = xorValue;
+    }
+}
+void ParseArray::setBufferValueAdjustFooterIndex(byte* buf, int buflength, int checkSumStart)
+{
+    bufferLength = (headerIndex + headerLength + buflength + footerLength + 1);
+    bufferArray = new byte[bufferLength];
+    int i = 0,j=0;
+    // header values
+    for ( i = headerIndex, j = 0; i < headerLength; i++, j++)
+        bufferArray[i] = headerValue[j];
+    for ( i = headerIndex + headerLength, j = 0; j < buflength; i++, j++)
+        bufferArray[i] = buf[j];
+
+    // footer values
+    for ( i = headerIndex + headerLength + buflength, j = 0; j < footerLength; i++, j++)
+        bufferArray[i] = footerValue[j];
+    // compute xorValue
+    byte xorValue = 0;
+    for ( i = checkSumStart; i < bufferLength - 1; i++) {
+        xorValue ^= bufferArray[i];
+    }
+    bufferArray[bufferLength - 1] = xorValue;
+}
+
+void ParseArray::setBufferValue(byte* buf,int lengthTmp)
+{
+    if (hasCheckXOR) {
+        bufferLength = checkXORIndex + 1;
+    } else if (hasFooter) {
+        bufferLength = footerIndex + footerLength + 1;
+    } else if (hasHeader)
+        bufferLength = headerIndex + headerLength + 1;
+    else
+        bufferLength = lengthTmp;
+
+
+    bufferArray = new byte[bufferLength];
+
+    // header values
+    if (hasHeader) {
+        for (int i = headerIndex, j = 0; i < headerLength; i++, j++)
+            bufferArray[i] = headerValue[j];
+        for (int i = headerIndex + headerLength, j=0; j < lengthTmp; i++, j++)
+            bufferArray[i] = buf[j];
+    } else {
+        for (int j=0; j<lengthTmp; j++)
+            bufferArray[j] = buf[j];
+    }
+    // footer values
+    if (hasFooter) {
+        for (int i = footerIndex, j = 0; j < footerLength; i++, j++)
+            bufferArray[i] = footerValue[j];
+    }
+    // compute xorValue
+    if (hasCheckXOR) {
+        byte xorValue=0;
+        for (int i = 0; i < bufferLength - 1; i++) {
+            xorValue ^= bufferArray[i];
+        }
+        bufferArray[bufferLength - 1] = xorValue;
+    }
+}
+
+
+// header
+void ParseArray::enableHeader(int headerindex, int headerlength, string headervalue)
+{
+    hasHeader = true;
+    headerIndex = headerindex;
+    headerLength = headerlength;
+    headerValueTmp = headervalue;
+    headerValue = stringToByte(headervalue, headerlength);
+}
+// footer
+void ParseArray::enableFooter(int footerindex, int footerlength, string footervalue)
+{
+    hasFooter = true;
+    footerIndex = footerindex;
+    footerLength = footerlength;
+    footerValueTmp = footervalue;
+    footerValue = stringToByte(footervalue, footerlength);
+}
+// crc
+void ParseArray::enableCRC(int crcindex, int crclength)
+{
+    hasCRC = true;
+    checkCRCIndex = crcindex;
+    checkCRCLength = crclength;
+}
+// xorValue
+void ParseArray::enableCheckXOR(int xorIndex)
+{
+    hasCheckXOR = true;
+    checkXORIndex = xorIndex;
+}
+
+
+
+// check Header
+bool ParseArray::testHeader(int hindex)
+{
+    int i;
+    for (i = 0; i < headerLength; i++) {
+        if (bufferArray[(hindex + i) % bufferLength] != headerValue[i])
+            return false;
+    }
+    return true;
+}
+// check Footer
+bool ParseArray::testFooter(int findex)
+{
+    int i;
+    for (i = 0; i < footerLength; i++) {
+        if (bufferArray[(findex + i) % bufferLength] != footerValue[i])
+            return false;
+    }
+    return true;
+}
+// test xorValue
+bool ParseArray::testXOR()// set xindex as headerIndex
+{
+    int i = 0;
+    byte xorValue = 0 ;
+    for (i = 0; i < length; i++) {
+        xorValue ^= bufferArray[i % bufferLength];
+    }
+    if (xorValue == 0)
+        return true;
+    else
+        return false;
+}
+// parsing
+bool ParseArray::parse(int curIndex)
+{
+    // shift curIndex to end
+    if (hasCheckXOR) {
+        curIndex -= checkXORIndex;
+        curIndex += bufferLength;
+        curIndex %= bufferLength;
+    } else if (hasFooter) {
+        curIndex -= footerIndex;
+        curIndex += bufferLength;
+        curIndex %= bufferLength;
+    }
+
+    curIndex %= bufferLength;
+
+    //
+    if (hasHeader) {
+        // test header
+        if (testHeader(curIndex)) {
+            // pass header check
+        } else // fail header
+            return false;
+    }
+    // test footer
+    if (hasFooter) {
+        if (testFooter((curIndex + (footerIndex - headerIndex)) % bufferLength)) {
+            // pass footer check
+        } else // fail footer
+            return false;
+    }
+    // test xorValue
+    if (hasCheckXOR) {
+        if (!testXOR())// fail check
+            return false;
+    }
+    // pass all test
+    currentHeaderIndex = curIndex;
+    currentFooterIndex = (curIndex + (footerIndex - headerIndex));
+    return true;
+}
+// compute xor
+byte ParseArray::computeXOR(byte t1[], int len)
+{
+    byte XOR =0;
+    for(int i=0; i<len; i++) {
+        XOR=XOR^t1[i]; // xor
+    }
+    return XOR;
+}
+
+