From dcb7cd39f9e3026ac926fa471056bf52f3f4023f Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 9 Jan 2010 21:53:45 +0100 Subject: [PATCH] traj circle --- modules/base/math/fixed_point/test/.config | 37 ++++- .../trajectory_manager/trajectory_manager.h | 16 +++ .../trajectory_manager_core.c | 127 ++++++++++++++++-- projects/microb2009/mainboard/commands.c | 2 + projects/microb2009/mainboard/commands_traj.c | 28 ++++ 5 files changed, 198 insertions(+), 12 deletions(-) diff --git a/modules/base/math/fixed_point/test/.config b/modules/base/math/fixed_point/test/.config index 296da2c..4bcbaf7 100644 --- a/modules/base/math/fixed_point/test/.config +++ b/modules/base/math/fixed_point/test/.config @@ -45,9 +45,12 @@ # CONFIG_MCU_ATMEGA645 is not set # CONFIG_MCU_ATMEGA6450 is not set CONFIG_MCU_ATMEGA128=y +# CONFIG_MCU_ATMEGA1281 is not set # CONFIG_MCU_AT90CAN128 is not set # CONFIG_MCU_AT94K is not set # CONFIG_MCU_AT90S1200 is not set +# CONFIG_MCU_ATMEGA2560 is not set +# CONFIG_MCU_ATMEGA256 is not set CONFIG_QUARTZ=16000000 # @@ -60,6 +63,7 @@ CONFIG_QUARTZ=16000000 CONFIG_OPTM_S=y CONFIG_MATH_LIB=y # CONFIG_FDEVOPEN_COMPAT is not set +# CONFIG_NO_PRINTF is not set # CONFIG_MINIMAL_PRINTF is not set # CONFIG_STANDARD_PRINTF is not set CONFIG_ADVANCED_PRINTF=y @@ -78,13 +82,17 @@ CONFIG_MODULE_CIRBUF=y # CONFIG_MODULE_CIRBUF_LARGE is not set CONFIG_MODULE_FIXED_POINT=y # CONFIG_MODULE_VECT2 is not set +# CONFIG_MODULE_GEOMETRY is not set # CONFIG_MODULE_SCHEDULER is not set +# CONFIG_MODULE_SCHEDULER_STATS is not set # CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set # CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set CONFIG_MODULE_SCHEDULER_TIMER0=y # CONFIG_MODULE_SCHEDULER_MANUAL is not set # CONFIG_MODULE_TIME is not set # CONFIG_MODULE_TIME_CREATE_CONFIG is not set +# CONFIG_MODULE_TIME_EXT is not set +# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set # # Communication modules @@ -94,7 +102,10 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y # uart needs circular buffer, mf2 client may need scheduler # CONFIG_MODULE_UART=y +# CONFIG_MODULE_UART_9BITS is not set CONFIG_MODULE_UART_CREATE_CONFIG=y +# CONFIG_MODULE_SPI is not set +# CONFIG_MODULE_SPI_CREATE_CONFIG is not set # CONFIG_MODULE_I2C is not set # CONFIG_MODULE_I2C_MASTER is not set # CONFIG_MODULE_I2C_MULTIMASTER is not set @@ -113,6 +124,7 @@ CONFIG_MODULE_UART_CREATE_CONFIG=y # CONFIG_MODULE_TIMER_DYNAMIC is not set # CONFIG_MODULE_PWM is not set # CONFIG_MODULE_PWM_CREATE_CONFIG is not set +# CONFIG_MODULE_PWM_NG is not set # CONFIG_MODULE_ADC is not set # CONFIG_MODULE_ADC_CREATE_CONFIG is not set @@ -126,6 +138,7 @@ CONFIG_MODULE_UART_CREATE_CONFIG=y # CONFIG_MODULE_RDLINE_KILL_BUF is not set # CONFIG_MODULE_RDLINE_HISTORY is not set # CONFIG_MODULE_PARSE is not set +# CONFIG_MODULE_PARSE_NO_FLOAT is not set # # External devices modules @@ -134,6 +147,8 @@ CONFIG_MODULE_UART_CREATE_CONFIG=y # CONFIG_MODULE_LCD_CREATE_CONFIG is not set # CONFIG_MODULE_MULTISERVO is not set # CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set +# CONFIG_MODULE_AX12 is not set +# CONFIG_MODULE_AX12_CREATE_CONFIG is not set # # Brushless motor drivers (you should enable pwm modules to see all) @@ -144,22 +159,27 @@ CONFIG_MODULE_UART_CREATE_CONFIG=y # CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set # -# Encoders +# Encoders (you need comm/spi for encoders_spi) # # CONFIG_MODULE_ENCODERS_MICROB is not set # CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set # CONFIG_MODULE_ENCODERS_EIRBOT is not set # CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set +# CONFIG_MODULE_ENCODERS_SPI is not set +# 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 # CONFIG_MODULE_TRAJECTORY_MANAGER is not set # CONFIG_MODULE_BLOCKING_DETECTION_MANAGER is not set # CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set +# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set # # Control system modules @@ -176,6 +196,16 @@ CONFIG_MODULE_UART_CREATE_CONFIG=y # CONFIG_MODULE_QUADRAMP_DERIVATE is not set # CONFIG_MODULE_BIQUAD is not set +# +# 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 # @@ -237,7 +267,9 @@ CONFIG_AVRDUDE_PROG_STK200=y # CONFIG_AVRDUDE_PROG_BSD is not set # CONFIG_AVRDUDE_PROG_DAPA is not set # CONFIG_AVRDUDE_PROG_JTAG1 is not set +# CONFIG_AVRDUDE_PROG_AVR109 is not set CONFIG_AVRDUDE_PORT="/dev/parport0" +CONFIG_AVRDUDE_BAUDRATE=19200 # # Avarice @@ -246,3 +278,4 @@ CONFIG_AVARICE_PORT="/dev/ttyS0" CONFIG_AVARICE_DEBUG_PORT=1234 CONFIG_AVARICE_PROG_MKI=y # CONFIG_AVARICE_PROG_MKII is not set +# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.h b/modules/devices/robot/trajectory_manager/trajectory_manager.h index 648ec1c..3ebd586 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager.h +++ b/modules/devices/robot/trajectory_manager/trajectory_manager.h @@ -47,6 +47,15 @@ enum trajectory_state { RUNNING_CIRCLE, }; +struct circle_target { + vect2_cart center; /**< center of the circle */ + double radius; /**< radius of the circle */ + int32_t dest_angle; /**< dst angle in inc */ + +#define TRIGO 1 /* rotation is counterclockwise */ +#define FORWARD 2 /* go forward or backward */ + uint8_t flags; /**< flags for this trajectory */ +}; struct trajectory { enum trajectory_state state; /*<< describe the type of target, and if we reached the target */ @@ -54,6 +63,7 @@ struct trajectory { union { vect2_cart cart; /**<< target, if it is a x,y vector */ struct rs_polar pol; /**<< target, if it is a d,a vector */ + struct circle_target circle; /**<< target, if it is a circle */ } target; double d_win; /**<< distance window (for END_NEAR) */ @@ -160,4 +170,10 @@ void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a); /** go forward to a x,y relative point, using a trajectory event */ void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm); +/** make the robot orbiting around (x,y) on a circle whose radius is + * radius_mm, and exit when relative destination angle is reached. The + * flags set if we go forward or backwards, and CW/CCW. */ +void trajectory_circle_rel(struct trajectory *traj, double x, double y, + double radius_mm, double rel_a_deg, uint8_t flags); + #endif //TRAJECTORY_MANAGER diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index 7adde48..a7473c4 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -46,6 +46,14 @@ #define RESET_D 4 #define RESET_A 8 +static uint8_t evt_debug_cpt = 0; +#define EVT_DEBUG(args...) do { \ + if (((evt_debug_cpt ++) & 0x07) == 0) { \ + DEBUG(args); \ + } \ + } while (0) + + /** * update angle and/or distance * this function is not called directly by the user @@ -355,7 +363,7 @@ void trajectory_manager_xy_event(struct trajectory *traj) case RUNNING_XY_B_ANGLE_OK: /* process the command vector from current position to - * absolute target, or to the center of the circle. */ + * absolute target. */ v2cart_pos.x = traj->target.cart.x - x; v2cart_pos.y = traj->target.cart.y - y; vect2_cart2pol(&v2cart_pos, &v2pol_target); @@ -433,7 +441,7 @@ void trajectory_manager_xy_event(struct trajectory *traj) q_a = traj->csm_angle->consign_filter_params; /* if d_speed is not 0, we are in start_angle_win */ if (get_quadramp_distance_speed(traj)) { - if(is_robot_in_xy_window(traj, traj->d_win)) { + if (is_robot_in_xy_window(traj, traj->d_win)) { delete_event(traj); } /* ANGLE -> ANGLE_OK */ @@ -447,7 +455,7 @@ void trajectory_manager_xy_event(struct trajectory *traj) case RUNNING_XY_F_ANGLE_OK: case RUNNING_XY_B_ANGLE_OK: /* If we reached the destination */ - if(is_robot_in_xy_window(traj, traj->d_win)) { + if (is_robot_in_xy_window(traj, traj->d_win)) { delete_event(traj); } break; @@ -458,10 +466,7 @@ void trajectory_manager_xy_event(struct trajectory *traj) /* step 3 : send the processed commands to cs */ - DEBUG(E_TRAJECTORY, "EVENT XY cur=(%f,%f,%f) cart=(%f,%f) pol=(%f,%f)", - x, y, a, v2cart_pos.x, v2cart_pos.y, v2pol_target.r, v2pol_target.theta); - - DEBUG(E_TRAJECTORY,"d_cur=%" PRIi32 ", d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", " + EVT_DEBUG(E_TRAJECTORY,"EVENT XY d_cur=%" PRIi32 ", d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", " "a_cur=%" PRIi32 ", a_consign=%" PRIi32 ", a_speed=%" PRIi32, rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj), rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj)); @@ -471,14 +476,82 @@ void trajectory_manager_xy_event(struct trajectory *traj) } /* trajectory event for circles */ +/* XXX static */ void trajectory_manager_circle_event(struct trajectory *traj) { -#if 0 + double coef = 1.0; 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; -#endif + int32_t d_consign = 0, a_consign = 0; + double angle_to_center_rad; + + /* These vectors contain target position of the robot in + * its own coordinates */ + vect2_cart v2cart_pos; + vect2_pol v2pol_target; + + /* step 1 : process new commands to quadramps */ + + /* process the command vector from current position to the + * center of the circle. */ + v2cart_pos.x = traj->target.circle.center.x - x; + v2cart_pos.y = traj->target.circle.center.y - y; + vect2_cart2pol(&v2cart_pos, &v2pol_target); + v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a); + + /* 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 > traj->target.circle.radius * 2) + angle_to_center_rad = 0; + else { + angle_to_center_rad = 1. - (v2pol_target.r / + (2 * traj->target.circle.radius)); + angle_to_center_rad *= M_PI; + } + + /* XXX check flags */ + v2pol_target.theta -= angle_to_center_rad; + + /* If the robot is correctly oriented to start moving in distance */ + /* here limit dist speed depending on v2pol_target.theta */ + 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); + } + + /* XXX check flags */ + d_consign = 40000 + rs_get_distance(traj->robot); + + /* angle consign */ + a_consign = (int32_t)(v2pol_target.theta * + (traj->position->phys.distance_imp_per_mm) * + (traj->position->phys.track_mm) / 2.0); + a_consign += rs_get_angle(traj->robot); + + /* step 2 : update state, or delete event if we reached the + * destination */ + + /* output angle -> delete event */ + if (a_consign >= traj->target.circle.dest_angle) { + a_consign = traj->target.circle.dest_angle; + delete_event(traj); + } + + /* step 3 : send the processed commands to cs */ + + EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32 + ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32 + ", a_speed=%" PRIi32, + rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj), + rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj)); + + cs_set_consign(traj->csm_angle, a_consign); + cs_set_consign(traj->csm_distance, d_consign); } /* trajectory event */ @@ -510,6 +583,40 @@ void trajectory_manager_event(void * param) /*********** *CIRCLE */ +/* make the robot orbiting around (x,y) on a circle whose radius is + * radius_mm, and exit when relative destination angle is reached. The + * flags set if we go forward or backwards, and CW/CCW. */ +void trajectory_circle_rel(struct trajectory *traj, + double x, double y, + double radius_mm, + double rel_a_deg, + uint8_t flags) +{ + double dst_angle; + + delete_event(traj); + + traj->target.circle.center.x = x; + traj->target.circle.center.y = y; + traj->target.circle.radius = radius_mm; + traj->target.circle.flags = flags; + + /* convert in steps */ + dst_angle = RAD(rel_a_deg) * + (traj->position->phys.distance_imp_per_mm) * + (traj->position->phys.track_mm) / 2.0; + + traj->target.circle.dest_angle = rs_get_angle(traj->robot); + traj->target.circle.dest_angle += dst_angle; + + DEBUG(E_TRAJECTORY, "Circle rel (x,y)=%2.2f,%2.2f r=%2.2f flags=%x dst_angle=%"PRIi32"", + x, y, radius_mm, flags, traj->target.circle.dest_angle); + + traj->state = RUNNING_CIRCLE; + trajectory_manager_event(traj); + schedule_event(traj); +} + /* * Compute the fastest distance and angle speeds matching the radius * from current traj_speed diff --git a/projects/microb2009/mainboard/commands.c b/projects/microb2009/mainboard/commands.c index 107606e..61fa530 100644 --- a/projects/microb2009/mainboard/commands.c +++ b/projects/microb2009/mainboard/commands.c @@ -110,6 +110,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; @@ -207,6 +208,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/microb2009/mainboard/commands_traj.c b/projects/microb2009/mainboard/commands_traj.c index 715f167..82477a3 100644 --- a/projects/microb2009/mainboard/commands_traj.c +++ b/projects/microb2009/mainboard/commands_traj.c @@ -489,6 +489,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 */ @@ -539,6 +540,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(); @@ -592,6 +598,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 */ -- 2.20.1