pid first experiments..

posctl.cc

@@ -26,11 +26,36 @@ extern Simulation simulation;
 
 extern GtkBuilder *_builder_;			// work around for threads
 GtkWidget *posctl_rot_da = NULL;
-// GtkWidget *posctl_axis1_da = NULL;
+GtkWidget *posctl_axis1_da = NULL;
-// GtkWidget *posctl_axis1_da = NULL;
+GtkWidget *posctl_axis2_da = NULL;
 double linedash1[] = { 1.0, 4.0 };
 
 
+/*
+ * axis history chart.
+ */
+#define AXIS_HISTORY_CNT 400
+struct AxisHistory {
+	double diff;
+	double out;
+	struct timeval ts;
+};
+struct AxisHistory axis_history[2][AXIS_HISTORY_CNT];
+int axis_history_pos[2] = { 0, 0 };
+
+void axis_history_add(int axis, double diff, double out) {
+	if (axis > 1 || axis < 0) return;
+
+	axis_history_pos[axis]++;
+	if (axis_history_pos[axis] < 0) axis_history_pos[axis] = 0;
+	if (axis_history_pos[axis] >= AXIS_HISTORY_CNT) axis_history_pos[axis] = 0;
+
+	gettimeofday(&axis_history[axis][axis_history_pos[axis]].ts, NULL);
+	axis_history[axis][axis_history_pos[axis]].diff = diff;
+	axis_history[axis][axis_history_pos[axis]].out = out;
+}
+
+
 #define CALIB_MAXSPEED 2.0
 
 void posctl_gui_update();
@@ -169,6 +194,10 @@ void cb_posctl_angles_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer
 
 	if (posctl_rot_da == NULL) // should only be called once
 		posctl_rot_da = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_rotation"));
+	if (posctl_axis1_da == NULL) // should only be called once
+		posctl_axis1_da = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_rotation"));
+	if (posctl_axis2_da == NULL) // should only be called once
+		posctl_axis2_da = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_rotation"));
 	clienth = gtk_widget_get_allocated_height(posctl_rot_da);
 	clientw = gtk_widget_get_allocated_width(posctl_rot_da);
 	center.x = clientw >> 1;
@@ -267,13 +296,21 @@ void cb_posctl_entryanglelen (GtkWidget *widget, gpointer data) {
 };
 
 
+#define AXIS_DIFF_MIN -10.0
+#define AXIS_DIFF_MAX 10.0
+
 void cb_posctl_axis_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer data) {
 	GtkWidget *da1 = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_axis1"));
 	GtkWidget *da2 = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_axis2"));
 	position_2d center;
+	int axis = 0 , i, cnt;
+	double dx, dy;
+	double aoutmin[2];
+	double aoutmax[2];
 
 	int clienth = gtk_widget_get_allocated_height(area);
 	int clientw = gtk_widget_get_allocated_width(area);
+
 	center.x = clientw >> 1;
 	center.y = clienth >> 1;
 
@@ -291,16 +328,73 @@ void cb_posctl_axis_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer da
 	cairo_stroke(cr);
 	cairo_set_dash(cr, NULL, 0, 0);
 
-	cairo_set_source_rgb(cr, 0.5, 0.5, 0.5);
+	if (da1 == area) axis = 0;
-	cairo_select_font_face (cr, "sans-serif", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_NORMAL);
+	else if (da2 == area) axis = 1;
-	cairo_set_font_size (cr, 10);
+	else {
-	cairo_move_to(cr, 30, 10);
-	cairo_show_text(cr, (char *)"offset");
-	if (da1 != area && da2 != area) {
 		cairo_move_to(cr, 30, 0);
 		cairo_show_text(cr, (char *)"unknown");
 		return;
 	}
+	posctl.GetAxisParam(axis, &aoutmin[axis], &aoutmax[axis], NULL, NULL, NULL);
+
+	// draw diff
+	cairo_set_source_rgb(cr, 1.0, 0.5, 0.5);
+	cairo_select_font_face (cr, "sans-serif", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_NORMAL);
+	cairo_set_font_size (cr, 10);
+	cairo_move_to(cr, 00, 10);
+	cairo_show_text(cr, (char *)"diff");
+	// draw elements diff
+	i = axis_history_pos[axis];
+	cnt = 0;
+	do {
+		dx = ((double) clientw) * (axis_history[axis][i].diff - AXIS_DIFF_MIN) /
+				(AXIS_DIFF_MAX-AXIS_DIFF_MIN);
+		dy = ((double) clienth) * ((double)cnt) / ((double) AXIS_HISTORY_CNT);
+
+		if (dx >= clientw)	dx = clientw-1;
+		if (dy >= clienth) 	dy = clienth-1;
+		if (dx < 0) 		dx = 0;
+		if (dy < 0)		 	dy = 0;
+
+		if (cnt == 0) 	cairo_move_to(cr, (int)dx, int(dy));
+		else 			cairo_line_to(cr, (int)dx, int(dy));
+
+		cnt++;
+		i--;
+		if (i < 0) i = AXIS_HISTORY_CNT-1;
+	} while (i != axis_history_pos[axis]);
+	cairo_stroke(cr);
+
+	cairo_set_source_rgb(cr, 0.0, 1.0, 0.5);
+	cairo_select_font_face (cr, "sans-serif", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_NORMAL);
+	cairo_set_font_size (cr, 10);
+	cairo_move_to(cr, 40, 10);
+	cairo_show_text(cr, (char *)"out");
+	// draw elements diff
+	i = axis_history_pos[axis];
+	cnt = 0;
+	do {
+		dx = ((double) clientw) * (axis_history[axis][i].out - aoutmin[axis]) /
+				(aoutmax[axis]-aoutmin[axis]);
+		dy = ((double) clienth) * ((double)cnt) / ((double) AXIS_HISTORY_CNT);
+
+		if (dx >= clientw)	dx = clientw-1;
+		if (dy >= clienth) 	dy = clienth-1;
+		if (dx < 0) 		dx = 0;
+		if (dy < 0)		 	dy = 0;
+
+		if (cnt == 0) 	cairo_move_to(cr, (int)dx, int(dy));
+		else 			cairo_line_to(cr, (int)dx, int(dy));
+
+		cnt++;
+		i--;
+		if (i < 0) i = AXIS_HISTORY_CNT-1;
+	} while (i != axis_history_pos[axis]);
+	cairo_stroke(cr);
+
+
+	// draw chart
+
 };
 
 
@@ -433,7 +527,16 @@ void posctl_gui_update() {
  * the gtk/gui updates must and will be processed in a separate gui thread
  */
 gboolean cb_thread_posctl (gpointer data) {
+	GtkWidget *da1 = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_axis1"));
+	GtkWidget *da2 = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_axis2"));
+
 	posctl_gui_update();
+
+	gtk_widget_queue_draw(da1);
+	gtk_widget_queue_draw(da2);
+	if (posctl_axis1_da) gdk_window_invalidate_rect(gtk_widget_get_window(posctl_axis1_da), NULL, true);
+	if (posctl_axis2_da) gdk_window_invalidate_rect(gtk_widget_get_window(posctl_axis2_da), NULL, true);
+
 	return false;
 };
 
@@ -660,7 +763,11 @@ void PosCtl::Loop (int posx, int posy) {
 		OutputWriteValue(0, out1);
 		OutputWriteValue(1, out2);
 
+		axis_history_add(0, dist_axis2, out1);
+		axis_history_add(1, dist_axis1, out2);
+
 		UnLockMutex();
+		gdk_threads_add_idle(cb_thread_posctl, NULL);
 	}
 
 	else {