diff --git a/configuration.cc b/configuration.cc index 4c8fab6..287d66b 100644 --- a/configuration.cc +++ b/configuration.cc @@ -95,7 +95,7 @@ void Configuration::SaveConfig(std::string filename) { jaxis.AddObject("kp", akp); jaxis.AddObject("ki", aki); jaxis.AddObject("kd", akd); - printf ("%s:%d save config axis %d Out Range: %f - %f kP:%f kI:%f kD:%f\n", + printf ("%s:%d save config axis %d Out Range: %f - %f kP:%g kI:%g kD:%g\n", __FILE__, __LINE__, i, amin, amax, akp, aki, akd); jp.AddObject("posctl_axis" + to_string(i), jaxis); } diff --git a/gui.h b/gui.h index 154ff9e..b2938dd 100644 --- a/gui.h +++ b/gui.h @@ -181,6 +181,8 @@ G_MODULE_EXPORT void cb_posctl_axis_draw(GtkWidget *area, cairo_t *cr, int w, in G_MODULE_EXPORT void cb_posctl_change_entry (GtkWidget *widget, gpointer data); G_MODULE_EXPORT void cb_posctl_btn_axismove (GtkWidget *widget, gpointer data); +G_MODULE_EXPORT void cb_posctl_btnsimreset (GtkWidget *widget, gpointer data); + // // menu elements G_MODULE_EXPORT void cb_menu_set_rgbenc (GtkCheckMenuItem *checkmenuitem, gpointer user_data); diff --git a/pid.cc b/pid.cc index 2a58e2f..6b351f8 100644 --- a/pid.cc +++ b/pid.cc @@ -126,7 +126,7 @@ double PID::Update(double target, double value) { void PID::SetParam (double mi, double ma, double p, double i, double d) { Kp = p; - Ki = i; + Ki = i/1000000.0; Kd = d; Min = mi; Max = ma; @@ -136,7 +136,7 @@ void PID::SetParam (double mi, double ma, double p, double i, double d) { void PID::GetParam (double *mi, double *ma, double *p, double *i, double *d) { if (p != NULL) *p = Kp; - if (i != NULL) *i = Ki; + if (i != NULL) *i = Ki*1000000.0; if (d != NULL) *d = Kd; if (mi != NULL) *mi = Min; if (ma != NULL) *ma = Max; diff --git a/pid.drawio b/pid.drawio index 91f814e..8078e08 100644 --- a/pid.drawio +++ b/pid.drawio @@ -1 +1 @@ -7Vvfc6M2EP5reKwH/UDAY5xc2l6n05vJQ+/upaOAbHTFyCPk2M5fX2HABslOYh+BCe5T0CKtxH6fdrUrx0G3i82vki6TP0XMUge68cZBdw6EwPU9/aeQbEsJwbgUzCWPq04HwQN/ZvXISrriMctbHZUQqeLLtjASWcYi1ZJRKcW63W0m0vasSzpnluAhoqkt/ZvHKimlAfQP8t8Ynyf1zICE5ZsFrTtXX5InNBbrhgh9ctCtFEKVT4vNLUsL49V2Kcfdn3i7X5hkmXrLgB9/xH99/hw++1P/7sb/ffsswuwXWGp5oumq+uBqsWpbW4Bl8U1hSN2KUprnPHLQNFGLVAuAfpRilcWsmMTVrXI4iy2rHpYJ9h+vWcPEgim51V3WB/N6lcmShmVrmWQpVfyprZ5WKM/36vYzfBFcTwzdipGohqPiIw7ctopcrGTEqlFNcxqKfPKKIkXlnClLkX5ofPZBtEPrDOTQUMjpDbj7shfWhkeBsKkIuP0ijC2Ev1kQK7ZRbVBzJcW/7FakQmpJJjLdczrjaWqIaMrnWcEMjS7T8ukTk4prx3dTvVjwOC6mma4TrtjDkkbFnGvt5k9Qp1DANi+TxyZFNQCGbWMHVdxocIYc4QxyT9Ojhce5xvcs438dr/F9fGLLDGV90rVza2LgQBR7LIixBZh+E8BHRMgHCGSeEX8QuTSQGW5uv8Ce3Jw/VCD78MgRM0CF/SIXWshFK/nPUuTj9ZTENUxOBvaUdZ7UAIFueA5GDIFxUoCDQwDO8mCVVWOaJzuzgGOo1LkmtiyorSS3X4vGBHp1+1s1eNe42zS73m2Ph8BZELEoOhYCHwMPex1nBOGQDpcEBmMuzQigmfP17HDBeUlfxbSTgVJ766c9BU/yyn2FVx2yBLiD0oS8chB684kKGIremDhq3Oi20W1ZdMhfWLDpCVGrAqQfSo3dctBOSzvjYK4No+yT3k58z9Naw1sZN9gBz/A3yMT/0gPeviLRNZFMwoZ9EMlO8uh4Ty0WJ4B9aoG9nlo6z7vMHJuyYHb0gEGigD3OPsJG7ipTw8jYYKDng0Ng7zWdJdiIj2a/ITM2Qnu/9Zsl2Nnyd15clo0VAQ+f2DwNBHCfCNQMuJ5Kk3mDEULvMv9lXnZZit7Zf8HzUux+b7vKtXx4jE1FHu4ZY2hhfEX3XR4aOD5Bu7ZwRTde+10zmPnPS6v/v/JyijLepeHMcHUk7NnV2bfLPwl2o3oHEGqVhSeA+K+U8HatL0xy/WHF5uw8QpJR0MbzDEVmOvjetLErJqO/cDOziGDonyZAu25yZRduvjt0rLSrGS+5z/FduA3lSXGI2psRwws9KQ5aikJgKHrvH0/aucZPBuDxFz8vDptm8ZN4/Z62kJ3ZXFnxk5CBPTays5vrKn4GR2Jmr8VPZKcc4lG3Y1rs1AUtFPxgiZpMJvrJQffjxQYbv+HyjhSmgd8NOLp5+A+P0qEd/k8GffoP \ No newline at end of file +3VnLdpswEP0aL9MDiOcytpM06enjNIsk3fQoIBs1MnKF/MrXVxhhg2Rsx6XEZQUaDSMx92oe0AODyfKGwWn8mUaI9CwjWvbAsGdZpuE54pJJVrnEte1cMGY4kkpbwT1+RcWTUjrDEUoripxSwvG0KgxpkqCQV2SQMbqoqo0oqa46hWOkCe5DSHTpA454nEt9y9vKPyI8jouVTTfIZyawUJZvksYwoouSCFz1wIBRyvO7yXKASOa8wi/5c9c1s5uNMZTwYx749Sn6encXvHp9b3jp3a5eaZBcWLmVOSQz+cJys3xVeAAl0WXmSDEKCUxTHPZAP+YTIgSmuGV0lkQoW8QQI31XcqMoqjha7vEG0QnibCUUFlv3OtJlccmzhYwhAjmeV+GBEuXxxtxmhW8Ui51YhmQkKOCQfLR9o2oipTMWIvlU2Z2KIc89YIhDNkZcMyRuSq+9Fa3RegNyoDXk8hfZsxW7mwirhkyjXYRtDeEnDWKOlrwKasoZfUEDSigTkoQmQrM/woQoIkjwOMmYIeBGQt6fI8axCHyXcmKCoyhbpr+IMUf3Uxhmay5EmD+eOplNtNxLCjlrBVVn+zJvlDjj7uAMMOrpUcHjrc53NOc/dtf5nl1zZN7L+27Twa2MQc8CkYP8yNYAEzO+9Qxc9z9IZI6Sf4B7aiJTwtxmgy2FOa+1RNY15Fw1QQXtIhdoyIUz9nNK0+5GStdQXO6+c6Qs+qQSCHCJU7PDECiVgvXuEJgaBPsimPRqBNN47RZzFypFr2lrHhSOY6vHbPDBcorxk3x4PRguy6rDVU0KhMgfhbtSoBv66Hn0Vx1BcFYB1/UVxpzaEVhqz9dywDXf1vRJptUmShGt5xsK1vLKOMCr01lSxK5zoYl7oBA6uqIyFUNHNo4CN7gqqU0zhXTPhtVICCpfgMRNbrFZDuptaWMcTIVjuF7prcXXmBQWahl3JkRylHgDVPxPLfA2XySaJpJK2KANIulNHuxu1aJxwtSrFqvVqqXxvqvBAuNcDnJTnZoNlANmtlw4+PpZE12CjnhnzhtQc6Oln7d2uwS9W/6Bs59lXUXAsWsOTwkB+x8h8BsOvjwi+5lcvZDvw9v5zOk/XLTapp36wVGJfDsweLfy+NDXpqODoXo0G/tzJobb/6m5+vavNLj6Aw== \ No newline at end of file diff --git a/posctl.cc b/posctl.cc index ae72319..1def7ae 100644 --- a/posctl.cc +++ b/posctl.cc @@ -297,8 +297,8 @@ void cb_posctl_entryanglelen (GtkWidget *widget, gpointer data) { }; -#define AXIS_DIFF_MIN -10.0 -#define AXIS_DIFF_MAX 10.0 +#define AXIS_DIFF_MIN -20.0 +#define AXIS_DIFF_MAX 20.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")); @@ -406,13 +406,12 @@ void posctl_gui_update() { char txt[255]; double kp, ki, kd, mi, ma; - printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); - GtkWidget *caliblabel = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_calib_label")); GtkWidget *btnclib = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_calib")); GtkWidget *btnstop = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_stop")); GtkWidget *btncontrol = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_control")); + GtkWidget *btnsimreset = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_resetsim")); GtkWidget *e_cal_rotangle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_rotangle")); GtkWidget *e_cal_rotlen = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_rotlen")); @@ -422,6 +421,8 @@ void posctl_gui_update() { GtkWidget *e_cal_a2angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2angle")); GtkWidget *e_cal_a2len = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2len")); + GtkWidget *lb_earthaxis = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_earthaxis")); + GtkWidget *lb_axisaxis = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_axisaxis")); GtkWidget *e_posdevice = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_device")); GtkWidget *btn_a1min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a1_min")); @@ -431,6 +432,7 @@ void posctl_gui_update() { GtkWidget *btn_a2center = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a2_center")); GtkWidget *btn_a2max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a2_max")); + GtkWidget *a1_min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_min")); GtkWidget *a1_max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_max")); GtkWidget *a1_kp = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_kp")); @@ -493,8 +495,12 @@ void posctl_gui_update() { strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2.l); gtk_entry_set_text (GTK_ENTRY(e_cal_a2len), txt); - gtk_entry_set_text (GTK_ENTRY(e_posdevice), posctl.GetDevice().c_str()); + strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_rot.a - posctl.calib_axis1.a); + gtk_label_set_text (GTK_LABEL(lb_earthaxis), txt); + strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis1.a - posctl.calib_axis2.a); + gtk_label_set_text (GTK_LABEL(lb_axisaxis), txt); + gtk_entry_set_text (GTK_ENTRY(e_posdevice), posctl.GetDevice().c_str()); strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.axis_op[0]); gtk_label_set_text(GTK_LABEL(a1_out), txt); strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.axis_op[1]); @@ -529,6 +535,10 @@ void posctl_gui_update() { strfromd (txt, sizeof(txt-1), (const char *)"%f", kd); gtk_entry_set_text_nofocus (a2_kd, txt); + const gchar *txtptr = gtk_entry_get_text(GTK_ENTRY(e_posdevice)); + if (strncmp(txtptr, "SIMULATION", 11) == 0) gtk_widget_show (btnsimreset); + else gtk_widget_hide (btnsimreset); + posctl.UnLockMutex(); } @@ -752,27 +762,18 @@ void PosCtl::Loop (int posx, int posy) { // dist_axis2 --> will control pis_axis1 else if (mode == POSCTL_MODE_CONTROL) { position_f_2d p; - double d; LockMutex(); // calculate // dist_axis < 0.0 pid should increase the output. // > 0.0 pid should decrease the output - p.x = 0.0; p.y = 0.0; - d = calib_axis1_v.perpendicular(calib_axis2_v, p); - p.x = posx; p.y = posy; axis_pv[0] = calib_axis2_v.perpendicular(p, target_pos); axis_pv[1] = calib_axis1_v.perpendicular(p, target_pos); axis_op[0] = pid_axis[0].Update(0.0, axis_pv[0]); axis_op[1] = pid_axis[1].Update(0.0, axis_pv[1]); - printf ("%s:%d %s", __FILE__, __LINE__, __FUNCTION__); - printf (" d: %+6.3f ", d); - printf (" Axis1 [dist:%+6.3f out1:%+6.3f] ", axis_pv[0], axis_op[0]); - printf (" Axis2 [dist:%+6.3f out2:%+6.3f]\n", axis_pv[1], axis_op[1]); - OutputWriteValue(0, axis_op[0]); OutputWriteValue(1, axis_op[1]); diff --git a/simpleskycam.ui b/simpleskycam.ui index 48c8491..ff70451 100644 --- a/simpleskycam.ui +++ b/simpleskycam.ui @@ -650,6 +650,16 @@ False gtk-execute + + True + False + gtk-dialog-question + + + True + False + gtk-refresh + False 4 @@ -791,7 +801,7 @@ - + True False @@ -946,6 +956,32 @@ 3 + + + True + False + NAN ° + + + 3 + 2 + 2 + + + + + True + False + NAN ° + + + 3 + 1 + + + + + @@ -985,15 +1021,43 @@ - - Start Control + True - True - True - image_control - True - - + False + + + Reset Simulation + True + True + image_question + True + + + + + False + True + 0 + + + + + Start Control + True + True + True + image_control + True + + + + + False + True + end + 1 + + False @@ -1605,7 +1669,7 @@ True False - X + Axis1: 0 @@ -1616,7 +1680,7 @@ True False - Y + Axis2: 0 @@ -1744,11 +1808,6 @@ - - True - False - gtk-refresh - False Histogram diff --git a/videodev-simulation.cc b/videodev-simulation.cc index 8853283..05a7b46 100644 --- a/videodev-simulation.cc +++ b/videodev-simulation.cc @@ -18,6 +18,11 @@ Simulation simulation; gpointer simulation_threadprocess_wrapper (gpointer data); +void cb_posctl_btnsimreset (GtkWidget *widget, gpointer data) { + simulation.Reset(); +} + + VideoDev_Simulation::VideoDev_Simulation() { printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); conf_width = 1920; @@ -180,14 +185,21 @@ gpointer simulation_threadprocess_wrapper (gpointer data) { } Simulation::Simulation() { - int i; - w = 1920; h = 1080; posX = w/2; posY = h/2; running = 0; + Reset(); +}; + + +void Simulation::Reset() { + int i; + + LockMutex(); + time_t t = time(NULL); srandom (t); @@ -195,7 +207,6 @@ Simulation::Simulation() { // object movement dAngle = 360.0 * (double)random() / (double) RAND_MAX; dLen = 1.0 + 5.0 * (double)random() / (double) RAND_MAX; - printf ("%s:%d %s dAngle:%f dLen:%f\n", __FILE__, __LINE__, __FUNCTION__, dAngle, dLen); // @@ -214,8 +225,8 @@ Simulation::Simulation() { printf ("%s:%d %s Axis1: %f° Len:%f Axis2: %f° Len:%f\n", __FILE__, __LINE__, __FUNCTION__, axis[0].defAngle,axis[0].defLen, axis[1].defAngle,axis[1].defLen); - -}; + UnLockMutex(); +} Simulation::~Simulation() { LockMutex (); diff --git a/videodev-simulation.h b/videodev-simulation.h index ff06707..57adf6e 100644 --- a/videodev-simulation.h +++ b/videodev-simulation.h @@ -55,7 +55,7 @@ public: void Start(); void Stop(); - + void Reset(); void AxisSetValue(int a, double v); void AxisStop();