steffen
/
SimpleSkyCam
2
0
Fork 0
Code Issues Pull Requests Projects Releases 1 Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '80097f0dbc'
${ noResults }
SimpleSkyCam/posctl.cc

763 lines
27 KiB
Raw Blame History

/***************************************************************************************
*
* posctl.cc is part of SimpleSkyCam.
*
*****************************************************************************************/
#include <list>
#include <string>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include "gui.h"
#include "pid.h"
#include "filter.h"
#include "detect.h"
#include "configuration.h"
#include "video.h"
#include "videodev.h"
#include "histogram.h"
#include "error.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__);
GtkWidget *wnd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "window-posctl"));
gtk_widget_show(wnd);
}
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);
posctl.StartCalibration();
posctl_gui_update();
}
void cb_posctl_show (GtkWidget *widget, gpointer data) {
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
posctl_gui_update();
}
void cb_posctl_btn_axismove (GtkWidget *widget, gpointer data) {
GtkWidget *btn_a1min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a1_min"));
GtkWidget *btn_a1center = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a1_center"));
GtkWidget *btn_a1max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a1_max"));
GtkWidget *btn_a2min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a2_min"));
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"));
double a1min, a2min, a1max, a2max;
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
posctl.GetAxisParam(0, &a1min, &a1max, NULL, NULL, NULL);
posctl.GetAxisParam(1, &a2min, &a2max, NULL, NULL, NULL);
if (widget == btn_a1min) posctl.OutputWriteValue(0, a1min);
else if (widget == btn_a1center) posctl.OutputWriteValue(0, (a1max-a1min)/2.0+a1min);
else if (widget == btn_a1max) posctl.OutputWriteValue(0, a1max);
else if (widget == btn_a2min) posctl.OutputWriteValue(1, a1min);
else if (widget == btn_a2center) posctl.OutputWriteValue(1, (a1max-a1min)/2.0+a1min);
else if (widget == btn_a2max) posctl.OutputWriteValue(1, a1max);
posctl_gui_update();
}
void cb_posctl_change_entry (GtkWidget *widget, gpointer data) {
double a1min, a1max, a1p, a1i, a1d;
double a2min, a2max, a2p, a2i, a2d;
GtkWidget *e_posdevice = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_device"));
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"));
GtkWidget *a1_ki = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_ki"));
GtkWidget *a1_kd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_kd"));
GtkWidget *a2_min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_min"));
GtkWidget *a2_max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_max"));
GtkWidget *a2_kp = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_kp"));
GtkWidget *a2_ki = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_ki"));
GtkWidget *a2_kd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_kd"));
const char *s;
posctl.GetAxisParam(0, &a1min, &a1max, &a1p, &a1i, &a1d);
posctl.GetAxisParam(1, &a2min, &a2max, &a2p, &a2i, &a2d);
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
s = gtk_entry_get_text(GTK_ENTRY(widget));
posctl.LockMutex();
if (e_posdevice == widget) posctl.SetDevice(s);
else if (a1_min == widget) a1min = atof(s);
else if (a1_max == widget) a1max = atof(s);
else if (a1_kp == widget) a1p = atof(s);
else if (a1_ki == widget) a1i = atof(s);
else if (a1_kd == widget) a1d = atof(s);
else if (a2_min == widget) a2min = atof(s);
else if (a2_max == widget) a2max = atof(s);
else if (a2_kp == widget) a2p = atof(s);
else if (a2_ki == widget) a2i = atof(s);
else if (a2_kd == widget) a2d = atof(s);
posctl.SetAxisParam(0, a1min, a1max, a1p, a1i, a1d);
posctl.SetAxisParam(1, a2min, a2max, a2p, a2i, a2d);
posctl.UnLockMutex();
}
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_2d center;
//
// rotation da
GtkWidget *entry_rot_angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_rotangle"));
GtkWidget *entry_rot_len = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_rotlen"));
GtkWidget *entry_axis1_minangle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1minangle"));
GtkWidget *entry_axis1_minlen = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1minlen"));
GtkWidget *entry_axis1_maxangle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1maxangle"));
GtkWidget *entry_axis1_maxlen = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1maxlen"));
GtkWidget *entry_axis2_minangle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2minangle"));
GtkWidget *entry_axis2_minlen = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2minlen"));
GtkWidget *entry_axis2_maxangle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2maxangle"));
GtkWidget *entry_axis2_maxlen = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2maxlen"));
float rotangle = atof(gtk_entry_get_text(GTK_ENTRY(entry_rot_angle))) * (M_PI / 180);
float rotlen = atof(gtk_entry_get_text(GTK_ENTRY(entry_rot_len)));
float a1min_angle = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_minangle))) * (M_PI / 180);
float a1min_len = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_minlen)));
float a1max_angle = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_maxangle))) * (M_PI / 180);
float a1max_len = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_maxlen)));
float a2min_angle = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_minangle))) * (M_PI / 180);
float a2min_len = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_minlen)));
float a2max_angle = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_maxangle))) * (M_PI / 180);
float a2max_len = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_maxlen)));
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"));
clienth = gtk_widget_get_allocated_height(posctl_rot_da);
clientw = gtk_widget_get_allocated_width(posctl_rot_da);
center.x = clientw >> 1;
center.y = clienth >> 1;
//
// draw the background
cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
cairo_paint(cr);
cairo_fill (cr);
cairo_set_source_rgb(cr, 0.8, 0.8, 0.8);
cairo_rectangle (cr, 1, 1, clientw - 2, clienth - 2);
cairo_fill (cr);
cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
cairo_set_dash(cr, linedash1, 2, 0);
cairo_move_to(cr, center.x, center.y - center.y * 0.8);
cairo_line_to(cr, center.x, center.y + center.y * 0.8);
cairo_stroke(cr);
cairo_move_to(cr, center.x - center.x * 0.8, center.y);
cairo_line_to(cr, center.x + center.x * 0.8, center.y);
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(a1min_angle) * a1min_len;
lpos[1].y = lpos[0].y + cos(a1min_angle) * a1min_len;
lpos[2].x = lpos[0].x + sin(a1max_angle) * a1max_len;
lpos[2].y = lpos[0].y + cos(a1max_angle) * a1max_len;
lpos[3].x = lpos[0].x + sin(a2min_angle) * a2min_len;
lpos[3].y = lpos[0].y + cos(a2min_angle) * a2min_len;
lpos[4].x = lpos[0].x + sin(a2max_angle) * a2max_len;
lpos[4].y = lpos[0].y + cos(a2max_angle) * a2max_len;
// 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
cairo_set_source_rgb(cr, 0.0, 0.0, 0.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_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));
cairo_stroke(cr);
cairo_move_to(cr, center.x + float (lpos[2].x * center.x * 0.9 / lmax),
center.y - float (lpos[2].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));
cairo_stroke(cr);
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");
};
void cb_posctl_entryanglelen (GtkWidget *widget, gpointer data) {
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);
};
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 clienth = gtk_widget_get_allocated_height(area);
int clientw = gtk_widget_get_allocated_width(area);
center.x = clientw >> 1;
center.y = clienth >> 1;
cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
cairo_paint(cr);
cairo_fill (cr);
cairo_set_source_rgb(cr, 0.8, 0.8, 0.8);
cairo_rectangle (cr, 1, 1, clientw - 2, clienth - 2);
cairo_fill (cr);
cairo_set_source_rgb(cr, 0.0, 0.0, 0.0);
cairo_set_dash(cr, linedash1, 2, 0);
cairo_move_to(cr, center.x, center.y - center.y * 0.8);
cairo_line_to(cr, center.x, clienth);
cairo_stroke(cr);
cairo_set_dash(cr, NULL, 0, 0);
cairo_set_source_rgb(cr, 0.5, 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, 30, 10);
cairo_show_text(cr, (char *)"offset");
if (da1 != area && da2 != area) {
cairo_move_to(cr, 30, 0);
cairo_show_text(cr, (char *)"unknown");
return;
}
};
/*
* posctl gui update
*/
void posctl_gui_update() {
char txt[255];
double kp, ki, kd, mi, ma;
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 *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"));
GtkWidget *e_cal_a1minangle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1minangle"));
GtkWidget *e_cal_a1minlen = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1minlen"));
GtkWidget *e_cal_a1maxangle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1maxangle"));
GtkWidget *e_cal_a1maxlen = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1maxlen"));
GtkWidget *e_cal_a2minangle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2minangle"));
GtkWidget *e_cal_a2minlen = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2minlen"));
GtkWidget *e_cal_a2maxangle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2maxangle"));
GtkWidget *e_cal_a2maxlen = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2maxlen"));
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"));
GtkWidget *btn_a1center = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a1_center"));
GtkWidget *btn_a1max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a1_max"));
GtkWidget *btn_a2min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a2_min"));
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"));
GtkWidget *a1_ki = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_ki"));
GtkWidget *a1_kd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_kd"));
GtkWidget *a1_out = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_outx"));
GtkWidget *a2_min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_min"));
GtkWidget *a2_max = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_max"));
GtkWidget *a2_kp = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_kp"));
GtkWidget *a2_ki = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_ki"));
GtkWidget *a2_kd = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2_kd"));
GtkWidget *a2_out = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_outy"));
posctl.LockMutex();
int m = posctl.GetMode();
if (m == POSCTL_MODE_OFF) {
gtk_widget_set_sensitive(btnclib, true);
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 *) "");
}
else {
if (m == POSCTL_MODE_CALIB) {
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(btn_a1min, false);
gtk_widget_set_sensitive(btn_a1center, false);
gtk_widget_set_sensitive(btn_a1max, false);
gtk_widget_set_sensitive(btn_a2min, false);
gtk_widget_set_sensitive(btn_a2center, false);
gtk_widget_set_sensitive(btn_a2max, false);
gtk_widget_set_sensitive(e_posdevice, false);
}
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_rot_angle);
gtk_entry_set_text (GTK_ENTRY(e_cal_rotangle), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_rot_len);
gtk_entry_set_text (GTK_ENTRY(e_cal_rotlen), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis1min_angle);
gtk_entry_set_text (GTK_ENTRY(e_cal_a1minangle), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis1min_len);
gtk_entry_set_text (GTK_ENTRY(e_cal_a1minlen), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis1max_angle);
gtk_entry_set_text (GTK_ENTRY(e_cal_a1maxangle), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis1max_len);
gtk_entry_set_text (GTK_ENTRY(e_cal_a1maxlen), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2min_angle);
gtk_entry_set_text (GTK_ENTRY(e_cal_a2minangle), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2min_len);
gtk_entry_set_text (GTK_ENTRY(e_cal_a2minlen), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2max_angle);
gtk_entry_set_text (GTK_ENTRY(e_cal_a2maxangle), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2max_len);
gtk_entry_set_text (GTK_ENTRY(e_cal_a2maxlen), txt);
gtk_entry_set_text (GTK_ENTRY(e_posdevice), posctl.GetDevice().c_str());
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.out[0]);
gtk_label_set_text(GTK_LABEL(a1_out), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.out[1]);
gtk_label_set_text(GTK_LABEL(a2_out), txt);
posctl.GetAxisParam(0, &mi, &ma, &kp, &ki, &kd);
strfromd (txt, sizeof(txt-1), (const char *)"%f", mi);
gtk_entry_set_text (GTK_ENTRY(a1_min), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", ma);
gtk_entry_set_text (GTK_ENTRY(a1_max), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", kp);
gtk_entry_set_text (GTK_ENTRY(a1_kp), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", ki);
gtk_entry_set_text (GTK_ENTRY(a1_ki), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", kd);
gtk_entry_set_text (GTK_ENTRY(a1_kd), txt);
posctl.GetAxisParam(1, &mi, &ma, &kp, &ki, &kd);
strfromd (txt, sizeof(txt-1), (const char *)"%f", mi);
gtk_entry_set_text (GTK_ENTRY(a2_min), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", ma);
gtk_entry_set_text (GTK_ENTRY(a2_max), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", kp);
gtk_entry_set_text (GTK_ENTRY(a2_kp), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", ki);
gtk_entry_set_text (GTK_ENTRY(a2_ki), txt);
strfromd (txt, sizeof(txt-1), (const char *)"%f", kd);
gtk_entry_set_text (GTK_ENTRY(a2_kd), txt);
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;
calib_mode = POSCTL_CALIB_MODE_OFF;
calib_rot_len = 0.0;
calib_rot_angle = 0.0;
calib_axis1min_angle = 0.0;
calib_axis1min_len = 0.0;
calib_axis1max_angle = 0.0;
calib_axis1max_len = 0.0;
calib_axis2min_angle = 0.0;
calib_axis2min_len = 0.0;
calib_axis2max_angle = 0.0;
calib_axis2max_len = 0.0;
device_fd = -1;
device = "";
};
/*
* 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;
calib_mode = POSCTL_CALIB_MODE_START;
gettimeofday (&calib_timestamp, 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);
};
/*
* calibration functions
*/
void PosCtl::CalibModeStart(int x, int y) {
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
calib_pos.x = x;
calib_pos.y = y;
calib_mode = POSCTL_CALIB_MODE_DELTA;
OutputWriteStop();
gettimeofday (&calib_timestamp, NULL);
gdk_threads_add_idle(cb_thread_posctl, NULL);
};
void PosCtl::CalibModeDelta(int x, int y) {
struct timeval tv;
float timediff;
double a1min, a1max;
gettimeofday (&tv, NULL);
timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0);
if (timediff > 10.0) {
position_f_2d fp;
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
pid_axis[0].GetParam(&a1min, &a1max, NULL, NULL, NULL);
OutputWriteStart();
fp.x = (x - calib_pos.x) / (float)timediff;
fp.y = (y - calib_pos.y) / (float)timediff;
LockMutex();
calib_rot_angle = atan2(fp.x, fp.y) * 180.0 / M_PI;
calib_rot_len = sqrt(fp.x * fp.x + fp.y * fp.y);
UnLockMutex();
calib_mode = POSCTL_CALIB_MODE_AXIS1_MI;
OutputWriteValue(0, a1min);
gettimeofday (&calib_timestamp, NULL);
gdk_threads_add_idle(cb_thread_posctl, NULL);
}
};
void PosCtl::CalibModeAxis(int x, int y) {
struct timeval tv;
float timediff;
double a1min, a1max, a2min, a2max;
gettimeofday (&tv, NULL);
timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0);
if (timediff > 5.0) {
printf ("%s:%d %s calib_mode: %d\n", __FILE__, __LINE__, __FUNCTION__, calib_mode);
pid_axis[0].GetParam(&a1min, &a1max, NULL, NULL, NULL);
pid_axis[1].GetParam(&a2min, &a2max, NULL, NULL, NULL);
if (calib_mode == POSCTL_CALIB_MODE_AXIS1_MI) {
OutputWriteValue(0, a1max);
}
else if (calib_mode == POSCTL_CALIB_MODE_AXIS1_MA) {
OutputWriteValue(0, a2min);
}
else if (calib_mode == POSCTL_CALIB_MODE_AXIS2_MI) {
OutputWriteValue(0, a2max);
}
else if (calib_mode == POSCTL_CALIB_MODE_AXIS2_MA) {
OutputWriteStart(); // reset speed output to 50%
}
calib_mode++;
gettimeofday (&calib_timestamp, NULL);
gdk_threads_add_idle(cb_thread_posctl, NULL);
}
};
void PosCtl::CalibModeFinish() {
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
mode = POSCTL_MODE_OFF;
calib_mode = POSCTL_CALIB_MODE_OFF;
gettimeofday (&calib_timestamp, NULL);
gdk_threads_add_idle(cb_thread_posctl, NULL);
};
/*
* Loop, if new data is aviable
*/
void PosCtl::Loop (int posx, int posy) {
//
// calibration mode?
if (mode == POSCTL_MODE_CALIB) {
switch (calib_mode) {
case (POSCTL_CALIB_MODE_START):
CalibModeStart(posx, posy);
break;
case (POSCTL_CALIB_MODE_DELTA):
CalibModeDelta(posx, posy);
break;
case (POSCTL_CALIB_MODE_AXIS1_MI):
case (POSCTL_CALIB_MODE_AXIS1_MA):
case (POSCTL_CALIB_MODE_AXIS2_MI):
case (POSCTL_CALIB_MODE_AXIS2_MA):
CalibModeAxis(posx, posy);
break;
case (POSCTL_CALIB_MODE_FINISH):
CalibModeFinish();
break;
default:
calib_mode = POSCTL_CALIB_MODE_OFF;
mode = POSCTL_MODE_OFF;
gdk_threads_add_idle(cb_thread_posctl, NULL);
}
}
};
void PosCtl::SetDevice (std::string d) {
printf ("%s:%d %s new device:%s\n", __FILE__, __LINE__, __FUNCTION__, d.c_str());
OutputClose();
device = d;
};
int PosCtl::OutputClose() {
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
if (device_fd > 0) close(device_fd);
device_fd = -1;
return 0;
};
int PosCtl::OutputOpen() {
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
if (device_fd > 0) return 0;
device_fd = open (device.c_str(), O_RDWR);
if (device_fd < 0) {
printf ("%s:%d could not open device:%s Error:%s\n", __FILE__, __LINE__,
device.c_str(), strerror(errno));
errormessage_display ((char *)"window-posctl", (char *)"OutputOpen",
(char*)"%s:%d could not open device:%s Error:%s\n", __FILE__, __LINE__, device.c_str(), strerror(errno));
return -1;
}
return 0;
};
int PosCtl::WriteTTY (char * outbuf) {
ssize_t len;
printf ("%s:%d %s send: '%s'\n", __FILE__, __LINE__, __FUNCTION__, outbuf);
len = write (device_fd, outbuf, strlen(outbuf));
if ((size_t) len != strlen(outbuf) || len < 0) {
printf ("%s:%d could not write data to device:%s Error:%s\n", __FILE__, __LINE__,
device.c_str(), strerror(errno));
errormessage_display ((char *)"window-posctl", (char *)"WriteTTY",
(char *) "%s:%d could not write data to device:%s Error:%s\n", __FILE__, __LINE__,
device.c_str(), strerror(errno));
}
return 0;
}
int PosCtl::ReadTTY (char * inbuf, int length) {
ssize_t len;
// make device non-blocking
fcntl(device_fd, F_SETFL, fcntl(device_fd, F_GETFL) | O_NONBLOCK);
len = read (device_fd, inbuf, length);
// somehow the first read sometimes fails, no idea why
if (len < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) {
usleep(100000);
len = read (device_fd, inbuf, length);
}
if (len < 0) {
printf ("%s:%d could not read data from device:%s Error:%s\n", __FILE__, __LINE__,
device.c_str(), strerror(errno));
errormessage_display ((char *)"window-posctl", (char *)"ReadTTY",
(char *) "%s:%d could not read data from device:%s Error:%s\n", __FILE__, __LINE__,
device.c_str(), strerror(errno));
inbuf[0] = 0;
}
else {
inbuf[len] = 0;
printf ("%s:%d %s receive: '%s'\n", __FILE__, __LINE__, __FUNCTION__, inbuf);
}
// make device blocking
fcntl(device_fd, F_SETFL, fcntl(device_fd, F_GETFL) & ~O_NONBLOCK);
return 0;
}
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 (device_fd <= 0) if (OutputOpen() != 0) return -1;
//
// save language setting and set to plain C
std::string s = setlocale(LC_ALL, NULL);
setlocale (LC_ALL, "C");
snprintf(outbuf, 254, ":R%c%+09.4f#", (axis == 0 ? 'D' : 'R'), value);
outbuf[254] = 0;
// reset language setting to default
setlocale (LC_ALL, s.c_str());
WriteTTY(outbuf);
ReadTTY(outbuf, sizeof(outbuf) - 1);
return 0;
};
int PosCtl::OutputWriteStop () {
char outbuf[255];
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
snprintf (outbuf, 255, ":Q#");
if (device_fd <= 0) if (OutputOpen() != 0) return -1;
WriteTTY(outbuf);
return 0;
}
int PosCtl::OutputWriteStart () {
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
OutputWriteValue(0, 0.0);
OutputWriteValue(1, 0.0);
return 0;
}
Reference in new issue View Git Blame Copy Permalink