From b14123cac428083a50e2d0871514018810a779e5 Mon Sep 17 00:00:00 2001 From: zer0 Date: Mon, 5 Apr 2010 18:23:01 +0200 Subject: [PATCH] trajectories on hostsim --- .../devices/robot/robot_system/robot_system.c | 84 +++++++++---------- .../trajectory_manager/trajectory_manager.h | 9 ++ .../trajectory_manager_core.c | 8 +- projects/microb2010/mainboard/Makefile | 4 +- .../microb2010/mainboard/commands_mainboard.c | 77 ++++++++++++++++- projects/microb2010/mainboard/display.py | 34 +++++++- 6 files changed, 165 insertions(+), 51 deletions(-) diff --git a/modules/devices/robot/robot_system/robot_system.c b/modules/devices/robot/robot_system/robot_system.c index 5e91561..b5b3d32 100755 --- a/modules/devices/robot/robot_system/robot_system.c +++ b/modules/devices/robot/robot_system/robot_system.c @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation, Microb Technology, Eirbot (2005) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -40,7 +40,7 @@ #include "robot_system.h" -/** Call a pwm() pointer : +/** Call a pwm() pointer : * - lock the interrupts * - read the pointer to the pwm function * - unlock the interrupts @@ -62,7 +62,7 @@ safe_setpwm(void (*f)(void *, int32_t), void * param, int32_t value) } } -/** Call a encoder() pointer : +/** Call a encoder() pointer : * - lock the interrupts * - read the pointer to the encoder function * - unlock the interrupts @@ -134,7 +134,7 @@ void rs_set_right_pwm(struct robot_system * rs, void (*right_pwm)(void *, int32_ #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT /** define left motor encoder function and param */ -void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encoder)(void *), +void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encoder)(void *), void *left_mot_encoder_param, double gain) { uint8_t flags; @@ -147,7 +147,7 @@ void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encode } /** define right motor encoder function and param */ -void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_encoder)(void *), +void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_encoder)(void *), void *right_mot_encoder_param, double gain) { uint8_t flags; @@ -161,7 +161,7 @@ void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_enco #endif /** define left external encoder function and param */ -void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encoder)(void *), +void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encoder)(void *), void *left_ext_encoder_param, double gain) { uint8_t flags; @@ -178,7 +178,7 @@ void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encode } /** define right external encoder function and param */ -void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_encoder)(void *), +void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_encoder)(void *), void *right_ext_encoder_param, double gain) { uint8_t flags; @@ -196,9 +196,9 @@ void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_enco /**** Virtual encoders and PWM */ -/** +/** * set the real pwms according to the specified angle (it also - * depends on the last distance command sent) + * depends on the last distance command sent) */ void rs_set_angle(void * data, int32_t angle) { @@ -214,14 +214,14 @@ void rs_set_angle(void * data, int32_t angle) p.angle = angle; rs_get_wheels_from_polar(&w, &p); - + safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left); safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right); } -/** +/** * set the real pwms according to the specified distance (it also - * depends on the last angle command sent) + * depends on the last angle command sent) */ void rs_set_distance(void * data, int32_t distance) { @@ -237,37 +237,37 @@ void rs_set_distance(void * data, int32_t distance) p.distance = distance; rs_get_wheels_from_polar(&w, &p); - + safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left); safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right); } -/** - * get the virtual angle according to real encoders value. +/** + * get the virtual angle according to real encoders value. */ int32_t rs_get_angle(void * data) { struct robot_system * rs = data; int32_t angle; uint8_t flags; - + IRQ_LOCK(flags); - angle = rs->virtual_encoders.angle ; + angle = rs->virtual_encoders.angle ; IRQ_UNLOCK(flags); return angle; } -/** - * get the virtual distance according to real encoders value. +/** + * get the virtual distance according to real encoders value. */ int32_t rs_get_distance(void * data) { struct robot_system * rs = data; int32_t distance; uint8_t flags; - + IRQ_LOCK(flags); - distance = rs->virtual_encoders.distance ; + distance = rs->virtual_encoders.distance ; IRQ_UNLOCK(flags); return distance; } @@ -277,9 +277,9 @@ int32_t rs_get_ext_angle(void * data) struct robot_system * rs = data; int32_t angle; uint8_t flags; - + IRQ_LOCK(flags); - angle = rs->pext_prev.angle ; + angle = rs->pext_prev.angle ; IRQ_UNLOCK(flags); return angle; } @@ -289,9 +289,9 @@ int32_t rs_get_ext_distance(void * data) struct robot_system * rs = data; int32_t distance; uint8_t flags; - + IRQ_LOCK(flags); - distance = rs->pext_prev.distance ; + distance = rs->pext_prev.distance ; IRQ_UNLOCK(flags); return distance; } @@ -301,10 +301,10 @@ int32_t rs_get_mot_angle(void * data) { struct robot_system * rs = data; int32_t angle; - uint8_t flags; + uint8_t flags; IRQ_LOCK(flags); - angle = rs->pmot_prev.angle ; + angle = rs->pmot_prev.angle ; IRQ_UNLOCK(flags); return angle; } @@ -314,9 +314,9 @@ int32_t rs_get_mot_distance(void * data) struct robot_system * rs = data; int32_t distance; uint8_t flags; - + IRQ_LOCK(flags); - distance = rs->pmot_prev.distance ; + distance = rs->pmot_prev.distance ; IRQ_UNLOCK(flags); return distance; } @@ -327,9 +327,9 @@ int32_t rs_get_ext_left(void * data) struct robot_system * rs = data; int32_t left; uint8_t flags; - + IRQ_LOCK(flags); - left = rs->wext_prev.left ; + left = rs->wext_prev.left ; IRQ_UNLOCK(flags); return left; } @@ -339,9 +339,9 @@ int32_t rs_get_ext_right(void * data) struct robot_system * rs = data; int32_t right; uint8_t flags; - + IRQ_LOCK(flags); - right = rs->wext_prev.right ; + right = rs->wext_prev.right ; IRQ_UNLOCK(flags); return right; } @@ -351,10 +351,10 @@ int32_t rs_get_mot_left(void * data) { struct robot_system * rs = data; int32_t left; - uint8_t flags; + uint8_t flags; IRQ_LOCK(flags); - left = rs->wmot_prev.left ; + left = rs->wmot_prev.left ; IRQ_UNLOCK(flags); return left; } @@ -364,9 +364,9 @@ int32_t rs_get_mot_right(void * data) struct robot_system * rs = data; int32_t right; uint8_t flags; - + IRQ_LOCK(flags); - right = rs->wmot_prev.right ; + right = rs->wmot_prev.right ; IRQ_UNLOCK(flags); return right; } @@ -375,13 +375,13 @@ int32_t rs_get_mot_right(void * data) void rs_set_flags(struct robot_system * rs, uint8_t flags) { uint8_t i_flags; - + IRQ_LOCK(i_flags); rs->flags = flags; IRQ_UNLOCK(i_flags); } -/** +/** * Read the encoders, and update internal virtual counters. Call this * function is needed before reading the virtual encoders.The program * will decide if it the external encoders or the motor encoders are @@ -398,7 +398,7 @@ void rs_update(void * data) #endif int32_t delta_angle, delta_distance; uint8_t flags; - + /* read encoders */ wext.left = safe_getencoder(rs->left_ext_encoder, rs->left_ext_encoder_param); wext.right = safe_getencoder(rs->right_ext_encoder, rs->right_ext_encoder_param); @@ -407,7 +407,7 @@ void rs_update(void * data) wmot.left = safe_getencoder(rs->left_mot_encoder, rs->left_mot_encoder_param); wmot.right = safe_getencoder(rs->right_mot_encoder, rs->right_mot_encoder_param); #endif - + /* apply gains to each wheel */ if (! (rs->flags & RS_IGNORE_EXT_GAIN )) { #ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.h b/modules/devices/robot/trajectory_manager/trajectory_manager.h index 43730ea..6e7e82e 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager.h +++ b/modules/devices/robot/trajectory_manager/trajectory_manager.h @@ -197,6 +197,15 @@ void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_r void trajectory_circle_rel(struct trajectory *traj, double x, double y, double radius_mm, double rel_a_deg, uint8_t flags); +/* + * Compute the fastest distance and angle speeds matching the radius + * from current traj_speed + */ +void circle_get_da_speed_from_radius(struct trajectory *traj, + double radius_mm, + double *speed_d, + double *speed_a); + /* do a line */ void trajectory_line_abs(struct trajectory *traj, double x1, double y1, double x2, double y2, double advance); diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index 33c2a30..0177419 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -479,10 +479,10 @@ void trajectory_manager_xy_event(struct trajectory *traj) * Compute the fastest distance and angle speeds matching the radius * from current traj_speed */ -/* static */void circle_get_da_speed_from_radius(struct trajectory *traj, - double radius_mm, - double *speed_d, - double *speed_a) +void circle_get_da_speed_from_radius(struct trajectory *traj, + double radius_mm, + double *speed_d, + double *speed_a) { /* speed_d = coef * speed_a */ double coef; diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile index 7eff3cb..141943f 100755 --- a/projects/microb2010/mainboard/Makefile +++ b/projects/microb2010/mainboard/Makefile @@ -7,14 +7,14 @@ SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c SRC += strat_utils.c strat_base.c strat.c -ifneq ($(HOST),avr) +ifeq ($(H),1) SRC += robotsim.c endif ASRC = CFLAGS += -Wall -Werror -ifeq ($(HOST),avr) +ifneq ($(H),1) LDFLAGS = -T ../common/avr6.x endif diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index 9f39f86..ced8e58 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -1074,16 +1074,92 @@ parse_pgm_inst_t cmd_servo_balls = { struct cmd_test_result { fixed_string_t arg0; int32_t radius; + int32_t dist; }; +/* function called when cmd_test is parsed successfully */ +static void line2line(double line1x1, double line1y1, + double line1x2, double line1y2, + double line2x1, double line2y1, + double line2x2, double line2y2, + double radius, double dist) +{ + uint8_t err; + int32_t dist_imp_target; + double speed_d, speed_a; + double distance, angle; + double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1); + double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1); + + printf("%s()\n", __FUNCTION__); + strat_set_speed(500, 500); + circle_get_da_speed_from_radius(&mainboard.traj, radius, + &speed_d, &speed_a); + trajectory_line_abs(&mainboard.traj, + line1x1, line1y1, + line1x2, line1y2, 150.); + err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) < + dist, TRAJ_FLAGS_NO_NEAR); + /* circle */ + strat_set_speed(speed_d, speed_a); + angle = line2_angle - line1_angle; + distance = angle * radius; + if (distance < 0) + distance = -distance; + dist_imp_target = rs_get_distance(&mainboard.rs) + + distance * mainboard.pos.phys.distance_imp_per_mm; + angle = DEG(angle); + distance += 100; /* take some margin to avoid deceleration */ + trajectory_d_a_rel(&mainboard.traj, distance, angle); + + err = WAIT_COND_OR_TRAJ_END(rs_get_distance(&mainboard.rs) > dist_imp_target, + TRAJ_FLAGS_NO_NEAR); + + strat_set_speed(500, 500); + trajectory_line_abs(&mainboard.traj, + line2x1, line2y1, + line2x2, line2y2, 150.); +} + /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { + // struct cmd_test_result *res = parsed_result; +#ifdef HOST_VERSION + strat_reset_pos(400, 400, 90); + mainboard.angle.on = 1; + mainboard.distance.on = 1; +#endif + line2line(375, 347, 375, 1847, + 375, 1847, 1050, 1472, + 100, 200); + line2line(825, 1596, 1050, 1472, + 1050, 1472, 1500, 1722, + 180, 120); + line2line(1050, 1472, 1500, 1722, + 1500, 1722, 2175, 1347, + 180, 120); + line2line(1500, 1722, 2175, 1347, + 2175, 1347, 2175, 847, + 150, 120); + line2line(2175, 1347, 2175, 847, + 2175, 847, 2400, 722, + 150, 120); + line2line(2175, 847, 2400, 722, + 2400, 722, 2625, 847, + 150, 100); + line2line(2400, 722, 2625, 847, + 2625, 847, 2625, 1847, + 150, 100); + line2line(2625, 847, 2625, 1847, + 2625, 1847, 375, 597, + 100, 200); } prog_char str_test_arg0[] = "test"; parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0); parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32); +parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32); prog_char help_test[] = "Test function"; parse_pgm_inst_t cmd_test = { @@ -1092,7 +1168,6 @@ parse_pgm_inst_t cmd_test = { .help_str = help_test, .tokens = { /* token list, NULL terminated */ (prog_void *)&cmd_test_arg0, - (prog_void *)&cmd_test_arg1, NULL, }, }; diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py index 239b2d6..3ca1a47 100644 --- a/projects/microb2010/mainboard/display.py +++ b/projects/microb2010/mainboard/display.py @@ -15,7 +15,7 @@ save_pos = [] robot = box(pos = (0, 0, 150), size = (250,320,350), - color = (1, 0, 0) ) + color = (0.3, 0.3, 0.3) ) last_pos = robot.pos.x, robot.pos.y, robot.pos.z hcenter_line = curve() @@ -23,6 +23,18 @@ 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)] +yellowarea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ] +yellowareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , yellowarea) +yellowarea_box = box(pos=(-AREA_X/2+250,-AREA_Y/2+250,0), size=yellowareasize, color=(1.0, 1.0, 0.0)) + +bluearea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ] +blueareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , bluearea) +bluearea_box = box(pos=(AREA_X/2-250,-AREA_Y/2+250,0), size=blueareasize, color=(0.0, 0.0, 1.0)) + +greyarea = [ (0.0, 0.0, -0.5), (1520.0, 500.0, 0.5) ] +greyareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , greyarea) +greyarea_box = box(pos=(0,-AREA_Y/2+250,0), size=greyareasize, color=(0.3, 0.6, 0.3)) + def square(sz): sq = curve() sq.pos = [(-sz, -sz, 0.3), @@ -54,7 +66,8 @@ TYPE_DANGEROUS=1 TYPE_WHITE_CORN=2 TYPE_BLACK_CORN=3 TYPE_OBSTACLE=4 -TYPE_NEIGH=5 +TYPE_BALL=5 +TYPE_NEIGH=6 col = [TYPE_WAYPOINT] * WAYPOINTS_NBY waypoints = [col[:] for i in range(WAYPOINTS_NBX)] @@ -131,6 +144,15 @@ def init_waypoints(): if c >= 0: waypoints[i][j] = corn_table[c] continue + + # balls + if (i & 1) == 0 and j > 3: + waypoints[i][j] = TYPE_BALL + continue + if (i == 0 or i == WAYPOINTS_NBX-1) and j > 2: + waypoints[i][j] = TYPE_BALL + continue + # too close of border if (i & 1) == 1 and j == WAYPOINTS_NBY -1: waypoints[i][j] = TYPE_OBSTACLE @@ -178,6 +200,14 @@ def toggle_obj_disp(): radius=25, color=(0.2,0.2,0.2), pos=(x-AREA_X/2,y-AREA_Y/2,75)) area_objects.append(c) + elif waypoints[i][j] == TYPE_BALL: + c = sphere(radius=50, color=(1., 0.,0.), + pos=(x-AREA_X/2,y-AREA_Y/2,50)) + area_objects.append(c) + else: + c = sphere(radius=5, color=(0., 0.,1.), + pos=(x-AREA_X/2,y-AREA_Y/2,5)) + area_objects.append(c) j += 1 y += STEP_CORN_Y i += 1 -- 2.39.5