X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=modules%2Fdevices%2Frobot%2Ftrajectory_manager%2Ftrajectory_manager_core.c;h=33c2a3040a3838226d70161f10ae719ac69b576a;hp=7adde48ac37e053b23b970ee29db02d8e2ec38ec;hb=d1cc3debac68c0012f2c5ca33da9bcf0882f4988;hpb=91987ff2747a521681d087935148964eed6b3556 diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index 7adde48..33c2a30 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -29,6 +29,8 @@ #include #include #include +#include +#include #include #include @@ -46,6 +48,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 +365,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); @@ -381,8 +391,6 @@ void trajectory_manager_xy_event(struct trajectory *traj) } } - /* XXX circle */ - /* 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) // || ABS(v2pol_target.r) < traj->d_win) @@ -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)); @@ -470,17 +475,176 @@ void trajectory_manager_xy_event(struct trajectory *traj) cs_set_consign(traj->csm_distance, d_consign); } +/* + * 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) +{ + /* speed_d = coef * speed_a */ + double coef; + double speed_d2, speed_a2; + + coef = 2. * radius_mm / traj->position->phys.track_mm; + + speed_d2 = traj->a_speed * coef; + if (speed_d2 < traj->d_speed) { + *speed_d = speed_d2; + *speed_a = traj->a_speed; + } + else { + speed_a2 = traj->d_speed / coef; + *speed_d = traj->d_speed; + *speed_a = speed_a2; + } +} + /* trajectory event for circles */ +/* XXX static */ void trajectory_manager_circle_event(struct trajectory *traj) { -#if 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; -#endif + int32_t d_consign = 0, a_consign = 0; + double angle_to_center_rad; + double coef_p, coef_d; + double d_speed, a_speed; + + /* 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); + + /* radius consign */ + radius = traj->target.circle.radius; + + coef_p = v2pol_target.r / radius; + coef_p = 1. * coef_p; + + angle_to_center_rad = v2pol_target.theta - (M_PI / 2.); + angle_to_center_rad = simple_modulo_2pi(angle_to_center_rad); + if (angle_to_center_rad > 0.5) + angle_to_center_rad = 0.5; + if (angle_to_center_rad < -0.5) + angle_to_center_rad = -0.5; + coef_d = exp(5*angle_to_center_rad); + coef_d = coef_d; + + circle_get_da_speed_from_radius(traj, radius / (coef_p * coef_d), + &d_speed, &a_speed); + set_quadramp_speed(traj, d_speed, a_speed); + + EVT_DEBUG(E_TRAJECTORY, "angle=%2.2f radius=%2.2f r=%2.2f coef_p=%2.2f coef_d=%2.2f " + "d_speed=%2.2f a_speed=%2.2f", + angle_to_center_rad, radius, v2pol_target.r, + coef_p, coef_d, d_speed, a_speed); + + /* XXX check flags */ + d_consign = 400000 + rs_get_distance(traj->robot); + a_consign = 400000 + rs_get_angle(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 ", 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), */ +/* radius); */ + + cs_set_consign(traj->csm_angle, a_consign); + cs_set_consign(traj->csm_distance, d_consign); +} + +/* trajectory event for lines */ +static void trajectory_manager_line_event(struct trajectory *traj) +{ + 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); + double advance, dist_to_line; + point_t robot, proj, target_pt; + int32_t d_consign = 0, a_consign = 0; + vect2_cart v2cart_pos; + vect2_pol v2pol_target; + + robot.x = x; + robot.y = y; + + /* target point on the line is further on the line */ + proj_pt_line(&robot, &traj->target.line.line, &proj); + dist_to_line = pt_norm(&robot, &proj); + if (dist_to_line > traj->target.line.advance) + advance = 0; + else + advance = traj->target.line.advance - dist_to_line; + target_pt.x = proj.x + advance * cos(traj->target.line.angle); + target_pt.y = proj.y + advance * sin(traj->target.line.angle); + + /* target vector */ + v2cart_pos.x = target_pt.x - x; + v2cart_pos.y = target_pt.y - y; + vect2_cart2pol(&v2cart_pos, &v2pol_target); + v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a); + + /* 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) // || ABS(v2pol_target.r) < traj->d_win) + set_quadramp_speed(traj, 0, traj->a_speed); + else { + double coef; + coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad; + set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed); + } + + /* position consign is infinite */ + d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm)); + d_consign += 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.2); + a_consign += rs_get_angle(traj->robot); + + EVT_DEBUG(E_TRAJECTORY, "target.x=%2.2f target.y=%2.2f " + "a_consign=%"PRIi32" d_consign=%"PRIi32, + target_pt.x, target_pt.y, a_consign, d_consign); + + cs_set_consign(traj->csm_angle, a_consign); + cs_set_consign(traj->csm_distance, d_consign); } + /* trajectory event */ void trajectory_manager_event(void * param) { @@ -503,6 +667,10 @@ void trajectory_manager_event(void * param) trajectory_manager_circle_event(traj); break; + case RUNNING_LINE: + trajectory_manager_line_event(traj); + break; + default: break; } @@ -510,31 +678,38 @@ void trajectory_manager_event(void * param) /*********** *CIRCLE */ -/* - * 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) +/* 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) { - /* speed_d = coef * speed_a */ - double coef; - double speed_d2, speed_a2; + double dst_angle; - coef = 2. * radius_mm / traj->position->phys.track_mm; + delete_event(traj); - speed_d2 = traj->a_speed * coef; - if (speed_d2 < traj->d_speed) { - *speed_d = speed_d2; - *speed_a = traj->a_speed; - } - else { - speed_a2 = traj->d_speed / coef; - *speed_d = traj->d_speed; - *speed_a = speed_a2; - } + 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); } /* return the distance in millimeters that corresponds to an angle in @@ -583,3 +758,38 @@ void trajectory_circle_abs_dist_deg(struct trajectory *traj, { } + +/*********** *LINE */ + +/* Follow a line */ +void trajectory_line_abs(struct trajectory *traj, + double x1, double y1, + double x2, double y2, + double advance) +{ + point_t p1, p2; + + delete_event(traj); + + /* find the line EQ */ + p1.x = x1; + p1.y = y1; + p2.x = x2; + p2.y = y2; + pts2line(&p1, &p2, &traj->target.line.line); + + /* find the line angle */ + traj->target.line.angle = atan2(y2-y1, x2-x1); + traj->target.line.advance = advance; + + DEBUG(E_TRAJECTORY, "Line rel (a,b,c)=%2.2f,%2.2f,%2.2f", + traj->target.line.line.a, + traj->target.line.line.b, + traj->target.line.line.c, + traj->target.line.angle); + + traj->state = RUNNING_LINE; + trajectory_manager_event(traj); + schedule_event(traj); +} +