steer_control_t *steer_control_create (mobile_t *mob) {
assert (mob);
steer_control_t *con = xcalloc (1, sizeof (steer_control_t));
con->scene.paint = paint;
con->mob = mob;
con->left = sensor_create (mob, M_PI_4);
con->right = sensor_create (mob, -M_PI_4);
con->mid = sensor_create (mob, 0);
return con;
}
bool json_parse_sensors_internal(TokenVector* tokens_vector, const char* json_buffer, ListElement* sensors) {
int sensors_array_token_index, current_token_index, sensor_index;
jsmntok_t* tokens = token_vector_get_pointer(tokens_vector, 0);
// Find sensors array token:
sensors_array_token_index = find_json_array_token(tokens_vector, json_buffer, SENSORS_TAG);
if (sensors_array_token_index < 0) {
return false;
}
current_token_index = sensors_array_token_index + 1;
// iterate of all tokens, try to build sensors
for (sensor_index = 0 ; sensor_index < tokens[sensors_array_token_index].size; sensor_index++) {
int id = -1, port_index = -1, value;
const unsigned int next_sensor_token_index = current_token_index + tokens[current_token_index].size + 1;
// We're expecting something like - {"id":4,"port_index":6}
if (tokens[current_token_index].type != JSMN_OBJECT || tokens[current_token_index].size < 4) {
current_token_index = next_sensor_token_index;
continue;
}
current_token_index++;
// First token is the key, the second is the value
while (current_token_index < next_sensor_token_index) {
const unsigned int next_object_token_index = current_token_index + tokens[current_token_index].size + 1;
if (tokens[current_token_index].type != JSMN_STRING) { // Must be an error...
current_token_index = next_object_token_index;
continue;
}
if (tokens[current_token_index + 1].type != JSMN_PRIMITIVE) { // Must be an error...
current_token_index = next_object_token_index;
continue;
}
// Read the value
if (!token_to_int(json_buffer, &tokens[current_token_index + 1], &value)) {
current_token_index = next_object_token_index;
continue;
}
if (TOKEN_STRING(json_buffer, tokens[current_token_index], ID_TAG)) { // Id tag
id = value;
} else if (TOKEN_STRING(json_buffer, tokens[current_token_index], PORT_INDEX_TAG)) { // Port index tag
port_index = value;
} // else - ignore this key.
current_token_index += 2;
}
if (id >= 0 && port_index >= 0) { // Add sensor
Sensor* sensor = sensor_create();
sensor->id = id;
sensor->port_index = port_index;
list_add(sensors, sensor);
}
}
return true;
}
