From ac0bedbdbee0b2794def26b070c841ecd35a21bf Mon Sep 17 00:00:00 2001
From: Steffen Pohle <steffen@gulpe.de>
Date: Sat, 4 Feb 2023 12:46:23 +0100
Subject: [PATCH] pid seem to work..

---
 gui.h                  | 1 +
 posctl.cc              | 6 +++++-
 videodev-simulation.cc | 4 ++--
 3 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/gui.h b/gui.h
index 9adddad..8842ea9 100644
--- a/gui.h
+++ b/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;
 	}
 
diff --git a/posctl.cc b/posctl.cc
index 917d35a..aa7137c 100644
--- a/posctl.cc
+++ b/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) {
diff --git a/videodev-simulation.cc b/videodev-simulation.cc
index f42ef2f..9473163 100644
--- a/videodev-simulation.cc
+++ b/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;
 }