static PyObject *setVariable(PyObject *self, PyObject *args)
{
char *variableName;
char *value;
if(!PyArg_ParseTuple(args, "ss", &variableName, &value))
return 0;
static string scmdval; scmdval[0] = 0;
ident *id = idents->access(variableName);
if(id)
{
switch(id->type)
{
case ID_VAR:
{
int ret = parseint(value);
if(ret < id->minval || ret > id->maxval)
{
PyErr_Format(PyExc_ValueError,
id->flags&IDF_HEX ?
(id->minval <= 255 ? "valid range for %s is %d..0x%X" : "valid range for %s is 0x%X..0x%X") :
"valid range for %s is %d..%d", variableName, id->minval, id->maxval);
return 0;
}
setvar(variableName, ret);
break;
}
case ID_FVAR:
{
float ret = parsefloat(value);
if(ret < id->minvalf || ret > id->maxvalf)
{
PyErr_Format(PyExc_ValueError, "valid range for %s is %s..%s", variableName, floatstr(id->minvalf), floatstr(id->maxvalf));
return 0;
}
setfvar(variableName, ret);
break;
}
case ID_SVAR:
{
setsvar(variableName, value);
break;
}
default:
{
PyErr_SetString(PyExc_ValueError, "Unknown server variable type");
return 0;
break;
}
}
printf("hopefully to set: %s to %s\n", variableName, value);
Py_INCREF(Py_None);
return Py_None;
}
PyErr_SetString(PyExc_ValueError, "Invalid variable specified");
return 0;
}
float getfloat() const
{
switch(type)
{
case INT: return float(val.i);
case FLOAT: return val.f;
case STRING: return float(parsefloat(val.s));
default: return 0;
}
}
float_type wcstod(const wchar_t* str, wchar_t** end, float_type strtod_fn(const char*, char**)) {
const wchar_t* original_str = str;
while (iswspace(*str)) {
str++;
}
// What's the longest span of the input that might be part of the float?
size_t max_len = wcsspn(str, L"-+0123456789.xXeEpP()nNaAiIfFtTyY");
// We know the only valid characters are ASCII, so convert them by brute force.
char* ascii_str = new char[max_len + 1];
if (!ascii_str) return float_type();
for (size_t i = 0; i < max_len; ++i) {
ascii_str[i] = str[i] & 0xff;
}
ascii_str[max_len] = 0;
// Set up a fake FILE that points to those ASCII characters, for `parsefloat`.
FILE f;
__sfileext fext;
_FILEEXT_SETUP(&f, &fext);
f._flags = __SRD;
f._bf._base = f._p = reinterpret_cast<unsigned char*>(ascii_str);
f._bf._size = f._r = max_len;
f._read = [](void*, char*, int) { return 0; }; // aka `eofread`, aka "no more data".
f._lb._base = NULL;
// Ask `parsefloat` to look at the same data more carefully.
// We can't just do this straight away because we can't construct a suitable FILE*
// in the absence of any `fwmemopen` analogous to `fmemopen`. And we don't want to
// duplicate the `parsefloat` logic. We also don't want to actually have to have wchar_t
// implementations of the ASCII `strtod` logic (though if you were designing a libc
// from scratch, you'd probably want to just make that more generic and lose all the
// cruft on top).
size_t actual_len = parsefloat(&f, ascii_str, ascii_str + max_len);
// Finally let the ASCII conversion function do the work.
char* ascii_end;
float_type result = strtod_fn(ascii_str, &ascii_end);
if (ascii_end != ascii_str + actual_len) abort();
if (end) {
if (actual_len == 0) {
// There was an error. We need to set the end pointer back to the original string, not the
// one we advanced past the leading whitespace.
*end = const_cast<wchar_t*>(original_str);
} else {
*end = const_cast<wchar_t*>(str) + actual_len;
}
}
delete[] ascii_str;
return result;
}
int main() {
init_platform();
ble_init();
print("*************** BLE Test *****************\n\r\n\r");
// while(1) {
// if (ble_available()) {
// // get the stuff from the BLE buffer
// char buffer[BLE_UART_BUFF_SIZE];
// ble_read(buffer, BLE_UART_BUFF_SIZE);
//
// // print
// xil_printf("Got something: %s\r\n", buffer);
// }
// }
while (1) {
if (readPacket(10000)) {
// xil_printf("Got something: %s\r\n", packetbuffer);
// Buttons
if (packetbuffer[1] == 'B') {
uint8_t buttnum = packetbuffer[2] - '0';
uint8_t pressed = packetbuffer[3] - '0';
printf("Button %d", buttnum);
if (pressed) xil_printf(" pressed\r\n");
else xil_printf(" released\r\n");
}
// Accelerometer
if (packetbuffer[1] == 'A') {
float x, y, z;
// x = parsefloat(packetbuffer+2);
// y = parsefloat(packetbuffer+6);
// z = parsefloat(packetbuffer+10);
// printf("Y: %.4f\r\n", 1.1f);
// xil_printf("A: %c%c 0x",packetbuffer[0], packetbuffer[1]);
// There seems to be an endian mismatch...
uint32_t tempX = ((uint8_t)packetbuffer[2]) << 0;
tempX = tempX | ((uint8_t)packetbuffer[3]) << 8;
tempX = tempX | ((uint8_t)packetbuffer[4]) << 16;
tempX = tempX | ((uint8_t)packetbuffer[5]) << 24;
uint32_t tempY = ((uint8_t)packetbuffer[6]) << 0;
tempY = tempY | ((uint8_t)packetbuffer[7]) << 8;
tempY = tempY | ((uint8_t)packetbuffer[8]) << 16;
tempY = tempY | ((uint8_t)packetbuffer[9]) << 24;
uint32_t tempZ = ((uint8_t)packetbuffer[10]) << 0;
tempZ = tempZ | ((uint8_t)packetbuffer[11]) << 8;
tempZ = tempZ | ((uint8_t)packetbuffer[12]) << 16;
tempZ = tempZ | ((uint8_t)packetbuffer[13]) << 24;
// having printed out the data in this format,
// you can load it into MATLAB to process it
// and view what the accelerometer data looks like
// xil_printf("%x %x %x\r\n", tempX, tempY, tempZ);
x = parsefloat(tempX);
y = parsefloat(tempY);
z = parsefloat(tempZ);
/** Here are some thresholds **/
if (y < -0.8) {
xil_printf("move right 4x\r\n");
} else if (y < -0.55) {
xil_printf("move right 3x\r\n");
} else if (y < -0.3) {
xil_printf("move right 2x\r\n");
} else if (y < -0.1) {
xil_printf("move right 1x\r\n");
}
if (y > 0.8) {
xil_printf("move left 4x\r\n");
} else if (y > 0.55) {
xil_printf("move left 3x\r\n");
} else if (y > 0.3) {
xil_printf("move left 2x\r\n");
} else if (y > 0.1) {
xil_printf("move left 1x\r\n");
}
// xil_printf("Accel\tx: "); printFloat(x);
// xil_printf("\t\ty:"); printFloat(y);
// xil_printf("\t\tz:"); printFloat(z);
// xil_printf("\r\n");
}
}
}
cleanup_platform();
return 0;
}
