From: zer0 Date: Wed, 27 Jan 2010 22:22:19 +0000 (+0100) Subject: restore first circle algo + hostsim work X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=9fdc9cd2902ed50ebefe0b3c9309d4f173d65e2a restore first circle algo + hostsim work --- diff --git a/include/aversive/irq_lock.h b/include/aversive/irq_lock.h index ec45b68..12a46bb 100644 --- a/include/aversive/irq_lock.h +++ b/include/aversive/irq_lock.h @@ -45,8 +45,8 @@ #include /* we must use 'flags' to avoid a warning */ -#define IRQ_UNLOCK(flags) do { flags=0; hostsim_lock(); } while(0) -#define IRQ_LOCK(flags) do { flags=0; hostsim_unlock(); } while(0) +#define IRQ_UNLOCK(flags) do { flags=0; /* hostsim_lock(); */ } while(0) +#define IRQ_LOCK(flags) do { flags=0; /* hostsim_unlock(); */ } while(0) #define GLOBAL_IRQ_ARE_MASKED() hostsim_islocked() #else diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index 85b2e57..caa986c 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -479,38 +479,18 @@ void trajectory_manager_xy_event(struct trajectory *traj) /* XXX static */ void trajectory_manager_circle_event(struct trajectory *traj) { - double coef = 1.0; + double radius; double x = position_get_x_double(traj->position); double y = position_get_y_double(traj->position); double a = position_get_a_rad_double(traj->position); int32_t d_consign = 0, a_consign = 0; double angle_to_center_rad; - static int32_t d_prev, a_prev; - int32_t d_speed, a_speed; - int32_t d_pos, a_pos; - - d_pos = rs_get_distance(traj->robot); - a_pos = rs_get_angle(traj->robot); - d_speed = d_pos - d_prev; - a_speed = a_pos - a_prev; - d_prev = d_pos; - a_prev = a_pos; /* These vectors contain target position of the robot in * its own coordinates */ vect2_cart v2cart_pos; vect2_pol v2pol_target; - int32_t delta_d, delta_a; - double coef_deriv = traj->circle_coef; - double new_radius; - struct quadramp_filter * q_d, * q_a; - - q_d = traj->csm_distance->consign_filter_params; - q_a = traj->csm_angle->consign_filter_params; - delta_a = a_speed;//q_a->previous_var; - delta_d = d_speed;//q_d->previous_var; - /* step 1 : process new commands to quadramps */ /* process the command vector from current position to the @@ -520,31 +500,18 @@ void trajectory_manager_circle_event(struct trajectory *traj) vect2_cart2pol(&v2cart_pos, &v2pol_target); v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a); - /* pas trop mal, mais oscille */ - //new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv; - if (v2pol_target.r > traj->target.circle.radius/2) - new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv * traj->target.circle.radius / v2pol_target.r; - else - new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv ; - - /* oscille a mort */ - //new_radius = traj->target.circle.radius - traj->target.circle.radius * delta_a * delta_a * coef_deriv; - - /* ? */ - //new_radius = traj->target.circle.radius - traj->target.circle.radius * delta_a * coef_deriv; - - /* wanted direction of center of circle: * if we are far, go in the center direction, * if we are at radius, we want to see the center at 90° * if we are nearer than radius, angle to center is > 90° */ - if (v2pol_target.r > new_radius) { - angle_to_center_rad = new_radius / v2pol_target.r; + radius = traj->target.circle.radius; + if (v2pol_target.r > radius) { + angle_to_center_rad = radius / v2pol_target.r; angle_to_center_rad *= (M_PI / 2); } else { angle_to_center_rad = 1. - (v2pol_target.r / - (2 * new_radius)); + (2 * radius)); angle_to_center_rad *= M_PI; } @@ -556,8 +523,7 @@ void trajectory_manager_circle_event(struct trajectory *traj) if (ABS(v2pol_target.theta) > traj->a_start_rad) set_quadramp_speed(traj, 0, traj->a_speed); else { - coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad; - set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed); + set_quadramp_speed(traj, traj->d_speed, traj->a_speed); } /* XXX check flags */ @@ -585,7 +551,7 @@ void trajectory_manager_circle_event(struct trajectory *traj) ", a_speed=%" PRIi32 "radius = %f", rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj), rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj), - new_radius); + radius); cs_set_consign(traj->csm_angle, a_consign); cs_set_consign(traj->csm_distance, d_consign); diff --git a/projects/microb2010/tests/hostsim/commands.c b/projects/microb2010/tests/hostsim/commands.c index 111de4a..cb620ca 100644 --- a/projects/microb2010/tests/hostsim/commands.c +++ b/projects/microb2010/tests/hostsim/commands.c @@ -112,6 +112,7 @@ extern parse_pgm_inst_t cmd_pt_list_show; extern parse_pgm_inst_t cmd_goto1; extern parse_pgm_inst_t cmd_goto2; extern parse_pgm_inst_t cmd_goto3; +extern parse_pgm_inst_t cmd_goto4; extern parse_pgm_inst_t cmd_position; extern parse_pgm_inst_t cmd_position_set; extern parse_pgm_inst_t cmd_strat_infos; @@ -211,6 +212,7 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_pt_list_show, (parse_pgm_inst_t *)&cmd_goto1, (parse_pgm_inst_t *)&cmd_goto2, + (parse_pgm_inst_t *)&cmd_goto4, (parse_pgm_inst_t *)&cmd_position, (parse_pgm_inst_t *)&cmd_position_set, (parse_pgm_inst_t *)&cmd_strat_infos, diff --git a/projects/microb2010/tests/hostsim/commands_mainboard.c b/projects/microb2010/tests/hostsim/commands_mainboard.c index b07983b..3b2d82b 100644 --- a/projects/microb2010/tests/hostsim/commands_mainboard.c +++ b/projects/microb2010/tests/hostsim/commands_mainboard.c @@ -2214,9 +2214,17 @@ static void cmd_test_parsed(void *parsed_result, void *data) { struct cmd_test_result *res = parsed_result; double d,a; + uint8_t err; + + strat_reset_pos(1000, 500, 0); strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW); circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a); - printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a); + trajectory_d_rel(&mainboard.traj, 1000); + err = WAIT_COND_OR_TRAJ_END(position_get_x_double(&mainboard.pos) > 1500, 0xFF); + if (err) + return; + strat_set_speed(d, a); + trajectory_d_a_rel(&mainboard.traj, 10000, 1000); } prog_char str_test_arg0[] = "test"; diff --git a/projects/microb2010/tests/hostsim/commands_traj.c b/projects/microb2010/tests/hostsim/commands_traj.c index 0e23798..f693820 100644 --- a/projects/microb2010/tests/hostsim/commands_traj.c +++ b/projects/microb2010/tests/hostsim/commands_traj.c @@ -141,7 +141,7 @@ static void cmd_circle_coef_parsed(void *parsed_result, void *data) trajectory_set_circle_coef(&mainboard.traj, res->circle_coef); } - printf_P(PSTR("circle_coef %2.2f\r\n"), mainboard.traj.circle_coef); + printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef); } prog_char str_circle_coef_arg0[] = "circle_coef"; @@ -441,12 +441,17 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) printf_P(PSTR("List empty\r\n")); return; } + restart: for (i=0 ; iarg1, PSTR("start"))) { trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y); why = wait_traj_end(0xFF); /* all */ } + else if (!strcmp_P(res->arg1, PSTR("loop_start"))) { + trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y); + why = wait_traj_end(0xFF); /* all */ + } else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) { while (1) { why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF); @@ -457,7 +462,13 @@ static void cmd_pt_list_parsed(void * parsed_result, void * data) } if (why & (~(END_TRAJ | END_NEAR))) trajectory_stop(&mainboard.traj); + if (why & END_INTR) + break; } + if (why & END_INTR) + return; + if (!strcmp_P(res->arg1, PSTR("loop_start"))) + goto restart; } prog_char str_pt_list_arg0[] = "pt_list"; @@ -521,7 +532,7 @@ parse_pgm_inst_t cmd_pt_list_del = { }; /* show */ -prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start"; +prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start"; parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg); prog_char help_pt_list_show[] = "Show, start or reset pt_list"; @@ -548,6 +559,7 @@ struct cmd_goto_result { int32_t arg2; int32_t arg3; int32_t arg4; + int32_t arg5; }; /* function called when cmd_goto is parsed successfully */ @@ -598,6 +610,11 @@ static void cmd_goto_parsed(void * parsed_result, void * data) else if (!strcmp_P(res->arg1, PSTR("da_rel"))) { trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3); } + else if (!strcmp_P(res->arg1, PSTR("circle_rel"))) { + trajectory_circle_rel(&mainboard.traj, res->arg2, res->arg3, + res->arg4, res->arg5, 0); + return; /* XXX */ + } t1 = time_get_us2(); while ((err = test_traj_end(0xFF)) == 0) { t2 = time_get_us2(); @@ -651,6 +668,28 @@ parse_pgm_inst_t cmd_goto2 = { }, }; +prog_char str_goto_arg1_c[] = "circle_rel"; +parse_pgm_token_string_t cmd_goto_arg1_c = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_c); +parse_pgm_token_num_t cmd_goto_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg4, INT32); +parse_pgm_token_num_t cmd_goto_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg5, INT32); + +/* 4 params */ +prog_char help_goto4[] = "Do a circle: (x,y, radius, angle)"; +parse_pgm_inst_t cmd_goto4 = { + .f = cmd_goto_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_goto4, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_c, + (prog_void *)&cmd_goto_arg2, + (prog_void *)&cmd_goto_arg3, + (prog_void *)&cmd_goto_arg4, + (prog_void *)&cmd_goto_arg5, + NULL, + }, +}; + /**********************************************************/ /* Position tests */ diff --git a/projects/microb2010/tests/hostsim/cs.c b/projects/microb2010/tests/hostsim/cs.c index c2d0ff4..9766ea9 100644 --- a/projects/microb2010/tests/hostsim/cs.c +++ b/projects/microb2010/tests/hostsim/cs.c @@ -116,7 +116,7 @@ static void do_cs(void *dummy) #endif cpt++; - if ((cpt & 8) == 0) + if ((cpt & 7) == 0) robotsim_dump(); //dump_cs("distance", &mainboard.distance.cs); //dump_cs("angle", &mainboard.angle.cs); diff --git a/projects/microb2010/tests/hostsim/display.py b/projects/microb2010/tests/hostsim/display.py index a6c90e7..d21cc9e 100644 --- a/projects/microb2010/tests/hostsim/display.py +++ b/projects/microb2010/tests/hostsim/display.py @@ -10,53 +10,113 @@ area_box = box(size=areasize, color=(0.0, 1.0, 0.0)) scene.autoscale=0 +# all positions of robot every 5ms +save_pos = [] + robot = box(pos = (0, 0, 150), size = (300,300,300), color = (1, 0, 0) ) last_pos = robot.pos.x, robot.pos.y, robot.pos.z +hcenter_line = curve() +hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)] +vcenter_line = curve() +vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)] + +def square(sz): + sq = curve() + sq.pos = [(-sz, -sz, 0.3), + (-sz, sz, 0.3), + (sz, sz, 0.3), + (sz, -sz, 0.3), + (-sz, -sz, 0.3),] + return sq + +sq1 = square(250) +sq1 = square(500) + robot_trail = curve() +robot_trail_list = [] max_trail = 500 def set_robot(x, y, a): - global robot, last_pos + global robot, last_pos, robot_trail, robot_trail_list + global save_pos + robot.pos = (x - AREA_X/2, y - AREA_Y/2, 150) robot.axis = (math.cos(a*math.pi/180) * 300, math.sin(a*math.pi/180) * 300, 0) - pos = robot.pos.x, robot.pos.y, robot.pos.z + # save position + save_pos.append(robot.pos.x, robot.pos, a) + + pos = robot.pos.x, robot.pos.y, 0.3 if pos != last_pos: - robot_trail.append(pos) + robot_trail_list.append(pos) last_pos = pos - robot_trail_l = len(robot_trail.pos) - if robot_trail_l> max_trail: - robot_trail.pos = robot_trail.pos[robot_trail_l - max_trail:] + robot_trail_l = len(robot_trail_list) + if robot_trail_l > max_trail: + robot_trail_list = robot_trail_list[robot_trail_l - max_trail:] + robot_trail.pos = robot_trail_list -while True: +def graph(): + pass + +def save(): + f = open("/tmp/robot_save", "w") + for p in save_pos: + f.write("%f %f %f\n"%(p[0], p[1], p[2])) + f.close() + +def silent_mkfifo(f): try: - os.mkfifo("/tmp/.robot") + os.mkfifo(f) except: pass + +while True: + silent_mkfifo("/tmp/.robot_sim2dis") + silent_mkfifo("/tmp/.robot_dis2sim") while True: - f = open("/tmp/.robot") + fr = open("/tmp/.robot_sim2dis", "r") + fw = open("/tmp/.robot_dis2sim", "w", 0) while True: - l = f.readline() + l = fr.readline() if l == "": break - x,y,a = map(lambda x:int(x), l[:-1].split(" ")) - set_robot(x,y,a) - f.close() - - """ - k = scene.kb.getkey() - x,y,z = scene.center - if k == "left": - scene.center = x-10,y,z - elif k == "right": - scene.center = x+10,y,z - elif k == "up": - scene.center = x,y+10,z - elif k == "down": - scene.center = x,y-10,z - """ + try: + x,y,a = map(lambda x:int(x), l[:-1].split(" ")) + set_robot(x,y,a) + except ValueError: + pass + + if scene.kb.keys == 0: + continue + + k = scene.kb.getkey() + x,y,z = scene.center + if k == "left": + scene.center = x-10,y,z + elif k == "right": + scene.center = x+10,y,z + elif k == "up": + scene.center = x,y+10,z + elif k == "down": + scene.center = x,y-10,z + elif k == "l": + fw.write("l") + elif k == "r": + fw.write("r") + elif k == "c": + robot_trail_list = [] + elif k == "x": + save_pos = [] + elif k == "g": + graph() + elif k == "s": + save() + + fr.close() + fw.close() + diff --git a/projects/microb2010/tests/hostsim/robotsim.c b/projects/microb2010/tests/hostsim/robotsim.c index 85224a3..ee7ca85 100644 --- a/projects/microb2010/tests/hostsim/robotsim.c +++ b/projects/microb2010/tests/hostsim/robotsim.c @@ -53,17 +53,38 @@ static int32_t l_pwm, r_pwm; static int32_t l_enc, r_enc; -static int fd; +static int fdr, fdw; /* */ #define FILTER 97 #define FILTER2 (100-FILTER) +#define SHIFT 4 + +void robotsim_dump(void) +{ + char buf[BUFSIZ]; + int len; + + len = snprintf(buf, sizeof(buf), "%d %d %d\n", + position_get_x_s16(&mainboard.pos), + position_get_y_s16(&mainboard.pos), + position_get_a_deg_s16(&mainboard.pos)); + hostsim_lock(); + write(fdw, buf, len); + hostsim_unlock(); +} /* must be called periodically */ void robotsim_update(void) { + static int32_t l_pwm_shift[SHIFT]; + static int32_t r_pwm_shift[SHIFT]; static int32_t l_speed, r_speed; + static unsigned i = 0; + static unsigned cpt = 0; + int32_t local_l_pwm, local_r_pwm; double x, y, a, a2, d; + char cmd = 0; /* corners of the robot */ double xfl, yfl; /* front left */ @@ -71,12 +92,28 @@ void robotsim_update(void) double xrr, yrr; /* rear right */ double xfr, yfr; /* front right */ + /* time shift the command */ + l_pwm_shift[i] = l_pwm; + r_pwm_shift[i] = r_pwm; + i ++; + i %= SHIFT; + local_l_pwm = l_pwm_shift[i]; + local_r_pwm = r_pwm_shift[i]; + + /* read command */ + if (((cpt ++) & 0x7) == 0) { + if (read(fdr, &cmd, 1) != 1) + cmd = 0; + } + x = position_get_x_double(&mainboard.pos); y = position_get_y_double(&mainboard.pos); a = position_get_a_rad_double(&mainboard.pos); - l_speed = ((l_speed * FILTER) / 100) + ((l_pwm * 1000 * FILTER2)/1000); - r_speed = ((r_speed * FILTER) / 100) + ((r_pwm * 1000 * FILTER2)/1000); + l_speed = ((l_speed * FILTER) / 100) + + ((local_l_pwm * 1000 * FILTER2)/1000); + r_speed = ((r_speed * FILTER) / 100) + + ((local_r_pwm * 1000 * FILTER2)/1000); /* basic collision detection */ a2 = atan2(ROBOT_WIDTH/2, ROBOT_LENGTH/2); @@ -102,25 +139,17 @@ void robotsim_update(void) if (!is_in_area(xfr, yfr, 0) && r_speed > 0) r_speed = 0; + /* perturbation */ + if (cmd == 'l') + l_enc += 5000; /* push 1 cm */ + if (cmd == 'r') + r_enc += 5000; /* push 1 cm */ + /* XXX should lock */ l_enc += (l_speed / 1000); r_enc += (r_speed / 1000); } -void robotsim_dump(void) -{ - char buf[BUFSIZ]; - int len; - - len = snprintf(buf, sizeof(buf), "%d %d %d\n", - position_get_x_s16(&mainboard.pos), - position_get_y_s16(&mainboard.pos), - position_get_a_deg_s16(&mainboard.pos)); - hostsim_lock(); - write(fd, buf, len); - hostsim_unlock(); -} - void robotsim_pwm(void *arg, int32_t val) { // printf("%p, %d\n", arg, val); @@ -141,9 +170,15 @@ int32_t robotsim_encoder_get(void *arg) int robotsim_init(void) { - mkfifo("/tmp/.robot", 0600); - fd = open("/tmp/.robot", O_WRONLY, 0); - if (fd < 0) + mkfifo("/tmp/.robot_sim2dis", 0600); + mkfifo("/tmp/.robot_dis2sim", 0600); + fdw = open("/tmp/.robot_sim2dis", O_WRONLY, 0); + if (fdw < 0) + return -1; + fdr = open("/tmp/.robot_dis2sim", O_RDONLY | O_NONBLOCK, 0); + if (fdr < 0) { + close(fdw); return -1; + } return 0; }