void writeAsciiTestCube(char *filename, int count) {
FILE *out = fopen(filename, "w");
writeHeaderAscii(out);
float root[3] = { 0.0, 0.0, 0.0 };
stl_tri *tris = malloc(sizeof(stl_tri) * 12 * count);
int ndx = 0;
for(ndx = 0; ndx < count; ndx++)
createRectPrism(&tris[12 * ndx], root, 3.0, 2.5, 4.0);
writeTriArrayASCII(out, 12 * count, &tris[12 * ndx]);
free(tris);
fclose(out);
}
//SlimStatePacket
//==============================================================================
//Write
//==============================================================================
char* writeSlimStatePacket(const struct Packet* p, char* buffer, int ascii) {
//BinaryMode
if (!ascii) {
uint8_t lenght = 9;
//PACKET LENGTH
buffer = writeUint8(lenght, buffer);
//PACKET ID
buffer = writeUint8(SlimStatePacketID, buffer);
buffer = writeUint32(p->seq, buffer);
buffer = writeUint16(p->slimState.leftEncoder, buffer);
buffer = writeUint16(p->slimState.rightEncoder, buffer);
}//Ascii mode
else {
buffer = writeHeaderAscii(buffer);
buffer = writeUint8Ascii(SlimStatePacketID, buffer);
buffer = writeUint32Ascii(p->seq, buffer);
buffer = writeUint16Ascii(p->slimState.leftEncoder, buffer);
buffer = writeUint16Ascii(p->slimState.rightEncoder, buffer);
buffer = writeFooterAscii(buffer);
}
return buffer;
}