diff --git a/detect.h b/detect.h index aa74e62..bd31abd 100644 --- a/detect.h +++ b/detect.h @@ -59,6 +59,7 @@ class PosCtl { struct timeval calib_timestamp; int calib_mode; position_2d calib_pos; + position_f_2d target_pos; void CalibModeStart(int x, int y); void CalibModeDelta(int x, int y); diff --git a/gui.h b/gui.h index 0c5c2b1..9adddad 100644 --- a/gui.h +++ b/gui.h @@ -48,6 +48,24 @@ public: return res; } + double cosalpha(position_f_2d &pos) { + return (x * pos.x + y * pos.y) / (lenght() * pos.lenght()); + } + + double sinalpha(position_f_2d &pos) { + 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; + l = sinalpha(p) * p.lenght(); + + return l; + } + + double lenght() { return hypot(x, y); } + double x; double y; }; @@ -154,6 +172,8 @@ G_MODULE_EXPORT void cb_histogram_show_window (GtkWidget *widget, gpointer data) G_MODULE_EXPORT void cb_posctl_show (GtkWidget *widget, gpointer data); G_MODULE_EXPORT void cb_posctl_show_window (GtkWidget *widget, gpointer data); G_MODULE_EXPORT void cb_posctl_btncalib (GtkWidget *widget, gpointer data); +G_MODULE_EXPORT void cb_posctl_btnstop (GtkWidget *widget, gpointer data); +G_MODULE_EXPORT void cb_posctl_btncontrol (GtkWidget *widget, gpointer data); G_MODULE_EXPORT void cb_posctl_angles_draw (GtkWidget *area, cairo_t *cr, int w, int h, gpointer data); G_MODULE_EXPORT void cb_posctl_entryanglelen (GtkWidget *widget, gpointer data); G_MODULE_EXPORT void cb_posctl_axis_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer data); diff --git a/pid.drawio b/pid.drawio new file mode 100644 index 0000000..91f814e --- /dev/null +++ b/pid.drawio @@ -0,0 +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 diff --git a/posctl.cc b/posctl.cc index 6823134..917d35a 100644 --- a/posctl.cc +++ b/posctl.cc @@ -55,6 +55,21 @@ void cb_posctl_btncalib (GtkWidget *widget, gpointer data) { } +void cb_posctl_btnstop (GtkWidget *widget, gpointer data) { + printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); + + posctl.Stop(); + posctl_gui_update(); +} + + +void cb_posctl_btncontrol (GtkWidget *widget, gpointer data) { + printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); + + posctl.StartControl(); + posctl_gui_update(); +} + void cb_posctl_show (GtkWidget *widget, gpointer data) { printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); posctl_gui_update(); @@ -299,7 +314,8 @@ void posctl_gui_update() { 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 *cbenable = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_enablectl")); + 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 *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")); @@ -336,13 +352,14 @@ void posctl_gui_update() { if (m == POSCTL_MODE_OFF) { gtk_widget_set_sensitive(btnclib, true); + gtk_widget_set_sensitive(btncontrol, true); + gtk_widget_set_sensitive(btnstop, false); gtk_widget_set_sensitive(btn_a1min, true); gtk_widget_set_sensitive(btn_a1center, true); gtk_widget_set_sensitive(btn_a1max, true); gtk_widget_set_sensitive(btn_a2min, true); gtk_widget_set_sensitive(btn_a2center, true); gtk_widget_set_sensitive(btn_a2max, true); - gtk_widget_set_sensitive(cbenable, true); gtk_widget_set_sensitive(e_posdevice, true); gtk_label_set_label(GTK_LABEL(caliblabel), (char *) ""); } @@ -351,7 +368,8 @@ void posctl_gui_update() { gtk_label_set_label(GTK_LABEL(caliblabel), (char *) "running"); } gtk_widget_set_sensitive(btnclib, false); - gtk_widget_set_sensitive(cbenable, false); + gtk_widget_set_sensitive(btncontrol, false); + gtk_widget_set_sensitive(btnstop, true); gtk_widget_set_sensitive(btn_a1min, false); gtk_widget_set_sensitive(btn_a1center, false); gtk_widget_set_sensitive(btn_a1max, false); @@ -457,6 +475,9 @@ void PosCtl::StartControl() { printf ("%s:%d mode is not off, can't start control.\n", __FILE__, __LINE__); return; } + printf ("%s:%d %s start controlling Target:%f , %f\n", __FILE__, __LINE__, __FUNCTION__, + target_pos.x, target_pos.y); + pid_axis[0].Start(); pid_axis[1].Start(); mode = POSCTL_MODE_CONTROL; @@ -606,6 +627,44 @@ void PosCtl::Loop (int posx, int posy) { gdk_threads_add_idle(cb_thread_posctl, NULL); } } + + // + // dist_axis1+axis2 should contain the lot distance to the + // of the calibrated axis vector with the starting point at target_pos + // if the current position is below the axis line, the distance must be negative. + // + // this distance must be controlled by the PID axis controller. + // dist_axis1 --> will control pid_axis2 + // dist_axis2 --> will control pis_axis1 + else if (mode == POSCTL_MODE_CONTROL) { + double dist_axis1, out1; + double dist_axis2, out2; + position_f_2d p; + + LockMutex(); + + // calculate + // dist_axis < 0.0 pid should increase the output. + // > 0.0 pid should decrease the output + p.x = posx; p.y = 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_axis2); + out2 = pid_axis[1].Update(0.0, dist_axis1); + + OutputWriteValue(0, out1); + OutputWriteValue(1, out2); + + UnLockMutex(); + } + + else { + LockMutex(); + target_pos.x = posx; + target_pos.y = posy; + UnLockMutex(); + } }; @@ -698,6 +757,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; diff --git a/simpleskycam.ui b/simpleskycam.ui index e348e28..186ed4b 100644 --- a/simpleskycam.ui +++ b/simpleskycam.ui @@ -645,10 +645,10 @@ - + True False - gtk-refresh + gtk-execute False @@ -684,15 +684,44 @@ - - Calibration + True - True - True - image_refresh - True - - + False + + + gtk-stop + True + True + True + True + True + + + + + False + True + 0 + + + + + Calibration + True + True + True + image_refresh + True + + + + + False + True + end + 1 + + False @@ -956,12 +985,15 @@ - - Enabled + + Start Control True True - False - True + True + image_control + True + + False @@ -1712,6 +1744,11 @@ + + True + False + gtk-refresh + False Histogram diff --git a/video.cc b/video.cc index 634610c..f559163 100644 --- a/video.cc +++ b/video.cc @@ -601,6 +601,9 @@ void cb_vidctrl_entry_change (GtkWidget *widget, gpointer data) { }; +/* + * + */ void cb_videoda_motionevent (GtkWidget *widget, GdkEvent *event, gpointer data) { int dw, dh, sw, sh; float sar, dar;