Skip to content

Instantly share code, notes, and snippets.

@dunmatt
Last active March 22, 2016 18:38
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save dunmatt/16370fa5a781daf56097 to your computer and use it in GitHub Desktop.
Save dunmatt/16370fa5a781daf56097 to your computer and use it in GitHub Desktop.
This is a skeleton Arduino sketch that taps into the Mass Destruction telemetry stream.
#define TELEMETRY_ITEM_BUFFER_SIZE 40
#define OWN_NUMBER 100 // TODO: replace this with your assigned number
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
}
char telemetryBuffer[TELEMETRY_ITEM_BUFFER_SIZE];
int telemetryBufferPosition = 0;
bool completesTelemetryItem(char c) {
telemetryBuffer[telemetryBufferPosition] = (c == '\n' || c == ' ') ? 0 : c;
if (telemetryBufferPosition + 1 < TELEMETRY_ITEM_BUFFER_SIZE) {
++telemetryBufferPosition;
}
return c == '\n';
}
void parseTelemetryItem(int &robotId, double &x, double &y, double &theta) {
char* buf = telemetryBuffer;
robotId = atoi(buf);
buf += strlen(buf) + 1;
x = atof(buf);
buf += strlen(buf) + 1;
y = atof(buf);
buf += strlen(buf) + 1;
theta = atof(buf);
telemetryBufferPosition = 0;
}
void handleSelfUpdate(double x, double y, double theta) {
// TODO: do something violent!
}
void handleOtherUpdate(int robotId, double x, double y, double theta) {
// TODO: do something violent!
}
void loop() {
while (Serial.available()) {
char c = Serial.read();
if (completesTelemetryItem(c)) {
int i;
double x,y,t;
parseTelemetryItem(i, x, y, t);
if (i == OWN_NUMBER) {
handleSelfUpdate(x, y, t);
} else {
handleOtherUpdate(i, x, y, t);
}
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment