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=f5e0add261f5e221f8863fe7deb15e2db3c8e5a9;hp=cc2a58af368ea9df97bf4597c99f1ea53a5c1842;hb=450875fb3d6f1f6966e1b0f79af0572126eea3d3;hpb=966d2c97e5fda37297568c5e8ed66271e986bcdd

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
index cc2a58a..f5e0add 100644
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
@@ -485,12 +485,37 @@ void trajectory_manager_circle_event(struct trajectory *traj)
 	double a = position_get_a_rad_double(traj->position);
 	int32_t d_consign = 0, a_consign = 0;
 	double angle_to_center_rad;
+	static int32_t d_prev, a_prev;
+	int32_t d_speed, a_speed;
+	int32_t d_pos, a_pos;
+
+	d_pos = rs_get_distance(traj->robot);
+	a_pos = rs_get_angle(traj->robot);
+	d_speed = d_pos - d_prev;
+	a_speed = a_pos - a_prev;
+	d_prev = d_pos;
+	a_prev = a_pos;
 
 	/* These vectors contain target position of the robot in
 	 * its own coordinates */
 	vect2_cart v2cart_pos;
 	vect2_pol v2pol_target;
 
+	int32_t delta_d, delta_a;
+	double coef_deriv = traj->circle_coef;
+	double new_radius;
+	double new_angle;
+	struct  quadramp_filter * q_d, * q_a;
+
+	q_d = traj->csm_distance->consign_filter_params;
+	q_a = traj->csm_angle->consign_filter_params;
+	/*
+	delta_a = a_speed;//q_a->previous_var;
+	delta_d = d_speed;//q_d->previous_var;
+	*/
+	delta_a = q_a->previous_var;
+	delta_d = q_d->previous_var;
+
 	/* step 1 : process new commands to quadramps */
 
 	/* process the command vector from current position to the
@@ -500,17 +525,32 @@ void trajectory_manager_circle_event(struct trajectory *traj)
 	vect2_cart2pol(&v2cart_pos, &v2pol_target);
 	v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
 
+#if 0
+	/* pas trop mal, mais oscille */
+	//new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv;
+	if (v2pol_target.r > traj->target.circle.radius/2)
+		new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv * traj->target.circle.radius / v2pol_target.r;
+	else
+		new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv ;
+
+	/* oscille a mort */
+	//new_radius = traj->target.circle.radius - traj->target.circle.radius   * delta_a * delta_a * coef_deriv;
+
+	/* ? */
+	//new_radius = traj->target.circle.radius - traj->target.circle.radius   * delta_a * coef_deriv;
+
+
 	/* 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) {
-		angle_to_center_rad = traj->target.circle.radius / v2pol_target.r;
+	if (v2pol_target.r > new_radius) {
+		angle_to_center_rad = new_radius / v2pol_target.r;
 		angle_to_center_rad *= (M_PI / 2);
 	}
 	else {
 		angle_to_center_rad = 1. - (v2pol_target.r /
-					    (2 * traj->target.circle.radius));
+					    (2 * new_radius));
 		angle_to_center_rad *= M_PI;
 	}
 
@@ -535,6 +575,53 @@ void trajectory_manager_circle_event(struct trajectory *traj)
 			      (traj->position->phys.track_mm) / 2.0);
 	a_consign += rs_get_angle(traj->robot);
 
+#endif
+
+	d_speed/=100;
+	if (d_speed > v2pol_target.r)
+		d_speed = v2pol_target.r/2;
+	if (d_speed == 0)
+		d_speed = v2pol_target.r/2;
+	new_angle = (d_speed * d_speed);
+	new_angle += (v2pol_target.r * v2pol_target.r);
+	new_angle -= (traj->target.circle.radius * traj->target.circle.radius);
+	new_angle /= 2 * d_speed * v2pol_target.r;
+
+	printf("robot %f %f dspeed %d polr %f pola %f cnewa %f\n",
+	       x, y,
+	       d_speed,
+	       v2pol_target.r, v2pol_target.theta * 180 / M_PI,
+	       new_angle);
+
+	if (new_angle < -1)
+		new_angle = -1;
+
+	if (new_angle > 1)
+		new_angle = 1;
+
+
+	new_angle = acos(new_angle);
+	printf("new_a: %f\n", new_angle*180/M_PI);
+
+	new_angle = v2pol_target.theta - new_angle;
+	a_consign = (int32_t)(new_angle *
+			      (traj->position->phys.distance_imp_per_mm) *
+			      (traj->position->phys.track_mm) / 2.0);
+	a_consign += rs_get_angle(traj->robot);
+
+
+	/* XXX check flags */
+	d_consign = 40000 + rs_get_distance(traj->robot);
+
+
+
+
+
+
+
+
+
+
 	/* step 2 : update state, or delete event if we reached the
 	 * destination */
 
@@ -548,9 +635,10 @@ void trajectory_manager_circle_event(struct trajectory *traj)
 
 	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));
+		  ", 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),
+		  new_radius);
 
 	cs_set_consign(traj->csm_angle, a_consign);
 	cs_set_consign(traj->csm_distance, d_consign);