pid seem to work..

gui.h

@@ -61,6 +61,7 @@ public:
 		position_f_2d p = pos - base;
 		l = sinalpha(p) * p.lenght();
 
+		if (isnan(l)) l = 0.0;
 		return l;
 	}
 

posctl.cc

@@ -653,6 +653,10 @@ void PosCtl::Loop (int posx, int posy) {
 		out1 = pid_axis[0].Update(0.0, dist_axis2);
 		out2 = pid_axis[1].Update(0.0, dist_axis1);
 
+		printf ("%s:%d %s", __FILE__, __LINE__, __FUNCTION__);
+		printf ("Axis 1 dist: %5.3f out 2: %5.3f   ", dist_axis2, out1);
+		printf ("Axis 2 dist: %5.3f out 1: %5.3f\n", dist_axis1, out2);
+
 		OutputWriteValue(0, out1);
 		OutputWriteValue(1, out2);
 
@@ -755,7 +759,7 @@ int PosCtl::ReadTTY (char * inbuf, int length) {
 
 int PosCtl::OutputWriteValue (int axis, double value) {
 	char outbuf[255];
-	printf ("%s:%d %s   Axis %d   Value:%f\n", __FILE__, __LINE__, __FUNCTION__, axis, value);
+	// printf ("%s:%d %s   Axis %d   Value:%f\n", __FILE__, __LINE__, __FUNCTION__, axis, value);
 
 	if (isnan(value)) return -1;
 	if (device_type == POSCTL_DEVTYPE_SIM) {

videodev-simulation.cc

@@ -296,7 +296,7 @@ void Simulation::SetResolution (int w, int h) {
 
 
 void Simulation::AxisSetValue (int a, double v) {
-	printf ("%s:%d %s Axis:%d   Value:%f\n", __FILE__, __LINE__, __FUNCTION__, a, v);
+	// printf ("%s:%d %s Axis:%d   Value:%f\n", __FILE__, __LINE__, __FUNCTION__, a, v);
 	if (a < 0 || a > 1) return;
 	axis[a].v = v;
 	axis[a].active = 1;
@@ -304,7 +304,7 @@ void Simulation::AxisSetValue (int a, double v) {
 
 
 void Simulation::AxisStop() {
-	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
+	// printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
 	axis[0].active = 0;
 	axis[1].active = 0;
 }