#include <hostsim.h>
/* 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
/* 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
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;
}
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 */
", 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);
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;
(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,
{
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";
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";
printf_P(PSTR("List empty\r\n"));
return;
}
+ restart:
for (i=0 ; i<pt_list_len ; i++) {
printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
if (!strcmp_P(res->arg1, 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);
}
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";
};
/* 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";
int32_t arg2;
int32_t arg3;
int32_t arg4;
+ int32_t arg5;
};
/* function called when cmd_goto is parsed successfully */
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();
},
};
+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 */
#endif
cpt++;
- if ((cpt & 8) == 0)
+ if ((cpt & 7) == 0)
robotsim_dump();
//dump_cs("distance", &mainboard.distance.cs);
//dump_cs("angle", &mainboard.angle.cs);
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()
+
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 */
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);
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);
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;
}
git.droids-corp.org / aversive.git / commitdiff