From 68f68489f7cc36b7488fbc4bc3805d55da1ae75b Mon Sep 17 00:00:00 2001 From: Steffen Pohle Date: Wed, 18 Jan 2023 23:46:01 +0100 Subject: [PATCH] posctl ... gui basic implemention. Thread seperated and seem to work. only gui testing functionality now --- detect.cc | 8 ++-- detect.h | 32 ++++++++++++- gui.cc | 3 +- gui.h | 1 + main.cc | 1 + pid.cc | 52 +++++++++++++++++---- pid.h | 3 ++ posctl.cc | 134 +++++++++++++++++++++++++++++++++++++++++++++++------- 8 files changed, 203 insertions(+), 31 deletions(-) diff --git a/detect.cc b/detect.cc index 9b8c622..3df6d93 100644 --- a/detect.cc +++ b/detect.cc @@ -10,6 +10,7 @@ #include "detect.h" extern Detect detect; +extern PosCtl posctl; // // C / C++ Wrapper for the thread function @@ -120,7 +121,6 @@ void Detect::Thread() { break; } - printf ("%s:%d delta: %d , %d\n", __FILE__, __LINE__, oldX - objectX, oldY - objectY); if (abs(oldX - objectX) >= DET_MAXSHIFT/5 || abs(oldY - objectY) >= DET_MAXSHIFT/5) { objectX = -1; objectY = -1; @@ -141,14 +141,16 @@ void Detect::Thread() { if (detmatrix != NULL) memcpy (output.detmatrix, detmatrix, sizeof(uint32_t) * DET_MAXSHIFT * DET_MAXSHIFT); - UnLockMutex(); // unlock Config + posctl.Loop (objectX, objectY); + + UnLockMutex(); // unlock detect dataset gdk_threads_add_idle(cb_thread_detect , &output); } else UnLockInMutex(); - usleep (10000); + usleep (10000); // FIXME:: why is it here? } free (output.detmatrix); } diff --git a/detect.h b/detect.h index ee90f8f..2a538c7 100644 --- a/detect.h +++ b/detect.h @@ -8,6 +8,7 @@ #ifndef _DETECT_H_ #define _DETECT_H_ +#include "pid.h" #include "gui.h" #include "config.h" #include "video.h" @@ -26,6 +27,36 @@ enum { AUTOFOLLOW_CROSSC }; +enum { + POSCTL_MODE_OFF = 0, + POSCTL_MODE_CALIB, + POSCTL_MODE_CONTROL +}; + + +class PosCtl { + private: + GMutex mutex; + int mode; + PID pid_axis[2]; + + time_t test_t1; // FIXME: only for testing gui functions + public: + PosCtl(); + ~PosCtl() {}; + + void LockMutex() { g_mutex_lock(&mutex); }; + void UnLockMutex() { g_mutex_unlock(&mutex); }; + + void Stop (); + void StartCalibration (); + void StartControl (); + int GetMode () { return mode; }; + void SetAxisParam (int axis, double min, double max, double k, double i, double d); + void GetAxisParam (int axis, double *min, double *max, double *k, double *i, double *d); + void Loop (int posx, int posy); +}; + struct { VideoFrame *image; // detected image @@ -35,7 +66,6 @@ struct { } typedef DetectOutput; - class Detect { private: int running; diff --git a/gui.cc b/gui.cc index 5da4b84..2f9b122 100644 --- a/gui.cc +++ b/gui.cc @@ -105,7 +105,7 @@ void displayerror (std::string error) { }; -// +//data // callback from filter thread. Data will point to the output VideoFrame. // Access to this data must be Locked before use. gboolean cb_thread_filter (gpointer data) { @@ -139,7 +139,6 @@ gboolean cb_thread_filter (gpointer data) { gdk_window_invalidate_rect(gtk_widget_get_window(image_da), NULL, true); filter.UnLockImageMutex(); - return false; }; diff --git a/gui.h b/gui.h index 790f007..a4bcfbb 100644 --- a/gui.h +++ b/gui.h @@ -98,6 +98,7 @@ G_MODULE_EXPORT void cb_input_btnscale (GtkWidget *widget, gpointer data); G_MODULE_EXPORT void cb_imagetempda_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer data); G_MODULE_EXPORT gboolean cb_thread_filter (gpointer data); G_MODULE_EXPORT gboolean cb_thread_detect (gpointer data); +G_MODULE_EXPORT gboolean cb_thread_posctl (gpointer data); G_MODULE_EXPORT void cb_image_btnnew (GtkWidget *widget, gpointer data); G_MODULE_EXPORT void cb_image_btnsave (GtkWidget *widget, gpointer data); G_MODULE_EXPORT void cb_output_show_window (GtkWidget *widget, gpointer data); diff --git a/main.cc b/main.cc index dc5c529..ed02500 100644 --- a/main.cc +++ b/main.cc @@ -25,6 +25,7 @@ GtkBuilder *_builder_ = NULL; // work around for the thread situation Filter filter; Detect detect; Configuration config; +PosCtl posctl; int main (int argc, char **argv) { GtkBuilder *builder; diff --git a/pid.cc b/pid.cc index 775169d..ae9bf09 100644 --- a/pid.cc +++ b/pid.cc @@ -15,14 +15,28 @@ parameters must be adjusted by experiments. */ PID::PID(double min, double max, double kp, double kd, double ki) { - Min = min; - Max = max; - Kp = kp; - Kd = kd; - Ki = ki; - Integral = 0; - PreviousError = 0; - PreviousTime = GetTime(); + Min = min; + Max = max; + Kp = kp; + Kd = kd; + Ki = ki; + Integral = 0; + PreviousError = 0; + PreviousTime = GetTime(); +} + +/* + * default parameter + */ +PID::PID() { + Min = 0.0; + Max = 1.0; + Kp = 1.0; + Kd = 0.0; + Ki = 0.0; + Integral = 0; + PreviousError = 0; + PreviousTime = GetTime(); } /* @@ -108,3 +122,25 @@ double PID::Update(double target, double value, int64_t time) { double PID::Update(double target, double value) { return Update(target, value, GetTime()); } + + +void PID::SetParam (double mi, double ma, double p, double i, double d) { + Kp = p; + Ki = i; + Kd = d; + Min = mi; + Max = ma; +}; + + +void PID::GetParam (double *mi, double *ma, double *p, double *i, double *d) { + *p = Kp; + *i = Ki; + *d = Kd; + *mi = Min; + *ma = Max; +}; + + + + diff --git a/pid.h b/pid.h index d31160f..b589b44 100644 --- a/pid.h +++ b/pid.h @@ -14,9 +14,12 @@ class PID { public: PID(double min, double max, double kp, double kd, double ki); + PID(); ~PID(); void Start(void); + void SetParam(double mi, double ma, double p, double i, double d); + void GetParam(double *mi, double *ma, double *p, double *i, double *d); double Update(double target, double value, int64_t time); double Update(double target, double value); static int64_t GetTime(void); diff --git a/posctl.cc b/posctl.cc index d413f00..0d55190 100644 --- a/posctl.cc +++ b/posctl.cc @@ -11,6 +11,7 @@ #include #include #include "gui.h" +#include "pid.h" #include "filter.h" #include "detect.h" #include "configuration.h" @@ -18,12 +19,17 @@ #include "videodev.h" #include "histogram.h" +extern PosCtl posctl; + extern GtkBuilder *_builder_; // work around for threads GtkWidget *posctl_rot_da = NULL; // GtkWidget *posctl_axis1_da = NULL; // GtkWidget *posctl_axis1_da = NULL; double linedash1[] = { 1.0, 4.0 }; + +void posctl_gui_update(); + void cb_posctl_show_window (GtkWidget *widget, gpointer data) { printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); @@ -35,23 +41,12 @@ void cb_posctl_show_window (GtkWidget *widget, gpointer data) { void cb_posctl_btncalib (GtkWidget *widget, gpointer data) { printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); - GtkWidget *btn = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_calib")); - gtk_widget_set_sensitive(btn, false); - - printf ("%s:%d %s disable all axis motors\n", __FILE__, __LINE__, __FUNCTION__); - printf ("%s:%d %s wait 5 or 10 seconds\n", __FILE__, __LINE__, __FUNCTION__); - printf ("%s:%d %s set minimum axis 1\n", __FILE__, __LINE__, __FUNCTION__); - printf ("%s:%d %s wait 5 or 10 seconds\n", __FILE__, __LINE__, __FUNCTION__); - printf ("%s:%d %s set maximum axis 1\n", __FILE__, __LINE__, __FUNCTION__); - printf ("%s:%d %s wait 5 or 10 seconds\n", __FILE__, __LINE__, __FUNCTION__); - printf ("%s:%d %s set minimum axis 2\n", __FILE__, __LINE__, __FUNCTION__); - printf ("%s:%d %s wait 5 or 10 seconds\n", __FILE__, __LINE__, __FUNCTION__); - printf ("%s:%d %s set maximum axis 2\n", __FILE__, __LINE__, __FUNCTION__); - printf ("%s:%d %s wait 5 or 10 seconds\n", __FILE__, __LINE__, __FUNCTION__); - printf ("%s:%d %s calculation rotation angle for each axis\n", __FILE__, __LINE__, __FUNCTION__); - printf ("%s:%d %s set axis in config file and use it for corrections\n", __FILE__, __LINE__, __FUNCTION__); -} +// GtkWidget *btn = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_calib")); +// gtk_widget_set_sensitive(btn, false); + posctl.StartCalibration(); + posctl_gui_update(); +} void cb_posctl_angles_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer data) { int clientw, clienth; @@ -171,7 +166,6 @@ void cb_posctl_angles_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer cairo_move_to(cr, center.x + float (lpos[4].x * center.x * 0.9 / lmax), center.y - float (lpos[4].y * center.y * 0.9 / lmax)); cairo_show_text(cr, (char *)"2"); - }; @@ -218,3 +212,109 @@ void cb_posctl_axis_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer da } }; + +/* + * posctl gui update + */ +void posctl_gui_update() { + 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")); + + posctl.LockMutex(); + int m = posctl.GetMode(); + + if (m == POSCTL_MODE_OFF) { + gtk_widget_set_sensitive(btnclib, true); + gtk_widget_set_sensitive(cbenable, true); + } + else { + gtk_widget_set_sensitive(btnclib, false); + gtk_widget_set_sensitive(cbenable, false); + } + + posctl.UnLockMutex(); +} + + +/* + * callback from the detect thread. + * the gtk/gui updates must and will be processed in a separate gui thread + */ +gboolean cb_thread_posctl (gpointer data) { + posctl_gui_update(); + return false; +}; + + +PosCtl::PosCtl() { + mode = POSCTL_MODE_OFF; + test_t1 = 0; +}; + + +/* + * stop the control or the calibration + */ +void PosCtl::Stop() { + printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); + mode = POSCTL_MODE_OFF; + gdk_threads_add_idle(cb_thread_posctl, NULL); +} + + +void PosCtl::StartCalibration() { + printf ("%s:%d\n", __FILE__, __LINE__); + if (mode != POSCTL_MODE_OFF) { + printf ("%s:%d mode is not off, can't start calibration.\n", __FILE__, __LINE__); + return; + } + mode = POSCTL_MODE_CALIB; + test_t1 = time (NULL); + gdk_threads_add_idle(cb_thread_posctl, NULL); +} + + +void PosCtl::StartControl() { + if (mode != POSCTL_MODE_OFF) { + printf ("%s:%d mode is not off, can't start control.\n", __FILE__, __LINE__); + return; + } + pid_axis[0].Start(); + pid_axis[1].Start(); + mode = POSCTL_MODE_CONTROL; + gdk_threads_add_idle(cb_thread_posctl, NULL); +} + + +/* + * get and set PID parameter, no mutex lock + * the access to the data should be already locked by the curren tthread + */ +void PosCtl::SetAxisParam ( int axis, double min, double max, + double k, double i, double d) { + + if (axis < 0 || axis > 1) return; + pid_axis[axis].SetParam(min, max, k, i, d); +}; + +void PosCtl::GetAxisParam ( int axis, double *min, double *max, + double *k, double *i, double *d) { + + if (axis < 0 || axis > 1) return; + pid_axis[axis].GetParam(min, max, k, i, d); +}; + + +/* + * Loop, if new data is aviable + */ +void PosCtl::Loop (int posx, int posy) { + time_t t = time(NULL); + + // FIXME:: only for testing gui function + if ((t - test_t1) > 5) { + mode = POSCTL_MODE_OFF; + gdk_threads_add_idle(cb_thread_posctl, NULL); + } +}; +