From a15451f8535228e9193b5dc042929ca0a7f79d9c Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 3 Apr 2010 17:40:20 +0200 Subject: [PATCH] hostsim and cob handling --- .../tests/hostsim/commands_mainboard.c | 44 ++++++++++++++++++- projects/microb2010/tests/oa/.config | 34 +++++++++++++- projects/microb2010/tests/oa/.config.old | 34 ++------------ projects/microb2010/tests/oa/graph.py | 32 +++++++++++--- projects/microb2010/tests/oa/main.c | 16 +++---- 5 files changed, 112 insertions(+), 48 deletions(-) diff --git a/projects/microb2010/tests/hostsim/commands_mainboard.c b/projects/microb2010/tests/hostsim/commands_mainboard.c index ff808c3..0238257 100644 --- a/projects/microb2010/tests/hostsim/commands_mainboard.c +++ b/projects/microb2010/tests/hostsim/commands_mainboard.c @@ -2249,8 +2249,48 @@ void circle_get_da_speed_from_radius(struct trajectory *traj, /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { - trajectory_line_abs(&mainboard.traj, 0, 1050, - 1500, 1050, 100.); + uint8_t err; + double d,a; + +#if 0 + strat_set_speed(500, 500); + circle_get_da_speed_from_radius(&mainboard.traj, 225, &d, &a); + trajectory_line_abs(&mainboard.traj, + 375, 347, + 375, 1847, 100.); + err = WAIT_COND_OR_TRAJ_END(y_is_more_than(945), + TRAJ_FLAGS_NO_NEAR); + /* circle */ + strat_set_speed(d, a); + trajectory_d_a_rel(&mainboard.traj, 350, -61); + + err = WAIT_COND_OR_TRAJ_END(x_is_more_than(600), + TRAJ_FLAGS_NO_NEAR); + + strat_set_speed(500, 500); + trajectory_line_abs(&mainboard.traj, + 375, 1097, + 1725, 1847, 100.); +#else + strat_set_speed(500, 500); + circle_get_da_speed_from_radius(&mainboard.traj, 225, &d, &a); + trajectory_line_abs(&mainboard.traj, + 375, 347, + 375, 1847, 100.); + err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1445), + TRAJ_FLAGS_NO_NEAR); + /* circle */ + strat_set_speed(d, a); + trajectory_d_a_rel(&mainboard.traj, 700, -121); + + err = WAIT_COND_OR_TRAJ_END(x_is_more_than(750), + TRAJ_FLAGS_NO_NEAR); + + strat_set_speed(500, 500); + trajectory_line_abs(&mainboard.traj, + 825, 1596, + 1950, 972, 100.); +#endif } prog_char str_test_arg0[] = "test"; diff --git a/projects/microb2010/tests/oa/.config b/projects/microb2010/tests/oa/.config index 046653a..ad65973 100644 --- a/projects/microb2010/tests/oa/.config +++ b/projects/microb2010/tests/oa/.config @@ -1,5 +1,5 @@ # -# Automatically generated by make menuconfig: don't edit +# Automatically generated make config: don't edit # # @@ -74,11 +74,16 @@ CONFIG_FORMAT_IHEX=y # # Base modules # + +# +# Enable math library in generation options to see all modules +# # CONFIG_MODULE_CIRBUF is not set # CONFIG_MODULE_CIRBUF_LARGE is not set # CONFIG_MODULE_FIXED_POINT is not set # CONFIG_MODULE_VECT2 is not set CONFIG_MODULE_GEOMETRY=y +# CONFIG_MODULE_HOSTSIM is not set # CONFIG_MODULE_SCHEDULER is not set # CONFIG_MODULE_SCHEDULER_STATS is not set # CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set @@ -93,6 +98,10 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y # # Communication modules # + +# +# uart needs circular buffer, mf2 client may need scheduler +# # CONFIG_MODULE_UART is not set # CONFIG_MODULE_UART_9BITS is not set # CONFIG_MODULE_UART_CREATE_CONFIG is not set @@ -161,9 +170,10 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y # CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set # -# Robot specific modules +# Robot specific modules (fixed point lib may be needed) # # CONFIG_MODULE_ROBOT_SYSTEM is not set +# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set # CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set # CONFIG_MODULE_POSITION_MANAGER is not set # CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set @@ -176,6 +186,10 @@ CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG=y # Control system modules # # CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set + +# +# Filters +# # CONFIG_MODULE_PID is not set # CONFIG_MODULE_PID_CREATE_CONFIG is not set # CONFIG_MODULE_RAMP is not set @@ -186,12 +200,20 @@ CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG=y # # Radio devices # + +# +# Some radio devices require SPI to be activated +# # CONFIG_MODULE_CC2420 is not set # CONFIG_MODULE_CC2420_CREATE_CONFIG is not set # # Crypto modules # + +# +# Crypto modules depend on utils module +# # CONFIG_MODULE_AES is not set # CONFIG_MODULE_AES_CTR is not set # CONFIG_MODULE_MD5 is not set @@ -201,12 +223,20 @@ CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG=y # # Encodings modules # + +# +# Encoding modules depend on utils module +# # CONFIG_MODULE_BASE64 is not set # CONFIG_MODULE_HAMMING is not set # # Debug modules # + +# +# Debug modules depend on utils module +# # CONFIG_MODULE_DIAGNOSTIC is not set # CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set CONFIG_MODULE_ERROR=y diff --git a/projects/microb2010/tests/oa/.config.old b/projects/microb2010/tests/oa/.config.old index 112f126..046653a 100644 --- a/projects/microb2010/tests/oa/.config.old +++ b/projects/microb2010/tests/oa/.config.old @@ -1,5 +1,5 @@ # -# Automatically generated make config: don't edit +# Automatically generated by make menuconfig: don't edit # # @@ -56,11 +56,11 @@ CONFIG_QUARTZ=12000000 # # Generation options # -# CONFIG_OPTM_0 is not set +CONFIG_OPTM_0=y # CONFIG_OPTM_1 is not set # CONFIG_OPTM_2 is not set # CONFIG_OPTM_3 is not set -CONFIG_OPTM_S=y +# CONFIG_OPTM_S is not set CONFIG_MATH_LIB=y # CONFIG_FDEVOPEN_COMPAT is not set # CONFIG_NO_PRINTF is not set @@ -74,10 +74,6 @@ CONFIG_FORMAT_IHEX=y # # Base modules # - -# -# Enable math library in generation options to see all modules -# # CONFIG_MODULE_CIRBUF is not set # CONFIG_MODULE_CIRBUF_LARGE is not set # CONFIG_MODULE_FIXED_POINT is not set @@ -97,10 +93,6 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y # # Communication modules # - -# -# uart needs circular buffer, mf2 client may need scheduler -# # CONFIG_MODULE_UART is not set # CONFIG_MODULE_UART_9BITS is not set # CONFIG_MODULE_UART_CREATE_CONFIG is not set @@ -184,10 +176,6 @@ CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG=y # Control system modules # # CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set - -# -# Filters -# # CONFIG_MODULE_PID is not set # CONFIG_MODULE_PID_CREATE_CONFIG is not set # CONFIG_MODULE_RAMP is not set @@ -198,20 +186,12 @@ CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG=y # # Radio devices # - -# -# Some radio devices require SPI to be activated -# # CONFIG_MODULE_CC2420 is not set # CONFIG_MODULE_CC2420_CREATE_CONFIG is not set # # Crypto modules # - -# -# Crypto modules depend on utils module -# # CONFIG_MODULE_AES is not set # CONFIG_MODULE_AES_CTR is not set # CONFIG_MODULE_MD5 is not set @@ -221,20 +201,12 @@ CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG=y # # Encodings modules # - -# -# Encoding modules depend on utils module -# # CONFIG_MODULE_BASE64 is not set # CONFIG_MODULE_HAMMING is not set # # Debug modules # - -# -# Debug modules depend on utils module -# # CONFIG_MODULE_DIAGNOSTIC is not set # CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set CONFIG_MODULE_ERROR=y diff --git a/projects/microb2010/tests/oa/graph.py b/projects/microb2010/tests/oa/graph.py index d5946f1..7ba6948 100644 --- a/projects/microb2010/tests/oa/graph.py +++ b/projects/microb2010/tests/oa/graph.py @@ -170,7 +170,7 @@ def build_area(ax): # area x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)]) ax.plot(x, y, 'g-') - + x,y = build_poly([(0,0), (0,500), (500,500), (500,0)]) ax.plot(x, y, 'y-') @@ -179,16 +179,38 @@ def build_area(ax): x,y = build_poly([(740,0), (740,500), (2260,500), (2260,0)]) ax.plot(x, y, 'g--') - + + x,y = build_path([(375,0), (375,2050)]) + ax.plot(x, y, 'r-') + + x,y = build_path([(150,1972), (2850,472)]) + ax.plot(x, y, 'r-') + + x,y = build_path([(0,1722), (3000,1722)]) + ax.plot(x, y, 'r-') + + x,y = build_path([(150,972), (1950,1972)]) + ax.plot(x, y, 'r-') + + acoef = (2850-150)/5. + bcoef = (472-1972)/5. + x,y = build_path([(600,1972), (600+bcoef, 1972-acoef)]) + ax.plot(x, y, 'r-') + # limit #x,y = build_poly([(250,250), (2750,250), (2750,1850), (250,1850)]) #ax.plot(x, y, 'g--') - + + xtick = [i * STEP_CORN_X + OFFSET_CORN_X for i in range(WAYPOINTS_NBX)] + ax.set_xticks(xtick) + ytick = [(i-1) * STEP_CORN_Y/2 + OFFSET_CORN_Y for i in range(WAYPOINTS_NBY*2)] + ax.set_yticks(ytick) + init_corn_table(random.randint(0,8), random.randint(0,3)) waypoints = init_waypoints() mark_all_neigh(5,6) - + wcorn = [] bcorn = [] points = [] @@ -250,7 +272,7 @@ def graph(filename): build_area(ax) - ax.grid() + ax.grid(color = (0.3, 0.3, 0.3)) ax.set_xlim(-100, 3100) ax.set_ylim(-100, 2200) #ax.set_title('spline paths') diff --git a/projects/microb2010/tests/oa/main.c b/projects/microb2010/tests/oa/main.c index 5d05ae9..f2f4c86 100644 --- a/projects/microb2010/tests/oa/main.c +++ b/projects/microb2010/tests/oa/main.c @@ -48,7 +48,7 @@ struct djpoint { struct point pos; uint16_t weight; struct djpoint *parent; - + uint8_t type:3; uint8_t parent_pos:3; uint8_t updated:1; @@ -60,7 +60,7 @@ static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; /* table to find the symetric idx */ uint8_t corn_sym[] = { - 15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2 + 15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2 }; uint8_t corn_side_confs[9][2] = { @@ -95,7 +95,7 @@ void dump(void) printf(" %2d ", i); } printf("\n"); - + for (j=WAYPOINTS_NBY*2-1; j>=0; j--) { printf("%3d ", j/2); @@ -104,7 +104,7 @@ void dump(void) for (i=0; itype == TYPE_WAYPOINT) printf(" %5d ", pt->weight); else - printf(" ? "); + printf(" ? "); } printf("\n"); } @@ -285,7 +285,7 @@ int dijkstra(struct djpoint *start) todolist = 1; cur->todo = 1; cur->updated = 0; - } + } } return 0; /* XXX */ } @@ -365,7 +365,7 @@ void init_waypoints(void) if (i & 1) y = OFFSET_CORN_Y - STEP_CORN_Y/2; else - y = OFFSET_CORN_Y; + y = OFFSET_CORN_Y; for (j=0; j