- q_d->previous_var = 0;
- q_d->previous_out = rs_get_distance(traj->robot);
- q_a->previous_var = 0;
- q_a->previous_out = rs_get_angle(traj->robot);
-}
-
-
-/************ GOTO XY, USE EVENTS */
-
-/** goto a x,y point, using a trajectory event */
-void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y)
-{
- DEBUG(E_TRAJECTORY, "Goto XY");
- delete_event(traj);
- traj->target.cart.x = x;
- traj->target.cart.y = y;
- traj->state = RUNNING_XY_START;
- trajectory_manager_event(traj);
- schedule_event(traj);
-}
-
-/** go forward to a x,y point, using a trajectory event */
-void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y)
-{
- DEBUG(E_TRAJECTORY, "Goto XY_F");
- delete_event(traj);
- traj->target.cart.x = x;
- traj->target.cart.y = y;
- traj->state = RUNNING_XY_F_START;
- trajectory_manager_event(traj);
- schedule_event(traj);
-}
-
-/** go backward to a x,y point, using a trajectory event */
-void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y)
-{
- DEBUG(E_TRAJECTORY, "Goto XY_B");
- delete_event(traj);
- traj->target.cart.x = x;
- traj->target.cart.y = y;
- traj->state = RUNNING_XY_B_START;
- trajectory_manager_event(traj);
- schedule_event(traj);
-}
-
-/** go forward to a d,a point, using a trajectory event */
-void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a)
-{
- vect2_pol p;
- double x = position_get_x_double(traj->position);
- double y = position_get_y_double(traj->position);
-
- DEBUG(E_TRAJECTORY, "Goto DA rel");
-
- delete_event(traj);
- p.r = d;
- p.theta = RAD(a) + position_get_a_rad_double(traj->position);
- vect2_pol2cart(&p, &traj->target.cart);
- traj->target.cart.x += x;
- traj->target.cart.y += y;
-
- traj->state = RUNNING_XY_START;
- trajectory_manager_event(traj);
- schedule_event(traj);
-}
-
-/** 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)
-{
- vect2_cart c;
- vect2_pol p;
- double x = position_get_x_double(traj->position);
- double y = position_get_y_double(traj->position);
-
- DEBUG(E_TRAJECTORY, "Goto XY rel");
-
- delete_event(traj);
- c.x = x_rel_mm;
- c.y = y_rel_mm;
-
- vect2_cart2pol(&c, &p);
- p.theta += position_get_a_rad_double(traj->position);;
- vect2_pol2cart(&p, &traj->target.cart);
-
- traj->target.cart.x += x;
- traj->target.cart.y += y;
-
- traj->state = RUNNING_XY_START;
- trajectory_manager_event(traj);
- schedule_event(traj);
-}
-
-/************ FUNCS FOR GETTING TRAJ STATE */
-
-/** return true if the position consign is equal to the filtered
- * position consign (after quadramp filter), for angle and
- * distance. */
-uint8_t trajectory_finished(struct trajectory *traj)
-{
- return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) &&
- cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ;
-}
-
-/** return true if traj is nearly finished */
-uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad)
-{
- switch(traj->state) {
-
- case RUNNING_XY_ANGLE_OK:
- case RUNNING_XY_F_ANGLE_OK:
- case RUNNING_XY_B_ANGLE_OK:
- /* if robot coordinates are near the x,y target */
- return is_robot_in_xy_window(traj, d_win);
-
- case RUNNING_A:
- return is_robot_in_angle_window(traj, a_win_rad);
-
- case RUNNING_D:
- return is_robot_in_dist_window(traj, d_win);
-
- case RUNNING_AD:
- return is_robot_in_dist_window(traj, d_win) &&
- is_robot_in_angle_window(traj, a_win_rad);
-
- case RUNNING_XY_START:
- case RUNNING_XY_F_START:
- case RUNNING_XY_B_START:
- case RUNNING_XY_ANGLE:
- case RUNNING_XY_F_ANGLE:
- case RUNNING_XY_B_ANGLE:
- default:
- return 0;
- }
-}
-
-/*********** *TRAJECTORY EVENT FUNC */
-
-/** event called for xy trajectories */
-static void trajectory_manager_event(void * param)
-{
- struct trajectory *traj = (struct trajectory *)param;
- 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;
-
- /* 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 */
-
- switch (traj->state) {
- case RUNNING_XY_START:
- case RUNNING_XY_ANGLE:
- case RUNNING_XY_ANGLE_OK:
- case RUNNING_XY_F_START:
- case RUNNING_XY_F_ANGLE:
- case RUNNING_XY_F_ANGLE_OK:
- case RUNNING_XY_B_START:
- case RUNNING_XY_B_ANGLE:
- case RUNNING_XY_B_ANGLE_OK:
-
- /* process the command vector from current position to
- * absolute target, or to the center of the circle. */
- v2cart_pos.x = traj->target.cart.x - x;
- v2cart_pos.y = traj->target.cart.y - y;
- vect2_cart2pol(&v2cart_pos, &v2pol_target);
- v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
-
- /* asked to go backwards */
- if (traj->state >= RUNNING_XY_B_START &&
- traj->state <= RUNNING_XY_B_ANGLE_OK ) {
- v2pol_target.r = -v2pol_target.r;
- v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
- }
-
- /* if we don't need to go forward */
- if (traj->state >= RUNNING_XY_START &&
- traj->state <= RUNNING_XY_ANGLE_OK ) {
- /* If the target is behind the robot, we need to go
- * backwards. 0.52 instead of 0.5 because we prefer to
- * go forward */
- if ((v2pol_target.theta > 0.52*M_PI) ||
- (v2pol_target.theta < -0.52*M_PI ) ) {
- v2pol_target.r = -v2pol_target.r;
- v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
- }
- }
-
- /* 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)
- 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);
- }
-
- d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
- d_consign += rs_get_distance(traj->robot);
-
- /* angle consign */
- /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations */
- 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);
-
- break;
-
- default:
- /* hmmm quite odd, delete the event */
- DEBUG(E_TRAJECTORY, "GNI ???");
- delete_event(traj);
- traj->state = READY;
- }
-
-
- /* step 2 : update state, or delete event if we reached the
- * destination */
-
- /* XXX if target is our pos !! */
-
- switch (traj->state) {
- case RUNNING_XY_START:
- case RUNNING_XY_F_START:
- case RUNNING_XY_B_START:
- /* START -> ANGLE */
- DEBUG(E_TRAJECTORY, "-> ANGLE");
- traj->state ++;
- break;
-
- case RUNNING_XY_ANGLE:
- case RUNNING_XY_F_ANGLE:
- case RUNNING_XY_B_ANGLE: {
- struct quadramp_filter *q_a;
- 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)) {
- delete_event(traj);
- }
- /* ANGLE -> ANGLE_OK */
- traj->state ++;
- DEBUG(E_TRAJECTORY, "-> ANGLE_OK");
- }
- break;
- }
-
- case RUNNING_XY_ANGLE_OK:
- 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)) {
- delete_event(traj);
- }
- break;
-
- default:
- break;
- }
-
- /* 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 ", "
- "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);
-}
-
-/*********** *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)
-{
- /* 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;
- }
-}
-
-/* return the distance in millimeters that corresponds to an angle in
- * degree and a radius in mm */
-/* static */double circle_get_dist_from_degrees(double radius_mm, double a_deg)
-{
- double a_rad = RAD(a_deg);
- return a_rad * radius_mm;
-}
-
-/*
- * Start a circle of specified radius around the specified center
- * (relative with d,a). The distance is specified in mm.
- */
-void trajectory_circle(struct trajectory *traj,
- double center_d_mm, double center_a_rad,
- double radius_mm, double dist_mm)
-{
-/* double */
-
-/* DEBUG(E_TRAJECTORY, "CIRCLE to d=%f a_rad=%f", center_d_mm, */
-/* center_a_rad); */
-/* delete_event(traj); */
-/* traj->state = RUNNING_CIRCLE; */
-
-
-}
-
-/*
- * Start a circle of specified radius around the specified center
- * (absolute). The distance is specified in mm.
- */
-void trajectory_circle_abs_dist_mm(struct trajectory *traj,
- double x_rel_mm, double y_rel_mm,
- double radius_mm, double dist_mm)
-{
-}
-
-/*
- * Start a circle of specified radius around the specified center
- * (absolute). The distance is specified in degrees.
- */
-void trajectory_circle_abs_dist_deg(struct trajectory *traj,
- double x_rel_mm, double y_rel_mm,
- double radius_mm, double dist_degrees)
-{
-
-}