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);
}
int get_ball(){
int tries = 2; //number of tries when scanning to find the ball
int found = 0;
int angle;
int dist;
while(tries > 0){
found = scan_for_ball();
if(found == 1){
// Min point found
if(min.d <120){
// Grab the ball
set_motors_speed(NORMAL_SPEED);
angle = min.d * 2.1 - 67;
run_relative(angle);
while(!command_finish());
dist = (angle*18.0/360);
robot.x += dist * sin(robot.t);
robot.y += dist * cos(robot.t);
//add check that the ball is really there
tries = (grab_and_check())? 0: 2;
}else{
// Avvicinati
set_motors_speed(NORMAL_SPEED);
angle = (int)((min.d * 2.1 -67)*3/4);
run_relative(angle);
dist = (angle*18.0/360);
robot.x += dist * sin(robot.t);
robot.y += dist * cos(robot.t);
while(!command_finish());
}
}else{
// Minimum not found
//set_motors_speed(NORMAL_SPEED);
//run_relative(0);
//while(!command_finish());
tries--;
}
}
set_light(LIT_LEFT, LIT_OFF);
set_light(LIT_RIGHT, LIT_OFF);
//update robot angle
robot.t = get_gyro_value();
return 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;
}
//this function makes the robot avoid the obstacle running for a small distance in another direction
//this direction depends on the current angle and position
void obstacle_avoid(){
int quadrant = robot.t / 90;
int right_left_n = robot.x / BORDER_X_MAX/2;
int up_down_n = robot.y / BORDER_Y_MAX/2;
int angles[4][2][2];
//first quadrant
angles[0][0][0]=45; //down left
angles[0][0][1]=-45; //down right
angles[0][1][0]=45; //up left
angles[0][1][1]=-45; //up right
//second quadrant
angles[1][0][0]=-45; //down left
angles[1][0][1]=-45; //down right
angles[1][1][0]=45; //up left
angles[1][1][1]=45; //up right
//third quadrant
angles[2][0][0]=-45; //down left
angles[2][0][1]=45; //down right
angles[2][1][0]=-45; //up left
angles[2][1][1]=45; //up right
//fourth quadrant
angles[3][0][0]=45; //down left
angles[3][0][1]=45; //down right
angles[3][1][0]=-45; //up left
angles[3][1][1]=-45; //up right
int angle=(robot.t+angles[quadrant][up_down_n][right_left_n])%360;
if(robot_rank ==0 && send_action_flag) send_action(next,angle,40,NORMAL_SPEED/2);
set_motors_speed(NORMAL_SPEED);
turn_absolute_fb(angle,get_cal_location(robot.x,robot.y));
robot.t=angle;
run_relative((int)40*360.0/18);
while(!command_finish());
robot.x= robot.x + 40*sin(robot.t*M_PI/180);
robot.y= robot.y + 40*cos(robot.t*M_PI/180);
printf("\n\n>>>>>>>>pos %d %d %d\n\n",robot.x,robot.y,robot.t);
}
int main(int argc, char *argv[])
{
struct options options;
clock_t start;
bool create_db;
GError *error = NULL;
bool success;
daemonize_close_stdin();
#ifdef HAVE_LOCALE_H
/* initialize locale */
setlocale(LC_CTYPE,"");
#endif
g_set_application_name("Music Player Daemon");
/* enable GLib's thread safety code */
g_thread_init(NULL);
winsock_init();
idle_init();
dirvec_init();
songvec_init();
tag_pool_init();
config_global_init();
success = parse_cmdline(argc, argv, &options, &error);
if (!success) {
g_warning("%s", error->message);
g_error_free(error);
return EXIT_FAILURE;
}
glue_daemonize_init(&options);
stats_global_init();
tag_lib_init();
log_init(options.verbose, options.log_stderr);
success = listen_global_init(&error);
if (!success) {
g_warning("%s", error->message);
g_error_free(error);
return EXIT_FAILURE;
}
daemonize_set_user();
main_task = g_thread_self();
main_loop = g_main_loop_new(NULL, FALSE);
main_cond = g_cond_new();
event_pipe_init();
event_pipe_register(PIPE_EVENT_IDLE, idle_event_emitted);
path_global_init();
glue_mapper_init();
initPermissions();
playlist_global_init();
spl_global_init();
#ifdef ENABLE_ARCHIVE
archive_plugin_init_all();
#endif
decoder_plugin_init_all();
update_global_init();
create_db = !glue_db_init_and_load();
glue_sticker_init();
command_init();
initialize_decoder_and_player();
volume_init();
initAudioConfig();
audio_output_all_init();
client_manager_init();
replay_gain_global_init();
if (!input_stream_global_init(&error)) {
g_warning("%s", error->message);
g_error_free(error);
return EXIT_FAILURE;
}
playlist_list_global_init();
daemonize(options.daemon);
setup_log_output(options.log_stderr);
initSigHandlers();
initZeroconf();
player_create();
if (create_db) {
/* the database failed to load: recreate the
database */
unsigned job = update_enqueue(NULL, true);
if (job == 0)
g_error("directory update failed");
}
glue_state_file_init();
success = config_get_bool(CONF_AUTO_UPDATE, false);
#ifdef ENABLE_INOTIFY
if (success && mapper_has_music_directory())
mpd_inotify_init();
#else
if (success)
g_warning("inotify: auto_update was disabled. enable during compilation phase");
#endif
config_global_check();
/* enable all audio outputs (if not already done by
playlist_state_restore() */
pc_update_audio();
/* run the main loop */
g_main_loop_run(main_loop);
/* cleanup */
g_main_loop_unref(main_loop);
#ifdef ENABLE_INOTIFY
mpd_inotify_finish();
#endif
state_file_finish();
pc_kill();
finishZeroconf();
client_manager_deinit();
listen_global_finish();
playlist_global_finish();
start = clock();
db_finish();
g_debug("db_finish took %f seconds",
((float)(clock()-start))/CLOCKS_PER_SEC);
#ifdef ENABLE_SQLITE
sticker_global_finish();
#endif
g_cond_free(main_cond);
event_pipe_deinit();
playlist_list_global_finish();
input_stream_global_finish();
audio_output_all_finish();
volume_finish();
mapper_finish();
path_global_finish();
finishPermissions();
pc_deinit();
command_finish();
update_global_finish();
decoder_plugin_deinit_all();
#ifdef ENABLE_ARCHIVE
archive_plugin_deinit_all();
#endif
config_global_finish();
tag_pool_deinit();
songvec_deinit();
dirvec_deinit();
idle_deinit();
stats_global_finish();
daemonize_finish();
#ifdef WIN32
WSACleanup();
#endif
close_log_files();
return EXIT_SUCCESS;
}
//return REACHED if position reached, OBSTACLE if stopped because of obstacle A CUSTOM STOP MESSAGE MUST BE SENT, CUSTOM_STOP_MESSAGE if stopped because a custom message was sent
//robot position is updated
//position is reached running for distance in steps <=200 (first the dist%200 than n steps dist/200)
//it sleeps 1s after each step
int go_to_position(pos_t target_pos, int margin, int speed){
int32_t tacho_dist, start_tacho, end_tacho;
int angle, dist;
int i;
if(robot_rank == 0) while(wait);
compute_polar_coord(target_pos, &dist, &angle,margin);
target_pos.x = robot.x + dist*sin(angle*M_PI/180);
target_pos.y = robot.y + dist*cos(angle*M_PI/180);
//start movement, in two steps if bigger than 200
int div = dist/200;
int mod = dist%200;
int step;
for(i = 0; i < div+1 ; i++){
if(i == div) step = mod;
else step = 200;
if(robot_rank == 0 && send_action_flag) send_action(next, angle, step, speed/2);
//vado alla posizione
set_motors_speed(SLOW_SPEED);
turn_absolute_fb(angle, get_cal_location(robot.x,robot.y));
//aggiorno posizione final;
robot.t = angle;//get_gyro_value();
printf("robot.t = %d\n",robot.t);
//save initial tacho counts
start_tacho=get_motorR_position();
set_motors_speed(speed);
run_relative((int)(step*360.0/18));
while(!command_finish()){
if(cancel){
//cancel = 0;
run_relative(0);
end_tacho = get_motorR_position();
tacho_dist = end_tacho-start_tacho;
dist = tacho_dist/20.0;
robot.x = robot.x + dist*sin(robot.t*M_PI/180);
robot.y = robot.y + dist*cos(robot.t*M_PI/180);
printf("\n\n>>>>>>>>pos %d %d %d\n\n",robot.x,robot.y,robot.t);
if(send_action_flag) send_cancel(next, dist);
// Send to ourself an action in order came closer
act_val.dist = cancel - dist;
act_val.angle = robot.t;
act++;
return CUSTOM_STOP_MESSAGE;
}
if(obstacle()){
run_relative(0);
end_tacho = get_motorR_position();
tacho_dist = end_tacho-start_tacho;
dist = tacho_dist/20.0;
printf("step %d dist %d\n",step,dist);
if(step-dist < STOP_DIST*18/360.0){
set_motors_speed(speed);
run_relative((int)((step-dist)*360.0/18));
return REACHED;
}
printf("tacho dist %ld dist %ld\n",tacho_dist,dist);
robot.x = robot.x + dist*sin(robot.t*M_PI/180);
robot.y = robot.y + dist*cos(robot.t*M_PI/180);
printf("\n\n>>>>>>>>pos %d %d %d\n\n",robot.x,robot.y,robot.t);
if(send_action_flag) send_cancel(next,dist);
// HERE WE MUST SEND A CUSTOM MESSAGE STOP TO FOLLOWING ROBOT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
// HERE WE MUST SEND A CUSTOM MESSAGE STOP TO FOLLOWING ROBOT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
return OBSTACLE;
}
}
sleep(1);
}
//update pos
robot.x = target_pos.x;
robot.y = target_pos.y;
printf("\n\n>>>>>>>>pos %d %d %d\n\n",robot.x,robot.y,robot.t);
return REACHED;
}
void drop_the_ball(){
set_motors_speed(NORMAL_SPEED);
set_motor_arm_position(MOTOR_ARM_DOWN);
while(!command_finish());
sleep(5);
}
int
main (int argc, char **argv)
{
uint32_t c;
char *config_file;
bool custom_config;
char *tmpdir;
struct sigaction sigact;
set_quoting_style(NULL, escape_quoting_style);
if (setlocale(LC_ALL, "") == NULL)
warn(_("%s: Cannot set locale: %s\n"), argv[0], errstr);
#ifdef ENABLE_NLS
if (bindtextdomain(PACKAGE, LOCALEDIR) == NULL)
warn(_("%s: Cannot bind message domain: %s\n"), argv[0], errstr);
if (textdomain(PACKAGE) == NULL)
warn(_("%s: Cannot set message domain: %s\n"), argv[0], errstr);
#endif
custom_config = false;
get_package_file("config", &config_file);
while (true) {
c = getopt_long(argc, argv, short_opts, long_opts, NULL);
if (c == -1)
break;
switch (c) {
case 'c': /* --config */
custom_config = true;
free(config_file);
config_file = xstrdup(optarg);
break;
case 'n': /* --no-config */
free(config_file);
config_file = NULL;
break;
case HELP_OPT: /* --help */
printf(_("Usage: %s [OPTION]...\n"), quotearg(argv[0]));
puts(_("Start microdc, a command-line based Direct Connect client.\n"));
printf(_(" -n, --no-config do not read config file on startup\n"));
printf(_(" --help display this help and exit\n"));
printf(_(" --version output version information and exit\n"));
printf(_("\nReport bugs to <%s>.\n"), PACKAGE_BUGREPORT);
exit(EXIT_SUCCESS);
case VERSION_OPT: /* --version */
version_etc(stdout, NULL, PACKAGE, VERSION, /*"Oskar Liljeblad",*/ "Vladimir Chugunov", NULL);
exit(EXIT_SUCCESS);
default:
exit(EXIT_FAILURE);
}
}
if (pipe(signal_pipe) < 0) {
warn(_("Cannot create pipe pair - %s\n"), errstr);
goto cleanup;
}
main_process_id = getpid();
sigact.sa_handler = signal_received;
if (sigemptyset(&sigact.sa_mask) < 0) {
warn(_("Cannot empty signal set - %s\n"), errstr);
goto cleanup;
}
sigact.sa_flags = SA_RESTART;
#ifdef HAVE_STRUCT_SIGACTION_SA_RESTORER
sigact.sa_restorer = NULL;
#endif
/* Note: every signal registered with a non-ignore action here must
* also be registered in user.c, either with an action or as ignored.
*/
if (sigaction(SIGINT, &sigact, NULL) < 0 ||
sigaction(SIGTERM, &sigact, NULL) < 0 ||
sigaction(SIGUSR1, &sigact, NULL) < 0 ||
sigaction(SIGCHLD, &sigact, NULL) < 0) {
warn(_("Cannot register signal handler - %s\n"), errstr);
goto cleanup;
}
sigact.sa_handler = SIG_IGN;
if (sigaction(SIGPIPE, &sigact, NULL) < 0) {
warn(_("Cannot register signal handler - %s\n"), errstr);
goto cleanup;
}
FD_ZERO(&read_fds);
FD_ZERO(&write_fds);
FD_SET(signal_pipe[0], &read_fds);
/*FD_SET(STDIN_FILENO, &read_fds);*/
hub_recvq = byteq_new(128);
hub_sendq = byteq_new(128);
user_conns = hmap_new();
hub_users = hmap_new();
pending_userinfo = hmap_new();
set_main_charset("");
set_hub_charset("");
set_fs_charset("");
user_conn_unknown_free = ptrv_new();
delete_files = ptrv_new();
delete_dirs = ptrv_new();
search_udpmsg_out = ptrv_new();
our_searches = ptrv_new();
search_recvq = byteq_new(8192); // same size as DC++
my_nick = xstrdup(PACKAGE);
my_description = xstrdup("");
my_email = xstrdup("");
my_speed = xstrdup("56Kbps");
my_tag = xasprintf("%s V:%s", PACKAGE, VERSION);
download_dir = xstrdup(".");
tmpdir = tempdir();
if (tmpdir == NULL) {
warn(_("Cannot find directory for temporary files - %s\n"), errstr);
goto cleanup;
}
{
char *filename = xasprintf("%s.%d", PACKAGE, getpid());
listing_dir = catfiles(tmpdir, filename);
free(filename);
}
ptrv_append(delete_dirs, xstrdup(listing_dir));
is_active = false;
listen_port = 0;
if (!local_file_list_update_init())
goto cleanup;
if (!set_active(false, listen_port))
goto cleanup;
if (!enable_search())
goto cleanup;
my_ul_slots = 3;
if (!lookup_init())
goto cleanup;
if (!file_list_parse_init())
goto cleanup;
command_init();
if (!local_file_list_init()) {
goto cleanup;
}
if (config_file != NULL) {
run_script(config_file, !custom_config);
free(config_file);
config_file = NULL;
}
screen_prepare();
while (running) {
fd_set res_read_fds;
fd_set res_write_fds;
int res;
struct timeval tv;
tv.tv_sec = 1;
tv.tv_usec = 0;
screen_redisplay_prompt();
res_read_fds = read_fds;
res_write_fds = write_fds;
res = TEMP_FAILURE_RETRY(select(FD_SETSIZE, &res_read_fds, &res_write_fds, NULL, &tv));
if (res < 0) {
warn(_("Cannot select - %s\n"), errstr);
break;
}
if (running && FD_ISSET(signal_pipe[0], &res_read_fds))
read_signal_input();
if (running && FD_ISSET(STDIN_FILENO, &res_read_fds))
screen_read_input();
if (running && listen_socket >= 0 && FD_ISSET(listen_socket, &res_read_fds))
handle_listen_connection();
if (running && hub_socket >= 0 && FD_ISSET(hub_socket, &res_read_fds))
hub_input_available();
if (running && hub_socket >= 0 && FD_ISSET(hub_socket, &res_write_fds))
hub_now_writable();
if (running)
check_hub_activity();
if (running && search_socket >= 0 && FD_ISSET(search_socket, &res_read_fds))
search_input_available();
if (running && search_socket >= 0 && FD_ISSET(search_socket, &res_write_fds))
search_now_writable();
if (running && FD_ISSET(lookup_request_mq->fd, &res_write_fds))
lookup_request_fd_writable();
if (running && FD_ISSET(lookup_result_mq->fd, &res_read_fds))
lookup_result_fd_readable();
if (running && FD_ISSET(parse_request_mq->fd, &res_write_fds))
parse_request_fd_writable();
if (running && FD_ISSET(parse_result_mq->fd, &res_read_fds))
parse_result_fd_readable();
if (running && FD_ISSET(update_request_mq->fd, &res_write_fds))
update_request_fd_writable();
if (running && FD_ISSET(update_result_mq->fd, &res_read_fds))
update_result_fd_readable();
if (running) {
HMapIterator it;
hmap_iterator(user_conns, &it);
while (running && it.has_next(&it)) {
DCUserConn *uc = (DCUserConn*) it.next(&it);
if (uc->put_mq != NULL && FD_ISSET(uc->put_mq->fd, &res_write_fds))
user_request_fd_writable(uc);
if (uc->get_mq != NULL && FD_ISSET(uc->get_mq->fd, &res_read_fds))
user_result_fd_readable(uc);
}
}
}
cleanup:
hub_disconnect();
screen_finish();
command_finish();
local_file_list_update_finish();
file_list_parse_finish();
lookup_finish();
byteq_free(hub_recvq);
byteq_free(hub_sendq);
hmap_free(hub_users); /* Emptied by hub_disconnect */
hmap_free(pending_userinfo); /* Emptied by hub_disconnect */
byteq_free(search_recvq);
ptrv_foreach(user_conn_unknown_free, free);
ptrv_free(user_conn_unknown_free);
ptrv_foreach(search_udpmsg_out, free);
ptrv_free(search_udpmsg_out);
ptrv_foreach(our_searches, (PtrVForeachCallback) free_search_request);
ptrv_free(our_searches);
hmap_foreach_value(user_conns, (void (*) (void*)) user_conn_cancel);
/* XXX: follow up and wait for user connections to die? */
hmap_free(user_conns);
if (our_filelist != NULL)
filelist_free(our_filelist);
set_main_charset(NULL);
set_hub_charset(NULL);
set_fs_charset(NULL);
free(hub_name);
free(my_nick);
free(my_description);
free(my_email);
free(my_speed);
free(my_tag);
free(download_dir);
free(listing_dir);
if (delete_files != NULL) {
for (c = 0; c < delete_files->cur; c++) {
char *filename = (char*) delete_files->buf[c];
struct stat st;
if (stat(filename, &st) < 0) {
if (errno != ENOENT)
warn(_("%s: Cannot get file status - %s\n"), quotearg(filename), errstr);
free(filename);
continue;
}
if (unlink(filename) < 0)
warn(_("%s: Cannot remove file - %s\n"), quotearg(filename), errstr);
free(filename);
}
ptrv_free(delete_files);
}
if (delete_dirs != NULL) {
for (c = 0; c < delete_dirs->cur; c++) {
char *filename = (char*) delete_dirs->buf[c];
struct stat st;
if (stat(filename, &st) < 0) {
if (errno != ENOENT)
warn(_("%s: Cannot get file status - %s\n"), quotearg(filename), errstr);
free(filename);
continue;
}
if (rmdir(filename) < 0)
warn(_("%s: Cannot remove file - %s\n"), quotearg(filename), errstr);
free(filename);
}
ptrv_free(delete_dirs);
}
if (search_socket >= 0 && close(search_socket) < 0)
warn(_("Cannot close search results socket - %s\n"), errstr);
if (listen_socket >= 0 && close(listen_socket) < 0)
warn(_("Cannot close user connections socket - %s\n"), errstr);
if (signal_pipe[0] >= 0 && close(signal_pipe[0]) < 0)
warn(_("Cannot close signal pipe - %s\n"), errstr);
if (signal_pipe[1] >= 0 && close(signal_pipe[1]) < 0)
warn(_("Cannot close signal pipe - %s\n"), errstr);
free(config_file);
exit(EXIT_SUCCESS);
}
