From: zer0 <zer0@platinum>
Date: Mon, 25 Jan 2010 21:12:27 +0000 (+0100)
Subject: test serpi
X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=450875fb3d6f1f6966e1b0f79af0572126eea3d3

test serpi
---

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
index 85b2e57..f5e0add 100644
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
@@ -504,12 +504,17 @@ void trajectory_manager_circle_event(struct trajectory *traj)
 	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 */
 
@@ -520,6 +525,7 @@ 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)
@@ -569,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 */
 
diff --git a/projects/microb2010/tests/hostsim/display.py b/projects/microb2010/tests/hostsim/display.py
index a6c90e7..1e408c9 100644
--- a/projects/microb2010/tests/hostsim/display.py
+++ b/projects/microb2010/tests/hostsim/display.py
@@ -48,7 +48,7 @@ while True:
             set_robot(x,y,a)
         f.close()
 
-    """
+
     k = scene.kb.getkey()
     x,y,z = scene.center
     if k == "left":
@@ -59,4 +59,5 @@ while True:
         scene.center = x,y+10,z
     elif k == "down":
         scene.center = x,y-10,z
-    """
+    elif k == "k":
+        robot_trail.pos = robot_trail.pos[0:0]
diff --git a/projects/microb2010/tests/hostsim/main.c b/projects/microb2010/tests/hostsim/main.c
index 0182943..3907c2a 100644
--- a/projects/microb2010/tests/hostsim/main.c
+++ b/projects/microb2010/tests/hostsim/main.c
@@ -72,10 +72,19 @@ int main(void)
 	time_wait_ms(1000);
 	printf("init\n");
 	trajectory_d_rel(&mainboard.traj, 1000);
-	time_wait_ms(2000);
+	time_wait_ms(1300);
+	/*
 	printf("init\n");
 	trajectory_goto_xy_abs(&mainboard.traj, 1500, 2000);
 	time_wait_ms(2000);
+	*/
+	trajectory_circle_rel(&mainboard.traj,
+			      1000, 500,
+			      500,
+			      400,
+			      0);
+
+	time_wait_ms(10000);
 	return 0;
 }