static void pars_cells(t_game *game, t_player *player, char **msg)
{
unsigned int max_w;
unsigned int cur_h;
unsigned int cur_w;
t_coord2d pos;
printf("voir\n");
max_w = 1;
cur_h = -1;
pos.x = player->pos.x;
pos.y = player->pos.y;
while (++cur_h < player->level + 1)
{
init_position(player, &pos.x, &pos.y, max_w);
cur_w = -1;
while (++cur_w < max_w)
{
get_description_of_cell(game, player, &pos, msg);
inc_coord_w(player->direction, &pos.x, &pos.y);
}
inc_coord_h(player->direction, &pos.x, &pos.y);
max_w += 2;
}
}
void tex_monster(t_game *game, int i, int *x, int *y, SDL_Surface *surf)
{
game->Gmonster[i].monster = SDL_CreateTextureFromSurface(game->Grenderer, surf);
if (game->Gmonster[i].monster == NULL)
{
printf( "Load texture error %s for the monsters! SDL Error: %s\n",
"/../../../images/monster1.png", SDL_GetError() );
SDL_Quit();
exit(EXIT_FAILURE);
}
game->Gmonster[i].position = init_position(*x, *y, 20, 20);
if (*y < 110)
game->Gmonster[i].score = 30;
else if (*y > 110 && *y < 180)
game->Gmonster[i].score = 20;
else
game->Gmonster[i].score = 10;
if ((i + 1) % 10 != 0 || i == 0)
*x += 30;
else
{
*x = 10;
*y += 30;
}
}
void mtsl_init(){
#ifdef __START_POSITION_MODE__
init_position();
#endif
my_info.id = LOCAL_ADDR;
my_info.x = nb_table[LOCAL_ADDR].x;
my_info.y = nb_table[LOCAL_ADDR].y;
my_info.level = MTSL_LEVEL0;
}
int main(int argc, char* argv[]) {
store_map();
init_position(0);
display();
printf("prediction\n");
prediction(1);// we move from one meter to the right
display();
printf("estimation\n");
estimation(wall,wall);
normalization();
display();
printf("prediction\n");
prediction(1);// we move from one meter to the right
display();
printf("estimation\n");
estimation(wall,wall);
normalization();
display();
printf("prediction\n");
prediction(1);// we move from one meter to the right
display();
printf("estimation\n");
estimation(wall,wall);
normalization();
display();
printf("prediction\n");
prediction(1);// we move from one meter to the right
display();
printf("estimation\n");
estimation(wall,wall);
normalization();
display();
printf("prediction\n");
prediction(1);// we move from one meter to the right
display();
printf("estimation\n");
estimation(wall,wall);
normalization();
display();
return 0;
}// main
t_game init_player(t_game game)
{
game.Gplayer1.position = init_position(SCREEN_WIDTH /
2, SCREEN_HEIGHT - 100, 35, 35);
game.Gplayer1.nbr_bullet = 0;
game.Gplayer1.score = 0;
game.Gplayer1.lives = 3;
game.Gplayer2.nbr_bullet = 0;
game.Gplayer2.score = 0;
game.Gplayer2.lives = 3;
return (game);
}
/*
** Connexion d un nouveau joueur
*/
static void set_new_ia(t_world *world, t_cli *ia, t_team *team)
{
init_position(world, ia);
gen_ressource(world);
ia->statut = NOACTION;
ia->lvl = 1;
ia->team = team;
init_inventaire(&(ia->invent));
ia->invent.objet[FOOD] = DEFAULT_FOOD;
ia->elevation = 0;
set_timeval(world, &(ia->t_cur_cmd), NOACTION);
set_timeval(world, &(ia->t_alive), LIFETIME);
welcome_ia(world, team, ia);
if (world->pgui != NULL)
gui_pnw_serv(world, ia);
}
void init_graph()
{
init_position();
for(int i=0;i<n;i++)
{
for(int j=0;j<m;j++)
{
if(get_position(i,j)<point && get_position(i,j)!=-1)
{
graph[get_position(i,j)][0]=get_position(i-1,j);
graph[get_position(i,j)][1]=get_position(i+1,j);
graph[get_position(i,j)][2]=get_position(i,j-1);
graph[get_position(i,j)][3]=get_position(i,j+1);
}
}
}
}
t_game showGameOver(t_game game)
{
game.Gscreen = loadTexture( "/../../../images/background/main_menu.png", game.Grenderer );
game.begin.surface_play = TTF_RenderText_Solid(game.text.font, "GAME OVER", game.text.color);
game.begin.play_with_2 = SDL_CreateTextureFromSurface(game.Grenderer, game.begin.surface_play);
game.begin.play_with_2_position = init_position(135, 180, 180, 500);
SDL_Delay(50);
game.begin.state = 1;
if (game.Gplayer1.lives == -1)
{
write_score(game, game.Gplayer1);
game = init_player(game);
}
set_hscore(&game, get_hscore(hscore_path()));
render_hscore(&(game.text), game.Grenderer);
render_score(&(game.Gplayer1), game.text, game.Grenderer);
renderEnd(game);
return (game);
}
void realmain(int argc, char **argv)
{
char b[256];
struct stat statbuf;
/* vars from oldmain */
buffer *buf = NULL;
char path[1024];
int i;
setlocale(LC_NUMERIC, "C");
common_init("Egon Animator %s. No Warranty");
sprintf(b, "%s/%ld", siag_basedir, (long)getpid());
mkdir(b, 0700);
sprintf(b, "%s/egon.scm", siag_basedir);
egonrc = MwStrdup(b);
init_interpreters();
siod_interpreter = init_parser(argc, argv);
init_python_parser();
init_guile_parser();
init_ruby_parser();
waitforchild(0);
init_position();
init_cmds();
buf = new_buffer("noname.egon", "noname.egon");
sprintf(path, "%s/egon/egon.scm", datadir);
if (stat(path, &statbuf)) {
fprintf(stderr, "Can't find the runtime library (egon.scm).\n");
fprintf(stderr, "Expected it in %s\n", path);
fprintf(stderr, "SIAGHOME (if set) is '%s'\n", datadir);
fprintf(stderr, "Please read installation instructions.\n");
exit(EXIT_FAILURE);
}
setvar(cintern("libdir"), strcons(-1, libdir), NIL);
setvar(cintern("datadir"), strcons(-1, datadir), NIL);
setvar(cintern("docdir"), strcons(-1, docdir), NIL);
/* load runtime library */
sprintf(b, "(load \"%s/egon/egon.scm\")", datadir);
execute(b);
init_windows(buf, &argc, argv);
setlocale(LC_NUMERIC, "C"); /* possibly hosed by X */
/* load user customizations, if any */
if (!stat(egonrc, &statbuf)) {
sprintf(b, "(load \"%s\")", egonrc);
execute(b);
}
execute("(init-windows)");
execute("(create-menus)");
fileio_init();
for (i = 1; i < argc; i++) {
if (argv[i][0] != '-') {
strcpy(path, argv[i]);
free_buffer(w_list->buf);
buf = new_buffer(buffer_name(argv[i]), path);
loadmatrix(path, buf, guess_file_format(path));
buf->change = FALSE;
w_list->buf = buf;
}
}
pr_scr_flag = TRUE;
execute("(popup-editor)");
/* this works, for reasons beyond my comprehension */
execute("(print-version)");
execute("(print-version)");
activate_window(w_list);
#ifdef HAVE_LIBTCL
Tcl_Main(argc, argv, Tcl_AppInit);
#else
mainloop();
#endif
}
int main(int argc, char** argv)
{
init_robot(&robot);
struct coord *destination;
destination = new_coord(0, 0);
destination->x = 0;
destination->y = 0;
pthread_mutex_init(&state.frame_lock, NULL);
pthread_cond_init(&state.has_frame, NULL);
robot.dest = destination;
gpioSetMode(4, PI_OUTPUT);
gpioSetMode(24, PI_OUTPUT);
gpioSetMode(5, PI_OUTPUT);
gpioSetMode(14, PI_OUTPUT);
//870 for at the ground,
// gpioServo(24,1380);
// gpioServo(4, 2100);
pthread_t range_thread, render_thread, cv_thread;
pthread_create(&range_thread, NULL, &range_read, &robot);
pthread_create(&render_thread, NULL, &render_task, NULL);
// pthread_create(&cv_thread, NULL, &find_circles, &state);
gpioWrite(5, 0);
gpioWrite(14, 0);
start_control_panel(&robot, &state);
pthread_t this_thread = pthread_self();
//set_thread_priority(this_thread, 0);
//set_thread_priority(robot.server->serve_thread, 1);
//set_thread_priority(range_thread, 2);
read(robot.encoder_handle, robot.sensor_data, sizeof(robot.sensor_data));
getQuaternion(&robot.position.orientation, robot.sensor_data);
robot.position.last_left = *((int *) (robot.sensor_data + 42));
robot.position.last_right = *((int *) (robot.sensor_data + 46));
struct robot_task *position_task, *point_task, *remote_task,
*spin_task, *move_task, *adjust_task, *stand_up_task, *fly_task,
*avoid_task, *stay_task;
position_task = (struct robot_task *)malloc(sizeof(*position_task));
position_task->task_func = &update_position2;
fly_task = (struct robot_task *)malloc(sizeof(*fly_task));
fly_task->task_func = &buzz;
point_task = (struct robot_task *)malloc(sizeof(*point_task));
robot.turret.target.x = 500;
robot.turret.target.y = 0;
robot.turret.target.z = 0;
point_task->task_func = &pointer;
remote_task = (struct robot_task *)malloc(sizeof(*remote_task));
remote_task->task_func = &remote;
avoid_task = (struct robot_task *)malloc(sizeof(*avoid_task));
avoid_task->task_func = &stay_away;
stay_task = (struct robot_task *)malloc(sizeof(*stay_task));
stay_task->task_func = &stay_within;
move_task = (struct robot_task *)malloc(sizeof(*move_task));
move_task->task_func = &move;
struct waypoint the_point;
the_point.point.x = 1000;
the_point.point.y = 0;
the_point.point.z = -40;
INIT_LIST_HEAD(&the_point.list_entry);
// list_add(&the_point.list_entry, &robot.waypoints);
adjust_task = (struct robot_task *)malloc(sizeof(*adjust_task));
adjust_task->task_func = &adjust_speeds;
stand_up_task = (struct robot_task *)malloc(sizeof(*stand_up_task));
stand_up_task->task_func = &stand_up;
INIT_LIST_HEAD(&position_task->list_item);
INIT_LIST_HEAD(&fly_task->list_item);
INIT_LIST_HEAD(&point_task->list_item);
INIT_LIST_HEAD(&remote_task->list_item);
INIT_LIST_HEAD(&avoid_task->list_item);
INIT_LIST_HEAD(&stay_task->list_item);
INIT_LIST_HEAD(&move_task->list_item);
INIT_LIST_HEAD(&adjust_task->list_item);
INIT_LIST_HEAD(&stand_up_task->list_item);
list_add_tail(&position_task->list_item, &robot.task_list);
// list_add_tail(&fly_task->list_item, &robot.task_list);
list_add_tail(&point_task->list_item, &robot.task_list);
list_add_tail(&remote_task->list_item, &robot.task_list);
// list_add_tail(&avoid_task->list_item, &robot.task_list);
// list_add_tail(&stay_task->list_item, &robot.task_list);
list_add_tail(&move_task->list_item, &robot.task_list);
list_add_tail(&adjust_task->list_item, &robot.task_list);
list_add_tail(&stand_up_task->list_item, &robot.task_list);
get_sensor_data(&robot.position2, robot.sensor_data);
init_position(&robot.position2);
//Variables for Kalman filter
struct matrix_2x2 A, B, H, Q, R, current_prob_estimate;
float current_state_estimate[2];
current_state_estimate[0] = 0;
current_state_estimate[1] = 0;
A.elements[0][0] = 1;
A.elements[0][1] = 0;
A.elements[1][0] = 0;
A.elements[1][1] = 1;
Q.elements[0][0] = 0.01;
Q.elements[0][1] = 0;
Q.elements[1][0] = 0;
Q.elements[1][1] = 0.01;
R.elements[0][0] = 0.4;
R.elements[0][1] = 0;
R.elements[1][0] = 0;
R.elements[1][1] = 0.4;
current_prob_estimate.elements[0][0] = 1;
current_prob_estimate.elements[0][1] = 0;
current_prob_estimate.elements[1][0] = 0;
current_prob_estimate.elements[1][1] = 1;
static int look_wait = 0;
while(1)
{
read(robot.encoder_handle, robot.sensor_data, sizeof(robot.sensor_data));
get_sensor_data(&robot.position2, robot.sensor_data);
// fprintf(stderr, "%i, %i\n", robot.position2.left, robot.position2.right);
if (!((robot.position2.orientation.x == robot.position2.orientation.y) &&
(robot.position2.orientation.x == robot.position2.orientation.z) &&
(robot.position2.orientation.x == robot.position2.orientation.w)))
{
do_robot_tasks(&robot);
}
state.roll = robot.turret.roll * 180 / M_PI;
static struct vect sightings[3];
int has_dest = list_empty(&robot.waypoints);
int is_spinning = list_empty(&fly_task->list_item);
// fprintf(stderr, "%i, %i\n", has_dest, is_spinning);
//Prediction Step
float predicted_state_estimate[2];
mat_mult_2xv(predicted_state_estimate, &A, current_state_estimate);
struct matrix_2x2 predicted_prob_estimate;
mat_add_2x2(&predicted_prob_estimate, & current_prob_estimate, &Q);
if (!isnan(state.x_pos) && state.set_target >= 1)
{
float x_in_vid = (state.x_pos - .5) * -2;
float y_in_vid = (state.y_pos - .5) * -2;
// Get obervation.
screen_to_world_vect(&robot, &sightings[0], x_in_vid, y_in_vid);
float observation[2];
observation[0] = sightings[0].x;
observation[1] = sightings[0].y;
float innovation[2];
innovation[0] = observation[0] - predicted_state_estimate[0];
innovation[1] = observation[1] - predicted_state_estimate[1];
float innovation_magnitude = sqrtf((innovation[0] * innovation[0]) + (innovation [1] * innovation[1]));
//fprintf(stderr, "%f, %f, %f, %f, %f, %f, %f, %f\n", innovation_magnitude, robot.turret.yaw, robot.turret.pitch, x_in_vid, y_in_vid, sightings[0].x, sightings[0].y, sightings[0].z);
struct matrix_2x2 innovation_covariance;
mat_add_2x2(&innovation_covariance, &predicted_prob_estimate, &R);
// Update
struct matrix_2x2 kalman_gain, inv_innovation_covariance;
mat_inverse_2x2(&inv_innovation_covariance, &innovation_covariance);
mat_mult_2x2(&kalman_gain, &predicted_prob_estimate, &inv_innovation_covariance);
float step[2];
mat_mult_2xv(step, &kalman_gain, innovation);
current_state_estimate[0] = step[0] + predicted_state_estimate[0];
current_state_estimate[1] = step[1] + predicted_state_estimate[1];
struct matrix_2x2 intermediate;
intermediate.elements[0][0] = 1 - kalman_gain.elements[0][0];
intermediate.elements[0][1] = 0 - kalman_gain.elements[0][1];
intermediate.elements[1][0] = 0 - kalman_gain.elements[1][0];
intermediate.elements[1][1] = 1 - kalman_gain.elements[1][1];
mat_mult_2x2(¤t_prob_estimate, &intermediate, &predicted_prob_estimate);
//fprintf(stderr, "%f, %f\n", current_state_estimate[0], current_state_estimate[1]);
if (innovation_magnitude < 800)
{
look_wait = 300;
list_del_init(&fly_task->list_item);
if (innovation_magnitude < 100)
{
if (list_empty(&robot.waypoints))
{
list_add(&the_point.list_entry, &robot.waypoints);
}
robot.turret.target.x = current_state_estimate[0];
robot.turret.target.y = current_state_estimate[1];
robot.turret.target.z = -40;
the_point.point.x = current_state_estimate[0];
the_point.point.y = current_state_estimate[1];
the_point.point.z = -40;
//fprintf(stderr, "time to stop spinning\n");
}
//fprintf(stderr, "%f, %f, %f\n", current_state_estimate[0], current_state_estimate[1], innovation_magnitude);
}
/* if (fabs(state.x_pos) > .25 || fabs(state.y_pos) > .25)
{
robot.turret.target = temp;
the_point.point = temp;
}
else
{
the_point.point = temp;
}
if (list_empty(&robot.waypoints))
{
list_del_init(&fly_task->list_item);
list_add_tail(&the_point.list_entry, &robot.waypoints);
}*/
// fprintf(stderr, "%f, %f\n", state.x_pos, state.y_pos);
// fprintf(stderr, "%f, %f, %f\n", temp.x, temp.y, temp.z);
state.set_target = 0;
}
if (look_wait > 0)
{
look_wait--;
}
if (list_empty(&robot.waypoints) && list_empty(&fly_task->list_item) && look_wait == 0)
{
list_add_tail(&fly_task->list_item, &robot.task_list);
// fprintf(stderr, "I should spin\n");
}
// fprintf(stderr, "%i, %i\n", robot.position2.left, robot.position2.right);
float distance = get_expected_distance(&robot);
// fprintf(stderr, "%f, %f\n", distance, robot.range);
// fprintf(stderr, "%f, %f, %f\n", robot.turret.target.x, robot.turret.target.y, robot.turret.target.z);
// screen_to_world_vect(&robot, &robot.turret.target, 0, 0);
// fprintf(stderr, "%f, %f, %f\n", robot.turret.target.x, robot.turret.target.y, robot.turret.target.z);
struct timeval now, then;
// printf("%f, %f, %f, %f, %f, %f\n", robot.position2.position.x, robot.position2.position.y, robot.turret.target.x, robot.turret.target.y, robot.dest->x, robot.dest->y);
gettimeofday(&now, NULL);
/* fprintf(stderr, "%ld.%06ld, %f, %f, %f, %f, %i, %i, %f\n", now.tv_sec, now.tv_usec,
robot.position2.orientation.x, robot.position2.orientation.y,
robot.position2.orientation.z, robot.position2.orientation.w,
robot.position2.left, robot.position2.right, robot.position2.heading);
*/
//printf("%f\n", robot.range);
//printf("%f, %f, %f\n", robot.position2.position.x, robot.position2.position.y, robot.position2.tilt);
// fprintf(stderr, "%f, %f, %f, %i, %i\n", robot.position2.position.x, robot.position2.position.y, robot.position2.heading, robot.position2.left, robot.position2.right);
}
}
int main(int argc, char** argv)
{
//close server and RedPitaya if CTRL+C
signal(SIGINT, signal_callback_handler);
//client TCP initialisation
unsigned int MaxClient=5;
sock=0;
init_TCP_server(&sock, PORT, &client_list, MaxClient);
launch_server(&sock, &client_list);
//RedPitaya and settings initialisation
init_RP();
uint32_t buffer_length=0;
float level0=0.80, levelf=1.0;
int delay;
float *buffer_float=NULL;
char *buffer_char=NULL;
set_acquisition(dec);
init_ramp(1, level0, x0, levelf, xf);
init_variable(x0, xf, dec, &buffer_length, &delay);
printf("length = %i\n",(int)buffer_length);
buffer_float=(float *)malloc(((int)buffer_length)*sizeof(float));
buffer_char=(char *)malloc(((int)buffer_length+1)*sizeof(char));
//stepper initialisation
init_stepper(&stepper);
set_mode(&stepper, full_16);
double angle=0.0, speed=3.0;
angle=sector/((double)Nline);
init_position(&stepper, 160.0-sector/2.0);
int i=0;
while(1)
{
for (i=0 ; i<Nline ; i++)
{
move(&stepper, &angle, &speed, sens1);
trigg(delay);
pulse();
on_trigger_acquisition(buffer_float, buffer_length);
send_via_tcp(i+1, buffer_char, buffer_float, buffer_length, &client_list);
pulse();
}
for (i=Nline ; i>0 ; i--)
{
move(&stepper, &angle, &speed, sens2);
trigg(delay);
pulse();
on_trigger_acquisition(buffer_float, buffer_length);
send_via_tcp(i, buffer_char, buffer_float, buffer_length, &client_list);
pulse();
}
}
free(buffer_float);
free(buffer_char);
end_ramp();
disable_stepper(&stepper);
printf("Process done\n");
//close TCP client
close_TCP_client(&sock);
//close RedPitaya
close_RP();
return 0;
}
int main(int argc, char **argv)
{
setbuf(stdout, NULL);
signal(SIGINT, INThandler);
unsigned char ply = 6;
int opt;
while ((opt = getopt(argc, argv, "p:")) != -1) {
switch (opt) {
case 'p':
ply = atoi(optarg);
break;
default:
fprintf(stderr, "Usage: %s [-p ply]\n", argv[0]);
exit(EXIT_FAILURE);
}
}
struct position pos;
init_position(&pos);
assert(consistency(&pos));
struct timespec realt_old, realt_new;
tt = new_trans_tables();
#ifdef _USE_HISTORY
hist = init_history();
#endif
#ifdef _DEBUG
testing();
#endif
#ifndef _XBOARD
print_position(&pos);
#endif
while (1) {
if (is_check(&pos)) {
fprintf(stderr, "check.\n");
}
user_input(&pos);
#ifndef _XBOARD
print_position(&pos);
#endif
assert(consistency(&pos));
if (is_check(&pos)) {
fprintf(stderr, "check.\n");
}
current_utc_time(&realt_old);
computer_move(&pos, ply);
assert(consistency(&pos));
current_utc_time(&realt_new);
#ifndef _XBOARD
fprintf(stderr, "One second: %12llu ns\n",
(long long unsigned)BILLION);
fprintf(stderr, "Real time: %12llu ns\n",
(long long unsigned)minus_time(&realt_new, &realt_old));
print_position(&pos);
#endif
}
return (0);
}
void make_new_client(Window w)
{
Client *c, *p;
XWindowAttributes attr;
long dummy;
XWMHints *hints;
XSizeHints* shints;
XGrabServer(dpy);
XSetErrorHandler(ignore_xerror);
hints = XGetWMHints(dpy, w);
shints = XAllocSizeHints();
XGetWMNormalHints(dpy, w, shints, &dummy);
c = (Client *)calloc(1, sizeof(Client));
c->next = head_client;
head_client = c;
c->window = w;
c->name = null_str;
/* try these here instead */
c->size = shints;
XClassHint* hint = XAllocClassHint();
XGetClassHint(dpy, w, hint);
if (hint->res_class) {
c->name = hint->res_class;
if (hint->res_name)
XFree(hint->res_name);
}
XFree(hint);
XGetWindowAttributes(dpy, c->window, &attr);
c->x = attr.x;
c->y = attr.y;
c->width = attr.width;
c->height = attr.height;
c->border = opt_bw;
c->oldw = c->oldh = 0;
c->vdesk = vdesk;
c->index = 0;
if (!(attr.map_state == IsViewable)) {
init_position(c);
if (hints && hints->flags & StateHint)
set_wm_state(c, hints->initial_state);
}
if (hints)
XFree(hints);
/* client is initialised */
gravitate(c);
reparent(c);
XGrabButton(dpy, AnyButton, Mod1Mask, c->parent, False,
ButtonMask, GrabModeSync, GrabModeAsync, None, None);
XGrabButton(dpy, AnyButton, 0, c->parent, False,
ButtonPressMask, GrabModeSync, GrabModeAsync, None, None);
XMapWindow(dpy, c->window);
XMapRaised(dpy, c->parent);
set_wm_state(c, NormalState);
XSync(dpy, False);
XSetErrorHandler(handle_xerror);
XUngrabServer(dpy);
throwUnmaps = 0;
again:
for (p = c->next; p; p = p->next) {
if (c->index == p->index && !strcmp(c->name, p->name)) {
c->index++;
goto again;
}
}
}
