diff --git a/Makefile.config b/Makefile.config index 6cfa34e..f5defc9 100644 --- a/Makefile.config +++ b/Makefile.config @@ -7,6 +7,6 @@ USE_SVBONY = 1 USE_DNG = 1 USE_SER = 1 -DEBUG_ANGLES = 1 -DEBUG_POSCTL = 1 +DEBUG_ANGLES = 0 +DEBUG_POSCTL = 0 diff --git a/Makefile.rules.crosswindows b/Makefile.rules.crosswindows index 9e52897..fd0884c 100644 --- a/Makefile.rules.crosswindows +++ b/Makefile.rules.crosswindows @@ -1,13 +1,32 @@ include Makefile.config +USE_VFW = 1 TARGET = $(APP).exe CROSSENV = /opt/W64-cross-compile/ CPP = /usr/bin/x86_64-w64-mingw32-g++ -CPPFLAGS = -ggdb -Wall -O0 `PKG_CONFIG_PATH=$(CROSSENV)/lib/pkgconfig pkg-config --cflags gtk+-3.0 gmodule-export-2.0` -Wl,--export-dynamic -DBUILD_WINDOWS=1 -Wdeprecated +CPPFLAGS = -ggdb -Wall -O0 `PKG_CONFIG_PATH=$(CROSSENV)/lib/pkgconfig pkg-config --cflags gtk+-3.0 gmodule-export-2.0` -Wl,--export-dynamic -DBUILD_WINDOWS=1 -Wdeprecated -D_POSIX_C_SOURCE=200112L INCLUDES = LDFLAGS = -lws2_32 -ljpeg LIBS = `PKG_CONFIG_PATH=$(CROSSENV)/lib/pkgconfig pkg-config --libs gtk+-3.0 gmodule-export-2.0` -L/usr/lib -mwindows OBJECTS := $(OBJECTS) windows.oo +# +# add makefile configuration for svbony cams +# +ifeq ($(USE_SVBONY),1) +CPPFLAGS := $(CPPFLAGS) +LDFLAGS := $(LDFLAGS) -lSVBCameraSDK +OBJECTS := $(OBJECTS) videodev-svbcam.oo +endif +# + +# +# add makefile configuration for vfw cams +# +ifeq ($(USE_VFW),1) +OBJECTS := $(OBJECTS) videodev-vfw.oo +LDFLAGS := $(LDFLAGS) -lvfw32 +endif +# diff --git a/debug-angles.cc b/debug-angles.cc index 8d855ef..dc9184f 100644 --- a/debug-angles.cc +++ b/debug-angles.cc @@ -34,7 +34,7 @@ void debug_angles_draw(cairo_t *cr) { int centerX = DBGA_W >> 1; int centerY = DBGA_H >> 1; position_f_2d v; - double d; + double d, a; debug_angles_calculate(); @@ -71,13 +71,23 @@ void debug_angles_draw(cairo_t *cr) { cairo_stroke(cr); // draw axis + a = 180.0 * atan2(dbga_axis.x, dbga_axis.y) / M_PI; cairo_set_source_rgb(cr, 0.5, 0.5, 0.5); draw_printf(cr, 0, 25, 0.0, (char*)"Axis: %5.1f,%5.1f", dbga_axis.x, dbga_axis.y); + cairo_set_source_rgb(cr, 0.5, 0.5, 0.5); + draw_printf(cr, 0, 40, 0.0, (char*)"Axis: %5.1f", a); cairo_set_line_width(cr, 3.0); cairo_move_to(cr, centerX, centerY); cairo_line_to(cr, centerX + dbga_axis.x, centerY + dbga_axis.y); cairo_stroke(cr); + cairo_set_source_rgb(cr, 1.0, 1.0, 1.0); + cairo_set_line_width(cr, 1.0); + cairo_move_to(cr, centerX, centerY); + cairo_line_to(cr, centerX + sindeg(a) * (double)centerX * 0.8, centerY + cosdeg(a) * (double)centerX * 0.8); + cairo_stroke(cr); + + cairo_set_line_width(cr, 1.0); d = hypot(dbga_axis.x, dbga_axis.y); v.x = centerX * dbga_axis.x / d; diff --git a/debug.cc b/debug.cc index e4ace42..3aca478 100644 --- a/debug.cc +++ b/debug.cc @@ -3,48 +3,54 @@ #include #include #include - +#include +#include #include #include "debug.h" +int debug_uninit = 1; GMutex debug_mutex; - +time_t debug_time; #define LEN 2048 void debug_init() { + debug_uninit = 0; g_mutex_init (&debug_mutex); + debug_time = time(NULL); } -void debug_tofile(char *fname, char *fmt, ...) { - static struct timeval tv; - static int firstrun = 1; - struct timeval tv1; +void debug_tofile(char *fname, int isheader, char *fmt, ...) { + struct timeval tv; va_list args; char buffer[LEN]; int fd, len; - long long int ms; + std::string fn; + + if (debug_uninit) debug_init(); + g_mutex_lock(&debug_mutex); + // locale set to C, floating points decimals are . std::string s = setlocale(LC_ALL, NULL); setlocale (LC_ALL, "C"); - g_mutex_lock(&debug_mutex); + // append time to filename to the start of the filename + struct tm *timetm = localtime(&debug_time); + char timestr[64] = ""; + strftime(timestr, 63, "%F-%H%M%S", timetm); + fn = "debug-" + (std::string)timestr + "-" + (std::string)fname; + + gettimeofday(&tv,NULL); - if (firstrun) { - gettimeofday(&tv,NULL); - firstrun = 0; - } - gettimeofday(&tv1,NULL); - ms = ((tv1.tv_sec - tv.tv_sec) * 1000) + - ((tv1.tv_usec - tv.tv_usec) / 1000); #ifdef BUILD_WINDOWS - fd = open(fname, O_WRONLY | O_APPEND | O_CREAT); + fd = open(fn.c_str(), O_WRONLY | O_APPEND | O_CREAT); #else - fd = open(fname, O_WRONLY | O_APPEND | O_CREAT, S_IRUSR | S_IWUSR); + fd = open(fn.c_str(), O_WRONLY | O_APPEND | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH); #endif if (fd < 0) errorexit((char*)"%s:%d could not open debug '%s' file. Error:%s\n", __FILE__, __LINE__, fname, strerror(errno)); - snprintf (buffer, 32, "%lld,", ms); + if (isheader) snprintf (buffer, 32, "time,"); + else snprintf (buffer, 32, "%lld,", (long long int) (tv.tv_sec)*1000 + (long long int) tv.tv_usec/1000); len = strlen (buffer); va_start (args, fmt); vsnprintf (buffer+len, LEN-1-len, fmt, args); @@ -52,7 +58,9 @@ void debug_tofile(char *fname, char *fmt, ...) { buffer[LEN-1] = 0; write (fd, buffer, strlen(buffer)); close (fd); - g_mutex_unlock(&debug_mutex); + // reset locale to user setting setlocale (LC_ALL, s.c_str()); + + g_mutex_unlock(&debug_mutex); } diff --git a/debug.h b/debug.h index 07b82c3..88ecfbd 100644 --- a/debug.h +++ b/debug.h @@ -18,6 +18,6 @@ extern void debug_angles_btnpress(GdkEvent *event); #endif extern void debug_init(); -extern void debug_tofile(char *fname, char *fmt, ...); +extern void debug_tofile(char *fname, int isheader, char *fmt, ...); #endif diff --git a/detect.h b/detect.h index ac4c43b..1d2090d 100644 --- a/detect.h +++ b/detect.h @@ -8,6 +8,25 @@ #ifndef _DETECT_H_ #define _DETECT_H_ +#ifdef BUILD_WINDOWS +#define WIN32_LEAN_AND_MEAN + +#define _NTDDI_VERSION_FROM_WIN32_WINNT2(ver) ver##0000 +#define _NTDDI_VERSION_FROM_WIN32_WINNT(ver) _NTDDI_VERSION_FROM_WIN32_WINNT2(ver) + +#ifndef _WIN32_WINNT +# define _WIN32_WINNT 0x501 +#endif +#ifndef NTDDI_VERSION +# define NTDDI_VERSION _NTDDI_VERSION_FROM_WIN32_WINNT(_WIN32_WINNT) +#endif + +#include +#include +#include +#include +#endif + #include "pid.h" #include "gui.h" #include "config.h" @@ -84,6 +103,7 @@ class PosCtl { position_f_2d calib_axis2_v; vector_2d calib_axis2; position_f_2d target_pos; + position_f_2d current_pos; double axis_pv[2]; double axis_op[2]; diff --git a/gui.h b/gui.h index b2938dd..d46d4d7 100644 --- a/gui.h +++ b/gui.h @@ -23,10 +23,28 @@ enum { #define BUILDER_FILE "simpleskycam.ui" -struct { +class position_2d { +public: + position_2d() { x = 0; y = 0; }; + ~position_2d() {}; + + position_2d operator + (position_2d const &obj) { + position_2d res; + res.x = x + obj.x; + res.y = y + obj.y; + return res; + } + + position_2d operator - (position_2d const &obj) { + position_2d res; + res.x = x - obj.x; + res.y = y - obj.y; + return res; + } + int x; int y; -} typedef position_2d; +}; class position_f_2d { @@ -175,13 +193,13 @@ 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); 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); +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_axis_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer data); +G_MODULE_EXPORT void cb_posctl_track_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer data); // // menu elements diff --git a/posctl.cc b/posctl.cc index 997f40e..da8756a 100644 --- a/posctl.cc +++ b/posctl.cc @@ -174,12 +174,16 @@ void cb_posctl_change_entry (GtkWidget *widget, gpointer data) { } +/* + * draw position relative to target position. + * draw the axis vectors form the center of the screen. + * draw the earch movement to the center of the screen. + */ void cb_posctl_angles_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer data) { int clientw, clienth; - - position_f_2d lpos[5]; - float lmax; + position_f_2d lpos, p; position_2d center; + double pscale = 2.0; // // rotation da @@ -199,10 +203,6 @@ 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; @@ -228,69 +228,83 @@ void cb_posctl_angles_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer cairo_stroke(cr); cairo_set_dash(cr, NULL, 0, 0); - // - // calculate angles - lpos[0].x = sin(rotangle) * rotlen; - lpos[0].y = cos(rotangle) * rotlen; - - lpos[1].x = lpos[0].x + sin(a1_angle) * a1_len; - lpos[1].y = lpos[0].y + cos(a1_angle) * a1_len; - lpos[2].x = lpos[0].x; - lpos[2].y = lpos[0].y; - - lpos[3].x = lpos[0].x + sin(a2_angle) * a2_len; - lpos[3].y = lpos[0].y + cos(a2_angle) * a2_len; - lpos[4].x = lpos[0].x; - lpos[4].y = lpos[0].y; - - // find maximum - for (int i = 0; i < 5; i++) { - if (i == 0) { - lmax = fabs (lpos[0].x); - if (fabs(lpos[i].y) > lmax) lmax = fabs(lpos[i].y); - } else { - if (fabs(lpos[i].x) > lmax) lmax = fabs(lpos[i].x); - if (fabs(lpos[i].y) > lmax) lmax = fabs(lpos[i].y); - } - } - // // draw cairo_select_font_face (cr, "sans-serif", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_NORMAL); cairo_set_font_size (cr, 11); - cairo_set_line_width(cr, 4.0); // rot + lpos.x = -sin(rotangle) * rotlen * 4.0; + lpos.y = -cos(rotangle) * rotlen * 4.0; cairo_set_source_rgb(cr, 0.0, 0.0, 0.0); + cairo_set_line_width(cr, 4.0); cairo_move_to(cr, center.x, center.y); - cairo_line_to(cr, center.x + float (lpos[0].x * center.x * 0.8 / lmax), - center.y - float (lpos[0].y * center.y * 0.8 / lmax)); + cairo_line_to(cr, center.x + lpos.x, center.y + lpos.y); + cairo_stroke(cr); + cairo_set_line_width(cr, 0.5); + cairo_move_to(cr, center.x - lpos.x * 100.0, center.y - lpos.y * 100.0); + cairo_line_to(cr, center.x + lpos.x * 100.0, center.y + lpos.y * 100.0); cairo_stroke(cr); - cairo_move_to(cr, center.x + float (lpos[0].x * center.x * 0.9 / lmax), - center.y - float (lpos[0].y * center.y * 0.9 / lmax)); - cairo_show_text(cr, (char *)"E"); // axis1 - cairo_set_source_rgb(cr, 0.0, 1.0, 0.5); - cairo_move_to(cr, center.x + float (lpos[1].x * center.x * 0.8 / lmax), - center.y - float (lpos[1].y * center.y * 0.8 / lmax)); - cairo_line_to(cr, center.x + float (lpos[2].x * center.x * 0.8 / lmax), - center.y - float (lpos[2].y * center.y * 0.8 / lmax)); + lpos.x = sin(a1_angle) * a1_len * 4.0; + lpos.y = cos(a1_angle) * a1_len * 4.0; + cairo_set_source_rgb(cr, 0.0, 0.75, 0.3); + cairo_set_line_width(cr, 4.0); + cairo_move_to(cr, center.x, center.y); + cairo_line_to(cr, center.x + lpos.x, center.y + lpos.y); + cairo_stroke(cr); + cairo_set_line_width(cr, 0.5); + cairo_move_to(cr, center.x - lpos.x * 100.0, center.y - lpos.y * 100.0); + cairo_line_to(cr, center.x + lpos.x * 100.0, center.y + lpos.y * 100.0); cairo_stroke(cr); - cairo_move_to(cr, center.x + float (lpos[1].x * center.x * 0.9 / lmax), - center.y - float (lpos[1].y * center.y * 0.9 / lmax)); - cairo_show_text(cr, (char *)"1"); // axis2 - cairo_set_source_rgb(cr, 0.0, 0.5, 1.0); - cairo_move_to(cr, center.x + float (lpos[3].x * center.x * 0.8 / lmax), - center.y - float (lpos[3].y * center.y * 0.8 / lmax)); - cairo_line_to(cr, center.x + float (lpos[4].x * center.x * 0.8 / lmax), - center.y - float (lpos[4].y * center.y * 0.8 / lmax)); + lpos.x = sin(a2_angle) * a2_len * 4.0; + lpos.y = cos(a2_angle) * a2_len * 4.0; + cairo_set_source_rgb(cr, 0.0, 0.3, 0.75); + cairo_set_line_width(cr, 4.0); + cairo_move_to(cr, center.x, center.y); + cairo_line_to(cr, center.x + lpos.x, center.y + lpos.y); + cairo_stroke(cr); + cairo_set_line_width(cr, 0.5); + cairo_move_to(cr, center.x - lpos.x * 100.0, center.y - lpos.y * 100.0); + cairo_line_to(cr, center.x + lpos.x * 100.0, center.y + lpos.y * 100.0); + cairo_stroke(cr); + + // + // draw position relative to the target + p = posctl.current_pos - posctl.target_pos; + p.x *= pscale; + p.y *= pscale; + cairo_set_source_rgb(cr, 0.8, 0.1, 0.1); + cairo_set_line_width(cr, 1.0); + cairo_move_to(cr, center.x-4 + p.x, center.y + p.y); + cairo_line_to(cr, center.x+4 + p.x, center.y + p.y); + cairo_stroke(cr); + cairo_move_to(cr, center.x + p.x, center.y-4 + p.y); + cairo_line_to(cr, center.x + p.x, center.y+4 + p.y); + cairo_stroke(cr); + cairo_move_to(cr, center.x+4 + p.x, center.y-4 + p.y); + cairo_line_to(cr, center.x-4 + p.x, center.y+4 + p.y); + cairo_stroke(cr); + cairo_move_to(cr, center.x+4 + p.x, center.y+4 + p.y); + cairo_line_to(cr, center.x-4 + p.x, center.y-4 + p.y); + cairo_stroke(cr); + + // draw distance to axis + lpos.x = sindeg(posctl.calib_axis1.a) * posctl.axis_pv[0] * pscale; + lpos.y = cosdeg(posctl.calib_axis1.a) * posctl.axis_pv[0] * pscale; + cairo_move_to(cr, center.x + p.x, center.y + p.y); + cairo_line_to(cr, center.x + p.x + lpos.x, center.y + p.y + lpos.y); + cairo_stroke(cr); + + // draw distance to axis + lpos.x = sindeg(posctl.calib_axis2.a) * posctl.axis_pv[1] * pscale; + lpos.y = cosdeg(posctl.calib_axis2.a) * posctl.axis_pv[1] * pscale; + cairo_move_to(cr, center.x + p.x, center.y + p.y); + cairo_line_to(cr, center.x + p.x + lpos.x, center.y + p.y + lpos.y); cairo_stroke(cr); - cairo_move_to(cr, center.x + float (lpos[3].x * center.x * 0.9 / lmax), - center.y - float (lpos[3].y * center.y * 0.9 / lmax)); - cairo_show_text(cr, (char *)"2"); }; @@ -307,6 +321,7 @@ void cb_posctl_entryanglelen (GtkWidget *widget, gpointer data) { 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; @@ -346,7 +361,7 @@ void cb_posctl_axis_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer da 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_move_to(cr, 10, 10); cairo_show_text(cr, (char *)"diff"); // draw elements diff i = axis_history_pos[axis]; @@ -370,10 +385,11 @@ void cb_posctl_axis_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer da } while (i != axis_history_pos[axis]); cairo_stroke(cr); - cairo_set_source_rgb(cr, 0.0, 1.0, 0.5); + if (axis == 0) cairo_set_source_rgb(cr, 0.0, 0.75, 0.3); + else cairo_set_source_rgb(cr, 0.0, 0.3, 0.75); 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_move_to(cr, 60, 10); cairo_show_text(cr, (char *)"out"); // draw elements diff i = axis_history_pos[axis]; @@ -396,10 +412,6 @@ void cb_posctl_axis_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer da if (i < 0) i = AXIS_HISTORY_CNT-1; } while (i != axis_history_pos[axis]); cairo_stroke(cr); - - - // draw chart - }; @@ -559,8 +571,11 @@ gboolean cb_thread_posctl (gpointer data) { 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); + + 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")); + gdk_window_invalidate_rect(gtk_widget_get_window(posctl_rot_da), NULL, true); + return false; }; @@ -570,8 +585,7 @@ PosCtl::PosCtl() { mode = POSCTL_MODE_OFF; #ifdef DEBUG_POSCTL - debug_tofile((char*)"posctl.log", (char*)"\n"); - debug_tofile((char*)"posctl.log", (char*)"mode,posX,posY,targetX,targetY,dx,dy,axis1.pv,axis1.op,axis1.kp,axis1.ki,axis1.kd,axis2.pv,axis2.op,axis2.kp,axis2.ki,axis2.kd\n"); + debug_tofile((char*)"posctl.log", 1, (char*)"mode , calEV,calEL , posX,posY,targetX,targetY,dx,dy , cal1V,cal1L , axis1.pv,axis1.op,axis1.kp,axis1.ki,axis1.kd , cal2V,cal2L , axis2.pv,axis2.op,axis2.kp,axis2.ki,axis2.kd\n"); #endif calib_mode = POSCTL_CALIB_MODE_OFF; @@ -673,7 +687,7 @@ void PosCtl::CalibModeDelta(int x, int y) { pid_axis[0].GetParam(&a1min, &a1max, NULL, NULL, NULL); fp.x = (x - calib_pos.x) / (float)timediff; - fp.y = -(y - calib_pos.y) / (float)timediff; + fp.y = (y - calib_pos.y) / (float)timediff; LockMutex(); calib_rot_v = fp; @@ -704,7 +718,7 @@ void PosCtl::CalibModeAxis(int x, int y) { printf ("%s:%d %s calib_mode: %d\n", __FILE__, __LINE__, __FUNCTION__, calib_mode); fp.x = +(x - calib_pos.x) / timediff; - fp.y = -(y - calib_pos.y) / timediff; + fp.y = +(y - calib_pos.y) / timediff; if (calib_mode == POSCTL_CALIB_MODE_AXIS1) { calib_axis1_v = fp - calib_rot_v; @@ -745,7 +759,7 @@ void PosCtl::Loop (int posx, int posy) { static int lastmode = -1; double kp0,ki0,kd0, kp1,ki1,kd1; if (lastmode != mode && mode != POSCTL_MODE_CONTROL) { - debug_tofile((char*)"posctl.log", (char*)"%d,,,,,,,,,,,,,,,,\n", mode); + debug_tofile((char*)"posctl.log", 0, (char*)"%d,,,,,,,,,,,,,,,,,,,,,,\n", mode); } #endif // @@ -781,13 +795,14 @@ void PosCtl::Loop (int posx, int posy) { // dist_axis1 --> will control pid_axis2 // dist_axis2 --> will control pis_axis1 else if (mode == POSCTL_MODE_CONTROL) { - position_f_2d p; + position_f_2d p, target_p; LockMutex(); // calculate // dist_axis < 0.0 pid should increase the output. // > 0.0 pid should decrease the output + target_p.x = target_pos.x; target_p.y = target_pos.y; 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); @@ -797,13 +812,14 @@ void PosCtl::Loop (int posx, int posy) { #ifdef DEBUG_POSCTL pid_axis[0].GetParam(NULL, NULL, &kp0, &ki0, &kd0); pid_axis[1].GetParam(NULL, NULL, &kp1, &ki1, &kd1); - debug_tofile((char*)"posctl.log", (char*)"%d,%d,%d,%d,%d,%d,%d,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n", - mode, posx, posy, target_pos.x, target_pos.y, posx - target_pos.x, posy - target_pos.y, - axis_pv[0], axis_op[0],kp0,ki0,kd0, - axis_pv[1], axis_op[1],kp1,ki1,kd1); + debug_tofile((char*)"posctl.log", 0, (char*)"%d , %g,%g , %d,%d,%g,%g,%g,%g , %g,%g , %g,%g,%g,%g,%g , %g,%g , %g,%g,%g,%g,%g\n", + mode, calib_rot.a, calib_rot.l, + posx, posy, target_pos.x, target_pos.y, posx - target_pos.x, posy - target_pos.y, + calib_axis1.a, calib_axis1.l, axis_pv[0], axis_op[0],kp0,ki0,kd0, + calib_axis2.a, calib_axis2.l, axis_pv[1], axis_op[1],kp1,ki1,kd1); #endif - if (OutputWriteValue(0, axis_op[0]) || OutputWriteValue(1, axis_op[1])) + if (OutputWriteValue(0, axis_op[0]) || OutputWriteValue(1, axis_op[1])) mode = POSCTL_MODE_OFF; axis_history_add(0, axis_pv[0], axis_op[0]); @@ -820,6 +836,8 @@ void PosCtl::Loop (int posx, int posy) { UnLockMutex(); } + current_pos.x = posx; + current_pos.y = posy; #ifdef DEBUG_POSCTL lastmode = mode; #endif diff --git a/videodev-simulation.cc b/videodev-simulation.cc index 83d7fad..56ff557 100644 --- a/videodev-simulation.cc +++ b/videodev-simulation.cc @@ -197,8 +197,7 @@ Simulation::Simulation() { running = 0; #ifdef DEBUG_POSCTL - debug_tofile((char*)"simulation.log", (char*)"\n"); - debug_tofile((char*)"simulation.log", (char*)"dAngle,dLen,x,y,dx,dy,timedelay,a1.defAngle,a1.defLen,a1.v,a1.dx,a1.dy,a2.defAngle,a2.defLen,a2.v,a2.dx,a2.dy,finalX,finalY\n"); + debug_tofile((char*)"simulation.log", 1, (char*)"dAngle,dLen,x,y,dx,dy,timedelay,a1.defAngle,a1.defLen,a1.v,a1.dx,a1.dy,a2.defAngle,a2.defLen,a2.v,a2.dx,a2.dy,finalX,finalY\n"); #endif Reset(); @@ -207,52 +206,42 @@ Simulation::Simulation() { void Simulation::Reset() { int i; - static int _first_run = 1; LockMutex(); time_t t = time(NULL); - srand (t); - if (_first_run) { - // - // object movement - dAngle = 360.0 * (double)rand() / (double) RAND_MAX; - dLen = 1.0 + 5.0 * (double)rand() / (double) RAND_MAX; - printf ("%s:%d %s dAngle:%f dLen:%f\n", __FILE__, __LINE__, __FUNCTION__, dAngle, dLen); - - // - // axis movement - i = 0; - - axis[i % 2].defAngle = 180.0 + dAngle + (10.0 * (double)rand() / ((double) RAND_MAX)) - 5.0; - axis[i % 2].defLen = dLen + (3.0 * (double)rand() / ((double) RAND_MAX) - 1.5); - axis[i % 2].v = 1.0; - axis[i % 2].active = 1; - - i++; - axis[i % 2].defAngle = 180.0 + dAngle + (10.0 * (double)rand() / ((double) RAND_MAX)) + 85.0; - axis[i % 2].defLen = dLen + (3.0 * (double)rand() / ((double) RAND_MAX) - 1.5); - axis[i % 2].v = 1.0; - axis[i % 2].active = 1; - - // normalize angle between 0..360° - for (i = 0; i < 2; i++) { - while (axis[i].defAngle < 0.0) axis[i].defAngle += 360.0; - while (axis[i].defAngle > 360.0) axis[i].defAngle -= 360.0; - } - } - else { - dAngle +=45.0; - axis[0].defAngle += 45.0; - axis[1].defAngle += 45.0; + // + // object movement + dAngle = 360.0 * (double)rand() / (double) RAND_MAX; + dLen = 1.0 + 5.0 * (double)rand() / (double) RAND_MAX; + printf ("%s:%d %s dAngle:%f dLen:%f\n", __FILE__, __LINE__, __FUNCTION__, dAngle, dLen); + + // + // axis movement + i = 0; + + axis[i % 2].defAngle = 180.0 + dAngle + (10.0 * (double)rand() / ((double) RAND_MAX)) - 5.0; + axis[i % 2].defLen = dLen + ((dLen/2.0) * (double)rand() / ((double) RAND_MAX) - (dLen/4.0)); + axis[i % 2].v = 1.0; + axis[i % 2].active = 1; + + i++; + axis[i % 2].defAngle = 180.0 + dAngle + (10.0 * (double)rand() / ((double) RAND_MAX)) + 85.0; + axis[i % 2].defLen = dLen + ((dLen/2.0) * (double)rand() / ((double) RAND_MAX) - (dLen/4.0)); + axis[i % 2].v = 1.0; + axis[i % 2].active = 1; + + // normalize angle between 0..360° + for (i = 0; i < 2; i++) { + while (axis[i].defAngle < 0.0) axis[i].defAngle += 360.0; + while (axis[i].defAngle > 360.0) axis[i].defAngle -= 360.0; } 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(); - _first_run = 0; } Simulation::~Simulation() { @@ -293,14 +282,14 @@ void Simulation::ThreadProcess() { // calculate movement dx[0] = sindeg(dAngle) * dLen * ms; - dy[0] = -cosdeg(dAngle) * dLen * ms; + dy[0] = cosdeg(dAngle) * dLen * ms; // // simulate motor axis movement for (int i = 0; i < 2; i++) if (axis[i].active) { dx[i+1] = sindeg(axis[i].defAngle) * axis[i].defLen * (1.0 + axis[i].v) * ms; - dy[i+1] = -cosdeg(axis[i].defAngle) * axis[i].defLen * (1.0 + axis[i].v) * ms; + dy[i+1] = cosdeg(axis[i].defAngle) * axis[i].defLen * (1.0 + axis[i].v) * ms; } else { dx[i+1] = 0.0; @@ -319,7 +308,7 @@ void Simulation::ThreadProcess() { if (posY > h) posY = 0.0; #ifdef DEBUG_POSCTL - debug_tofile((char*)"simulation.log", (char*)"%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n", + debug_tofile((char*)"simulation.log", 0, (char*)"%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n", dAngle, dLen, x, y, dx[0], dy[0], ms, axis[0].defAngle,axis[0].defLen,axis[0].v,dx[1],dy[1], axis[1].defAngle,axis[1].defLen,axis[1].v,dx[2],dy[2], diff --git a/videodev-simulation.h b/videodev-simulation.h index a34bf38..9051636 100644 --- a/videodev-simulation.h +++ b/videodev-simulation.h @@ -39,10 +39,10 @@ class Simulation { private: int w; int h; - float posX; - float posY; - float dAngle; - float dLen; + double posX; + double posY; + double dAngle; + double dLen; int running; struct SimAxisCtl axis[2]; GMutex mutex;