steffen
/
SimpleSkyCam
2
0
Fork 0
Code Issues Pull Requests Projects Releases 1 Wiki Activity

posctl works finally.

Browse Source
master
Steffen Pohle 3 years ago
parent 2a6b5b099f
commit 3a11f81658
10 changed files with 230 additions and 148 deletions
Show Stats Download Patch File Download Diff File

4
Makefile.config
Unescape View File

@ -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

21
Makefile.rules.crosswindows
Unescape View File

@ -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
#

12
debug-angles.cc
Unescape View File

@ -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;

46
debug.cc
Unescape View File

@ -3,48 +3,54 @@
#include <gdk/gdk.h>
#include <glib.h>
#include <sys/time.h>
#include <string>
#include <time.h>
#include <fcntl.h>
#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);
}

2
debug.h
Unescape View File

@ -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

20
detect.h
Unescape View File

@ -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 <winsock2.h>
#include <io.h>
#include <ws2tcpip.h>
#include <windows.h>
#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];

28
gui.h
Unescape View File

@ -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

168
posctl.cc
Unescape View File

@ -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

69
videodev-simulation.cc
Unescape View File

@ -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],

8
videodev-simulation.h
Unescape View File

@ -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;

Write Preview
Loading…
Cancel
Save