some more work... and no solution

debug-angles.cc

@@ -17,8 +17,8 @@ position_f_2d dbga_pos;
 double dbga_angle = NAN;
 double dbga_len = NAN;
 
-#define DBGA_W 400
+#define DBGA_W 200
-#define DBGA_H 400
+#define DBGA_H 200
 
 void debug_angles_calculate() {
 	position_f_2d center;
@@ -45,20 +45,16 @@ void debug_angles_draw(cairo_t *cr) {
 	cairo_line_to(cr, DBGA_W, DBGA_H);
 	cairo_line_to(cr, 0, DBGA_H);
 	cairo_stroke(cr);
-	cairo_set_source_rgb(cr, 0.5, 0.5, 0.5);
-	cairo_select_font_face (cr, "sans-serif", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_BOLD);
-	cairo_set_font_size (cr, 12);
-	draw_text(cr, 10, 10, 0.5, "Debug Angles");
 
 	if (isnan(dbga_axis.x)) return;
 
 	// len
 	cairo_set_source_rgb(cr, 0.0, 0.0, 1.0);
-	draw_printf(cr, 250, 10, 0.0, (char*)"Len: %5.2f", dbga_len);
+	draw_printf(cr, 100, 10, 0.0, (char*)"Len: %5.2f", dbga_len);
 
 	// draw pos
 	cairo_set_source_rgb(cr, 0.3, 0.3, 1.0);
-	draw_printf(cr, 50, 10, 0.0, (char*)"Pos: %5.1f,%5.1f", dbga_pos.x, dbga_pos.y);
+	draw_printf(cr, 0, 10, 0.0, (char*)"Pos: %5.1f,%5.1f", dbga_pos.x, dbga_pos.y);
 	cairo_set_line_width(cr, 3.0);
 	cairo_move_to(cr, centerX, centerY);
 	cairo_line_to(cr, dbga_pos.x, dbga_pos.y);
@@ -76,7 +72,7 @@ void debug_angles_draw(cairo_t *cr) {
 
 	// draw axis
 	cairo_set_source_rgb(cr, 0.5, 0.5, 0.5);
-	draw_printf(cr, 50, 25, 0.0, (char*)"Axis: %5.1f,%5.1f", dbga_axis.x, dbga_axis.y);
+	draw_printf(cr, 0, 25, 0.0, (char*)"Axis: %5.1f,%5.1f", dbga_axis.x, dbga_axis.y);
 	cairo_set_line_width(cr, 3.0);
 	cairo_move_to(cr, centerX, centerY);
 	cairo_line_to(cr, centerX + dbga_axis.x, centerY + dbga_axis.y);

posctl.cc

@@ -745,25 +745,29 @@ void PosCtl::Loop (int posx, int posy) {
 		double dist_axis1, out1;
 		double dist_axis2, out2;
 		position_f_2d p;
+		double d;
 
 		LockMutex();
 
 		// calculate
 		// dist_axis < 0.0 pid should increase the output.
 		//           > 0.0 pid should decrease the output
+		p.x = 0.0; p.y = 0.0;
+		d = calib_axis1_v.perpendicular(calib_axis2_v, p);
+
 		p.x = posx; p.y = posy;
 		dist_axis1 = calib_axis1_v.perpendicular(p, target_pos);
 		dist_axis2 = calib_axis2_v.perpendicular(p, target_pos);
-
 		out1 = pid_axis[0].Update(0.0, dist_axis1);
 		out2 = pid_axis[1].Update(0.0, dist_axis2);
 
 		printf ("%s:%d %s", __FILE__, __LINE__, __FUNCTION__);
+		printf (" d: %+6.3f   ", d);
 		printf (" Axis1 [dist:%+6.3f out1:%+6.3f]  ", dist_axis1, out1);
 		printf (" Axis2 [dist:%+6.3f out2:%+6.3f]\n", dist_axis2, out2);
 
-		OutputWriteValue(0, out1);
+		OutputWriteValue(0, out2);
-		OutputWriteValue(1, out2);
+		OutputWriteValue(1, out1);
 
 		axis_history_add(0, dist_axis1, out1);
 		axis_history_add(1, dist_axis2, out2);

videodev-simulation.cc

@@ -202,12 +202,12 @@ Simulation::Simulation() {
 	i = random() % 2;
 
 	axis[i % 2].defAngle = dAngle + (10.0 * (double)random() / ((double) RAND_MAX)) - 5.0;
-	axis[i % 2].defLen = dLen + (1.0 * (double)random() / ((double) RAND_MAX) - 0.5);
+	axis[i % 2].defLen = dLen + (3.0 * (double)random() / ((double) RAND_MAX) - 1.5);
 	axis[i % 2].v = 1.0;
 	axis[i % 2].active = 1;
 	i++;
 	axis[i % 2].defAngle = dAngle + (10.0 * (double)random() / ((double) RAND_MAX)) + 85.0;
-	axis[i % 2].defLen = dLen + (1.0 * (double)random() / ((double) RAND_MAX) - 0.5);
+	axis[i % 2].defLen = dLen + (3.0 * (double)random() / ((double) RAND_MAX) - 1.5);
 	axis[i % 2].v = 1.0;
 	axis[i % 2].active = 1;