123 lines
3.1 KiB
C
123 lines
3.1 KiB
C
#include "TransferDataStructure.h"
|
|
|
|
String getValue(String data, char separator, int index)
|
|
{
|
|
int found = 0;
|
|
int strIndex[] = { 0, -1 };
|
|
int maxIndex = data.length() - 1;
|
|
|
|
for (int i = 0; i <= maxIndex && found <= index; i++) {
|
|
if (data.charAt(i) == separator || i == maxIndex) {
|
|
found++;
|
|
strIndex[0] = strIndex[1] + 1;
|
|
strIndex[1] = (i == maxIndex) ? i + 1 : i;
|
|
}
|
|
}
|
|
return found > index ? data.substring(strIndex[0], strIndex[1]) : "";
|
|
}
|
|
|
|
dataStruct2 GetSerialData(dataStruct2 &dataIn, bool debug = false)
|
|
{
|
|
if (Serial.available() && Serial.peek() != -1)
|
|
{
|
|
// String data = Serial.readStringUntil('\r');
|
|
// while (Serial.read() >= 0);
|
|
// int maxIndex = data.length() - 1;
|
|
// int index = 0;
|
|
// int next_index;
|
|
// String data_word;
|
|
// String channel;
|
|
// String angle;
|
|
// do {
|
|
// next_index = data.indexOf(',', index);
|
|
//
|
|
// data_word = data.substring(index, next_index);
|
|
// channel = data_word.substring(0, data_word.indexOf('_'));
|
|
// angle = data_word.substring(data_word.indexOf('_') + 1);
|
|
//
|
|
// if (isDigit(angle[0])) // prevent error overwriting first entry.
|
|
// dataIn.servoPositions[channel.toInt()] = angle.toFloat();
|
|
//
|
|
// index = next_index + 1;
|
|
// } while ((next_index != -1) && (next_index < maxIndex));
|
|
|
|
|
|
/*String s = Serial.readStringUntil('s');
|
|
for (int i = 0; i < 18; i++)
|
|
{
|
|
Serial.setTimeout(10);
|
|
String c = Serial.readStringUntil('_');
|
|
String v = Serial.readStringUntil(',');
|
|
String cc = Serial.readStringUntil('=');
|
|
int channel = c.toInt();
|
|
int value = v.toInt();
|
|
int crc = cc.toInt();
|
|
|
|
if ((crc == (channel) * value) && value >= 0 && value <= 180)
|
|
dataIn.servoPositions[channel] = value;*/
|
|
//byte recv[18 + 1];
|
|
if (Serial.available() >= 18 + 1 + 2 + 30)
|
|
{
|
|
bool recc = false;
|
|
byte recv[256] = {0};
|
|
//Serial.readBytesUntil(0xffffff, recv, 255);
|
|
Serial.readBytesUntil(0xff, recv, 255);
|
|
for (int i = 0; i < 255 - 19; i++)
|
|
{
|
|
// if (recv[i + 1] == 0xff && recv[i + 2] == 0xff && recv[i + 3] == 0xff)
|
|
if (recv[i + 19] == 0xfe)
|
|
{
|
|
for (int j = 0; j < 18 + 1; j++)
|
|
{
|
|
recv[j] = recv[(i) + j];
|
|
}
|
|
recc = true;
|
|
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
//s.getBytes(recv, s.length());
|
|
byte crc = 0x00;
|
|
for (int i = 0; i < 18; i++)
|
|
{
|
|
crc += recv[i];
|
|
}
|
|
|
|
|
|
if (crc == recv[18])
|
|
{
|
|
|
|
if (recc)
|
|
{
|
|
for (int i = 0; i < 18; i++)
|
|
{
|
|
dataIn.servoPositions[i] = recv[i];
|
|
}
|
|
}
|
|
}
|
|
//}
|
|
|
|
dataIn.remotePasstroughControl = true;
|
|
|
|
if (debug)
|
|
{
|
|
Serial.println("Enabled Servo Passtrough mode!");
|
|
|
|
Serial.println("Output line: ");
|
|
for (int i = 0; i < 18; i++)
|
|
{
|
|
Serial.print(i);
|
|
Serial.print(", ");
|
|
Serial.print(dataIn.servoPositions[i]);
|
|
Serial.println("");
|
|
}
|
|
Serial.println("");
|
|
}
|
|
}
|
|
}
|
|
|
|
return dataIn;
|
|
}
|