void entry(){
uint16 lcd, value;
while(1){
value = get_sensor_value(3) - get_sensor_value(1);
print_hex(value);
wait_1sec();
}
}
int do_test(int dis){
run_relative(dis);
while(!command_finish());
sleep(1);
run_relative(-1*dis);
while(!command_finish());
sleep(1);
update_sensor(SENSOR_US);
printf("%d\t%d\n", dis, get_sensor_value(SENSOR_US));
return get_sensor_value(SENSOR_US);
}
// get all the sensors together
void get_all_sensor_values(alt_u16* heart_rate_sensor, alt_u16* gsr_sensor, alt_u16* temperature, int temp_sensor_address)
{
*(heart_rate_sensor) = get_sensor_value(HEART_SENSOR_ADRS); //reading from channel0
// should add the real heart rate calculation here
//HR_value = (template_var / m_HR) + HR_min
*(gsr_sensor) = get_sensor_value(GSR_SENSOR_ADRS); //reading from channel1
// should add the real gsr value calculation here
//GSR_value = template_var / m_GSR + GSR_max
*(temperature) = altera_avalon_temp_sensor_read_temperature(temp_sensor_address);
// float temperature_real = temperature * TEMP_PRECISION; // real temperature value
}
//return 1 if obstacle found
int obstacle(){
update_sensor(SENSOR_US);
if(get_sensor_value(SENSOR_US) < STOP_DIST) {
return 1;
}
return 0;
}
void OCIA_HANDLE(){
counter++;
gio.P6DR |= 0x01;
if(counter >= 500){
print_hex(get_sensor_value(1));
counter = 0;
}
}
/**
* Get the sensor value
*/
json_t * b_device_get_sensor (json_t * device, const char * sensor_name, void * device_ptr) {
y_log_message(Y_LOG_LEVEL_INFO, "device-mock - Running command sensor for sensor %s on device %s", sensor_name, json_string_value(json_object_get(device, "name")));
if (0 == o_strcmp(sensor_name, "se1") || 0 == o_strcmp(sensor_name, "se2")) {
return json_pack("{sisf}", "result", WEBSERVICE_RESULT_OK, "value", get_sensor_value(sensor_name));
} else {
return json_pack("{si}", "result", WEBSERVICE_RESULT_NOT_FOUND);
}
}
int scan_for_ball(){
int dist;
int found=0;
min.d =2500;
set_light(LIT_RIGHT, LIT_GREEN);
set_motors_speed(SLOW_SPEED);
turn_relative(90);
while(!command_finish()){
update_sensor(SENSOR_US);
found |= check_ball(get_sensor_value(SENSOR_US), get_gyro_value());
}
//update robot angle
robot.t += 90;
turn_relative(-180);
while(!command_finish()){
update_sensor(SENSOR_US);
found |= check_ball(get_sensor_value(SENSOR_US), get_gyro_value());
}
//update robot angle
robot.t -= 180;
if(found == 1){
// Durante la scansione ho trovato un minimo
sleep(1);
printf("Start: %d -> Stop teorico: %d, Distanza teorica: %d\n", get_gyro_value(), min.t, min.d);
turn_relative_fb(min.t-get_gyro_value());
//update robot angle
robot.t = get_gyro_value();
printf("Stop reale: %d, Distanza reale: %d\n", robot.t, dist);
}else{
// Durante la scansione non ho trovato minimi
turn_relative_fb(90);
//while(!command_finish());
//update robot angle
robot.t = get_gyro_value();
}
set_light(LIT_RIGHT, LIT_OFF);
return found;
}
/**
* Get the device overview
* Returns a mocked overview with 2 sensors, 2 switches, 2 dimmers and 2 heaters
*/
json_t * b_device_overview (json_t * device, void * device_ptr) {
y_log_message(Y_LOG_LEVEL_INFO, "device-mock - Running command overview for device %s", json_string_value(json_object_get(device, "name")));
json_t * sw1 = b_device_get_switch(device, "sw1", device_ptr),
* sw2 = b_device_get_switch(device, "sw2", device_ptr),
* di1 = b_device_get_dimmer(device, "di1", device_ptr),
* di2 = b_device_get_dimmer(device, "di2", device_ptr),
* he1 = b_device_get_heater(device, "he1", device_ptr),
* he2 = b_device_get_heater(device, "he2", device_ptr);
json_object_del(he1, "result");
json_object_del(he2, "result");
json_t * result = json_pack("{sis{s{sssf}s{sosf}}s{sIsI}s{sIsI}s{soso}}",
"result",
WEBSERVICE_RESULT_OK,
"sensors",
"se1",
"unit", "C",
"value", get_sensor_value("se1"),
"se2",
"trigger", json_true(),
"value", get_sensor_value("se2"),
"switches",
"sw1", json_integer_value(json_object_get(sw1, "value")),
"sw2", json_integer_value(json_object_get(sw2, "value")),
"dimmers",
"di1", json_integer_value(json_object_get(di1, "value")),
"di2", json_integer_value(json_object_get(di2, "value")),
"heaters",
"he1", he1,
"he2", he2);
json_decref(sw1);
json_decref(sw2);
json_decref(di1);
json_decref(di2);
return result;
}
void init(int use_http, char *address, int port){
// Setup periferal and library
init_gui();
init_motors();
init_sensor();
init_compass();
calibrate_compass(0); // 0 = do not execute a new calibration of the compass
// Load ball predefined positions
get_positions(POINTS_FILE_NAME, &positions, &n_points);
// Start bluetooth
if(use_http)
bluetooth_test_init(address, port);
else
bluetooth_init();
//bluetooth_register_and_start(fAction_t, fAck_t, fLead_t, fStart_t, fStop_t, fWait_t, fKick_t, fCancel_t);
bluetooth_register_and_start(&follower_action_cb, NULL, &lead_cb, &start_cb, &stop_cb, NULL, NULL, &cancel_cb);
// Reset global variables
send_action_flag = 1;
act = 0;
end_game = 0;
start_game = 0;
wait = 0;
cancel = 0;
// Waiting for the game start
set_light(LIT_LEFT, LIT_GREEN);
set_light(LIT_RIGHT, LIT_GREEN);
while(!start_game); //start game, robot rank and snake size initialized by start_cb
// Initial position
home.x = BORDER_X_MAX/2;
home.y = (snake_size - robot_rank -1)*40 + 20;
center.x = BORDER_X_MAX/4;
center.y = BORDER_Y_MAX/2;
robot = home;
printf("[DEBUG] Starting position X: %d, Y: %d, T: %d\n", robot.x , robot.y, robot.t);
update_sensor(SENSOR_GYRO);
gyro_init_val = get_sensor_value(SENSOR_GYRO);
set_light(LIT_LEFT, LIT_OFF);
set_light(LIT_RIGHT, LIT_OFF);
}
int get_gyro_value(){
update_sensor(SENSOR_GYRO);
return (get_sensor_value(SENSOR_GYRO) - gyro_init_val);
}
void active_sensor_update(ActiveSensor *active_sensor,
SensorsApplet *sensors_applet) {
GtkTreeModel *model;
GtkTreeIter iter;
GtkTreePath *path;
/* instance data from the tree for this sensor */
gchar *sensor_path = NULL;
gchar *sensor_id = NULL;
gchar *sensor_label = NULL;
SensorType sensor_type;
gchar *sensor_interface;
gboolean sensor_enabled;
gdouble sensor_low_value;
gdouble sensor_high_value;
gboolean sensor_alarm_enabled;
gdouble sensor_multiplier;
gdouble sensor_offset;
gdouble sensor_value;
GdkPixbuf *icon_pixbuf;
gchar *graph_color;
/* to build the list of labels as we go */
gchar *value_text = NULL;
gchar *old_value_text;
TemperatureScale scale;
DisplayMode display_mode;
GError *error = NULL;
gchar *tooltip = NULL;
gchar *value_tooltip = NULL;
/* hidden gsettings options */
gint font_size = 0;
gboolean hide_units = FALSE;
g_assert(active_sensor);
g_assert(active_sensor->sensor_row);
g_assert(sensors_applet);
model = gtk_tree_row_reference_get_model(active_sensor->sensor_row);
path = gtk_tree_row_reference_get_path(active_sensor->sensor_row);
/* if can successfully get iter can proceed */
if (gtk_tree_model_get_iter(model, &iter, path)) {
gtk_tree_path_free(path);
gtk_tree_model_get(GTK_TREE_MODEL(sensors_applet->sensors),
&iter,
PATH_COLUMN, &sensor_path,
ID_COLUMN, &sensor_id,
LABEL_COLUMN, &sensor_label,
INTERFACE_COLUMN, &sensor_interface,
SENSOR_TYPE_COLUMN, &sensor_type,
ENABLE_COLUMN, &sensor_enabled,
LOW_VALUE_COLUMN, &sensor_low_value,
HIGH_VALUE_COLUMN, &sensor_high_value,
ALARM_ENABLE_COLUMN, &sensor_alarm_enabled,
MULTIPLIER_COLUMN, &sensor_multiplier,
OFFSET_COLUMN, &sensor_offset,
ICON_PIXBUF_COLUMN, &icon_pixbuf,
GRAPH_COLOR_COLUMN, &graph_color,
-1);
SensorsAppletPluginGetSensorValue get_sensor_value;
/* only call function if is in hash table for plugin */
if ((get_sensor_value = sensors_applet_plugins_get_sensor_value_func(sensors_applet, sensor_interface)) != NULL) {
sensor_value = get_sensor_value(sensor_path,
sensor_id,
sensor_type,
&error);
if (error) {
g_debug("Error updating active sensor: %s", error->message);
sensors_applet_notify_active_sensor(active_sensor,
SENSOR_INTERFACE_ERROR);
/* hard code text as ERROR */
value_text = g_strdup(_("ERROR"));
value_tooltip = g_strdup_printf("- %s", error->message);
g_error_free(error);
error = NULL;
/* set sensor value to an error code -
* note this is not unique */
sensor_value = -1;
} else {
/* use hidden gsettings key for hide_units */
hide_units = g_settings_get_boolean(sensors_applet->settings, HIDE_UNITS);
/* scale value and set text using this
* value */
switch (sensor_type) {
case TEMP_SENSOR:
scale = (TemperatureScale) g_settings_get_int(sensors_applet->settings, TEMPERATURE_SCALE);
/* scale value */
sensor_value = sensors_applet_convert_temperature(sensor_value,
CELSIUS,
scale);
sensor_value = (sensor_value * sensor_multiplier) + sensor_offset;
switch (scale) {
case FAHRENHEIT:
value_text = g_strdup_printf("%2.0f %s", sensor_value, (hide_units ? "" : UNITS_FAHRENHEIT));
/* tooltip should
* always display
* units */
value_tooltip = g_strdup_printf("%2.0f %s", sensor_value, UNITS_FAHRENHEIT);
break;
case CELSIUS:
value_text = g_strdup_printf("%2.0f %s", sensor_value, (hide_units ? "" : UNITS_CELSIUS));
value_tooltip = g_strdup_printf("%2.0f %s", sensor_value, UNITS_CELSIUS);
break;
case KELVIN:
value_text = g_strdup_printf("%2.0f", sensor_value);
value_tooltip = g_strdup(value_text);
break;
}
break;
case FAN_SENSOR:
sensor_value = (sensor_value * sensor_multiplier) + sensor_offset;
value_text = g_strdup_printf("%4.0f %s", sensor_value, (hide_units ? "" : UNITS_RPM));
value_tooltip = g_strdup_printf("%4.0f %s", sensor_value, UNITS_RPM);
break;
case VOLTAGE_SENSOR:
sensor_value = (sensor_value * sensor_multiplier) + sensor_offset;
value_text = g_strdup_printf("%4.2f %s", sensor_value, (hide_units ? "" : UNITS_VOLTAGE));
value_tooltip = g_strdup_printf("%4.2f %s", sensor_value, UNITS_VOLTAGE);
break;
case CURRENT_SENSOR:
sensor_value = (sensor_value * sensor_multiplier) + sensor_offset;
value_text = g_strdup_printf("%4.2f %s", sensor_value, (hide_units ? "" : UNITS_CURRENT));
value_tooltip = g_strdup_printf("%4.2f %s", sensor_value, UNITS_CURRENT);
break;
} /* end switch(sensor_type) */
} /* end else on error */
/* setup for tooltips */
tooltip = g_strdup_printf("%s %s", sensor_label, value_tooltip);
g_free(value_tooltip);
/* only do icons and labels / graphs if needed */
display_mode = g_settings_get_int (sensors_applet->settings, DISPLAY_MODE);
/* most users wont have a font size set */
font_size = g_settings_get_int (sensors_applet->settings, FONT_SIZE);
/* do icon if needed */
if (display_mode == DISPLAY_ICON ||
display_mode == DISPLAY_ICON_WITH_VALUE) {
/* update icon if icon range has changed if no
* update has been done before */
if ((sensor_value_range(sensor_value, sensor_low_value, sensor_high_value) != sensor_value_range(active_sensor->sensor_values[0], active_sensor->sensor_low_value, active_sensor->sensor_high_value)) || !(active_sensor->updated)) {
active_sensor_update_sensor_value(active_sensor,
sensor_value);
active_sensor->sensor_low_value = sensor_low_value;
active_sensor->sensor_high_value = sensor_high_value;
active_sensor_update_icon(active_sensor, icon_pixbuf, sensor_type);
}
/* always update tooltip */
gtk_widget_set_tooltip_text(active_sensor->icon,
tooltip);
}
active_sensor_update_sensor_value(active_sensor,
sensor_value);
active_sensor->sensor_low_value = sensor_low_value;
active_sensor->sensor_high_value = sensor_high_value;
/* do graph if needed */
if (display_mode == DISPLAY_GRAPH) {
/* update graph color in case has changed */
gdk_color_parse(graph_color,
&(active_sensor->graph_color));
gtk_widget_queue_draw (active_sensor->graph);
gtk_widget_set_tooltip_text(active_sensor->graph,
tooltip);
}
old_value_text = value_text;
if (sensor_alarm_enabled) {
if (sensor_value >= sensor_high_value ||
sensor_value <= sensor_low_value) {
/* make value text red and
* activate alarm */
if (display_mode == DISPLAY_LABEL_WITH_VALUE ||
display_mode == DISPLAY_ICON_WITH_VALUE ||
display_mode == DISPLAY_VALUE) {
value_text = g_markup_printf_escaped("<span foreground=\"#FF0000\">%s</span>", old_value_text);
g_free(old_value_text);
}
/* could have both coditions at once */
if (sensor_value >= sensor_high_value) {
active_sensor_alarm_on(active_sensor, HIGH_ALARM);
}
if (sensor_value <= sensor_low_value) {
active_sensor_alarm_on(active_sensor, LOW_ALARM);
}
} else {
/* make sure alarms are off */
active_sensor_all_alarms_off(active_sensor);
}
} else { /* else for if alarm enabled */
/* make sure all alarms are off */
active_sensor_all_alarms_off(active_sensor);
}
/* do value label */
if (display_mode == DISPLAY_LABEL_WITH_VALUE ||
display_mode == DISPLAY_ICON_WITH_VALUE ||
display_mode == DISPLAY_VALUE) {
if (font_size) {
old_value_text = value_text;
value_text = g_strdup_printf("<span font_desc=\"%d\">%s</span>", font_size, old_value_text);
g_free(old_value_text);
}
gtk_label_set_markup(GTK_LABEL(active_sensor->value),
value_text);
gtk_widget_set_tooltip_text(active_sensor->value,
tooltip);
}
/* finished with value text */
g_free(value_text);
/* do label label */
if (display_mode == DISPLAY_LABEL_WITH_VALUE) {
if (font_size) {
old_value_text = sensor_label;
sensor_label = g_strdup_printf("<span font_desc=\"%d\">%s</span>", font_size, old_value_text);
g_free(old_value_text);
}
gtk_label_set_markup(GTK_LABEL(active_sensor->label),
sensor_label);
gtk_widget_set_tooltip_text(active_sensor->label,
tooltip);
}
g_free(tooltip);
} else {
g_debug("no get_sensor_value function yet installed for interface %s.", sensor_interface);
}
g_free(sensor_path);
g_free(sensor_id);
g_free(sensor_label);
g_free(sensor_interface);
g_free(graph_color);
g_object_unref(icon_pixbuf);
} else {
g_debug("Error getting iter when updating sensor...");
}
active_sensor->updated = TRUE;
}
