From 047c075a9918d9219156f747008e9e3a35da442c Mon Sep 17 00:00:00 2001 From: Steffen Pohle Date: Sat, 4 Feb 2023 20:02:48 +0100 Subject: [PATCH] pid first experiments.. --- posctl.cc | 123 ++++++++++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 115 insertions(+), 8 deletions(-) diff --git a/posctl.cc b/posctl.cc index aa7137c..4fd84d2 100644 --- a/posctl.cc +++ b/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); - 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, 30, 10); - cairo_show_text(cr, (char *)"offset"); - if (da1 != area && da2 != area) { + if (da1 == area) axis = 0; + else if (da2 == area) axis = 1; + else { 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 {