gui.h

@@ -49,20 +49,23 @@ public:
 	    }
 
 	double cosalpha(position_f_2d &pos) {
-		return (x * pos.x + y * pos.y) / (length() * pos.length());
+		return (x * pos.x + y * pos.y) / (lenght() * pos.lenght());
 	}
 
 	double sinalpha(position_f_2d &pos) {
-		return (x * pos.y - y * pos.x) / (length() * pos.length());
+		return (x * pos.y - y * pos.x) / (lenght() * pos.lenght());
 	}
 
 	double perpendicular(position_f_2d &pos, position_f_2d &base) {
+		double l;
 		position_f_2d p = pos - base;
-		double l = p.length();
-		return l == 0.0 ? 0.0 : l = sinalpha(p);
+		l = sinalpha(p) * p.lenght();
+
+		if (isnan(l)) l = 0.0;
+		return l;
 	}
 
-	double length() { return hypot(x, y); }
+	double lenght() { return hypot(x, y); }
 
 	double x;
 	double y;

posctl.cc

@@ -57,8 +57,6 @@ void axis_history_add(int axis, double diff, double out) {
 
 
 #define CALIB_MAXSPEED 2.0
-#define CALIB_DURATION_DELTA 10.0
-#define CALIB_DURATION_AXIS (CALIB_DURATION_DELTA / CALIB_MAXSPEED)
 
 void posctl_gui_update();
 
@@ -634,7 +632,7 @@ void PosCtl::CalibModeDelta(int x, int y) {
 	gettimeofday (&tv, NULL);
 	timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0);
 
-	if (timediff > CALIB_DURATION_DELTA) {
+	if (timediff > 10.0) {
 		position_f_2d fp;
 
 		printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
@@ -668,11 +666,11 @@ void PosCtl::CalibModeAxis(int x, int y) {
 	gettimeofday (&tv, NULL);
 	timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0);
 
-	if (timediff > CALIB_DURATION_AXIS) {
+	if (timediff > 5.0) {
 		printf ("%s:%d %s calib_mode: %d\n", __FILE__, __LINE__, __FUNCTION__, calib_mode);
 
-		fp.x = +(x - calib_pos.x) / timediff;
-		fp.y = -(y - calib_pos.y) / timediff;
+		fp.x = (x - calib_pos.x) / (float)timediff;
+		fp.y = -(y - calib_pos.y) / (float)timediff;
 
 		if (calib_mode ==  POSCTL_CALIB_MODE_AXIS1)	{
 			calib_axis1_v = fp - calib_rot_v;
@@ -755,18 +753,18 @@ void PosCtl::Loop (int posx, int 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);
+		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 (" Axis1 [dist:%+6.3f out1:%+6.3f]  ", dist_axis1, out1);
-		printf (" Axis2 [dist:%+6.3f out2:%+6.3f]\n", dist_axis2, out2);
+		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);
 
-		axis_history_add(0, dist_axis1, out1);
-		axis_history_add(1, dist_axis2, out2);
+		axis_history_add(0, dist_axis2, out1);
+		axis_history_add(1, dist_axis1, out2);
 
 		UnLockMutex();
 		gdk_threads_add_idle(cb_thread_posctl, NULL);
@@ -870,6 +868,7 @@ int PosCtl::OutputWriteValue (int axis, double value) {
 	char outbuf[255];
 	// 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) {
 		simulation.AxisSetValue(axis, value);
 		return 0;

videodev-simulation.cc

@@ -55,6 +55,8 @@ int VideoDev_Simulation::Open() {
 
 	if (inframe != NULL) Close();
 
+//	conf_width = w;
+//	conf_height = h;
 	conf_format = convert_from_pixelformat (V4L2_PIX_FMT_RGB24);
 	simulation.SetResolution(conf_width, conf_height);
 
@@ -117,10 +119,10 @@ int VideoDev_Simulation::CaptureStop() {
  * If something goes wrong return an error code.
  * Return code VDEV_STATUS_AGAIN is not an error. There was no video image ready to read.
  */
-#define SIMULATION_SIZE 16
+#define SIMULATION_SIZE 15
 int VideoDev_Simulation::Grab(VideoFrame *vf) {
-	int posx, posy, x ,y, r, radius;
-	double a, dsec;
+	int posx, posy, x ,y;
+	double r, a, dsec;
 
 	// try to match a speed of 20Hz
 	dsec = get_cycletime(&lastframetv);
@@ -129,8 +131,7 @@ int VideoDev_Simulation::Grab(VideoFrame *vf) {
 	memset (inframe, 0x0, conf_width*conf_height*3);
 	simulation.GetPos(&posx, &posy);
 
-	radius = conf_width * SIMULATION_SIZE / 1920;
-	for (r = 1; r < radius; r++) for (a = 0; a < M_PI * 2.0; a += 0.1) {
+	for (r = 1; r < SIMULATION_SIZE; r++) for (a = 0; a < M_PI * 2.0; a += 0.1) {
 		x = posx + (sin(a) * r);
 		y = posy + (cos(a) * r);
 		if (x >= 0 && x < conf_width && y >= 0 && y < conf_height) {
@@ -307,3 +308,4 @@ void Simulation::AxisStop() {
 	axis[0].active = 0;
 	axis[1].active = 0;
 }
+