|
|
|
@ -297,8 +297,8 @@ void cb_posctl_entryanglelen (GtkWidget *widget, gpointer data) {
|
|
|
|
};
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#define AXIS_DIFF_MIN -10.0
|
|
|
|
#define AXIS_DIFF_MIN -20.0
|
|
|
|
#define AXIS_DIFF_MAX 10.0
|
|
|
|
#define AXIS_DIFF_MAX 20.0
|
|
|
|
|
|
|
|
|
|
|
|
void cb_posctl_axis_draw(GtkWidget *area, cairo_t *cr, int w, int h, 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 *da1 = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_da_axis1"));
|
|
|
|
@ -406,13 +406,12 @@ void posctl_gui_update() {
|
|
|
|
char txt[255];
|
|
|
|
char txt[255];
|
|
|
|
double kp, ki, kd, mi, ma;
|
|
|
|
double kp, ki, kd, mi, ma;
|
|
|
|
|
|
|
|
|
|
|
|
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
GtkWidget *caliblabel = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_calib_label"));
|
|
|
|
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 *btnclib = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_calib"));
|
|
|
|
GtkWidget *btnstop = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_stop"));
|
|
|
|
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 *btncontrol = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_control"));
|
|
|
|
|
|
|
|
GtkWidget *btnsimreset = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_resetsim"));
|
|
|
|
|
|
|
|
|
|
|
|
GtkWidget *e_cal_rotangle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_rotangle"));
|
|
|
|
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_rotlen = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_rotlen"));
|
|
|
|
@ -422,6 +421,8 @@ void posctl_gui_update() {
|
|
|
|
|
|
|
|
|
|
|
|
GtkWidget *e_cal_a2angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2angle"));
|
|
|
|
GtkWidget *e_cal_a2angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2angle"));
|
|
|
|
GtkWidget *e_cal_a2len = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2len"));
|
|
|
|
GtkWidget *e_cal_a2len = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2len"));
|
|
|
|
|
|
|
|
GtkWidget *lb_earthaxis = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_earthaxis"));
|
|
|
|
|
|
|
|
GtkWidget *lb_axisaxis = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_axisaxis"));
|
|
|
|
|
|
|
|
|
|
|
|
GtkWidget *e_posdevice = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_device"));
|
|
|
|
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_a1min = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a1_min"));
|
|
|
|
@ -431,6 +432,7 @@ void posctl_gui_update() {
|
|
|
|
GtkWidget *btn_a2center = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_a2_center"));
|
|
|
|
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 *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_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_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_kp = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1_kp"));
|
|
|
|
@ -493,8 +495,12 @@ void posctl_gui_update() {
|
|
|
|
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2.l);
|
|
|
|
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2.l);
|
|
|
|
gtk_entry_set_text (GTK_ENTRY(e_cal_a2len), txt);
|
|
|
|
gtk_entry_set_text (GTK_ENTRY(e_cal_a2len), txt);
|
|
|
|
|
|
|
|
|
|
|
|
gtk_entry_set_text (GTK_ENTRY(e_posdevice), posctl.GetDevice().c_str());
|
|
|
|
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_rot.a - posctl.calib_axis1.a);
|
|
|
|
|
|
|
|
gtk_label_set_text (GTK_LABEL(lb_earthaxis), txt);
|
|
|
|
|
|
|
|
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis1.a - posctl.calib_axis2.a);
|
|
|
|
|
|
|
|
gtk_label_set_text (GTK_LABEL(lb_axisaxis), txt);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
gtk_entry_set_text (GTK_ENTRY(e_posdevice), posctl.GetDevice().c_str());
|
|
|
|
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.axis_op[0]);
|
|
|
|
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.axis_op[0]);
|
|
|
|
gtk_label_set_text(GTK_LABEL(a1_out), txt);
|
|
|
|
gtk_label_set_text(GTK_LABEL(a1_out), txt);
|
|
|
|
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.axis_op[1]);
|
|
|
|
strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.axis_op[1]);
|
|
|
|
@ -529,6 +535,10 @@ void posctl_gui_update() {
|
|
|
|
strfromd (txt, sizeof(txt-1), (const char *)"%f", kd);
|
|
|
|
strfromd (txt, sizeof(txt-1), (const char *)"%f", kd);
|
|
|
|
gtk_entry_set_text_nofocus (a2_kd, txt);
|
|
|
|
gtk_entry_set_text_nofocus (a2_kd, txt);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
const gchar *txtptr = gtk_entry_get_text(GTK_ENTRY(e_posdevice));
|
|
|
|
|
|
|
|
if (strncmp(txtptr, "SIMULATION", 11) == 0) gtk_widget_show (btnsimreset);
|
|
|
|
|
|
|
|
else gtk_widget_hide (btnsimreset);
|
|
|
|
|
|
|
|
|
|
|
|
posctl.UnLockMutex();
|
|
|
|
posctl.UnLockMutex();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
@ -752,27 +762,18 @@ void PosCtl::Loop (int posx, int posy) {
|
|
|
|
// dist_axis2 --> will control pis_axis1
|
|
|
|
// dist_axis2 --> will control pis_axis1
|
|
|
|
else if (mode == POSCTL_MODE_CONTROL) {
|
|
|
|
else if (mode == POSCTL_MODE_CONTROL) {
|
|
|
|
position_f_2d p;
|
|
|
|
position_f_2d p;
|
|
|
|
double d;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
LockMutex();
|
|
|
|
LockMutex();
|
|
|
|
|
|
|
|
|
|
|
|
// calculate
|
|
|
|
// calculate
|
|
|
|
// dist_axis < 0.0 pid should increase the output.
|
|
|
|
// dist_axis < 0.0 pid should increase the output.
|
|
|
|
// > 0.0 pid should decrease the output
|
|
|
|
// > 0.0 pid should decrease the output
|
|
|
|
p.x = 0.0; p.y = 0.0;
|
|
|
|
|
|
|
|
d = calib_axis1_v.perpendicular(calib_axis2_v, p);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
p.x = posx; p.y = posy;
|
|
|
|
p.x = posx; p.y = posy;
|
|
|
|
axis_pv[0] = calib_axis2_v.perpendicular(p, target_pos);
|
|
|
|
axis_pv[0] = calib_axis2_v.perpendicular(p, target_pos);
|
|
|
|
axis_pv[1] = calib_axis1_v.perpendicular(p, target_pos);
|
|
|
|
axis_pv[1] = calib_axis1_v.perpendicular(p, target_pos);
|
|
|
|
axis_op[0] = pid_axis[0].Update(0.0, axis_pv[0]);
|
|
|
|
axis_op[0] = pid_axis[0].Update(0.0, axis_pv[0]);
|
|
|
|
axis_op[1] = pid_axis[1].Update(0.0, axis_pv[1]);
|
|
|
|
axis_op[1] = pid_axis[1].Update(0.0, axis_pv[1]);
|
|
|
|
|
|
|
|
|
|
|
|
printf ("%s:%d %s", __FILE__, __LINE__, __FUNCTION__);
|
|
|
|
|
|
|
|
printf (" d: %+6.3f ", d);
|
|
|
|
|
|
|
|
printf (" Axis1 [dist:%+6.3f out1:%+6.3f] ", axis_pv[0], axis_op[0]);
|
|
|
|
|
|
|
|
printf (" Axis2 [dist:%+6.3f out2:%+6.3f]\n", axis_pv[1], axis_op[1]);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
OutputWriteValue(0, axis_op[0]);
|
|
|
|
OutputWriteValue(0, axis_op[0]);
|
|
|
|
OutputWriteValue(1, axis_op[1]);
|
|
|
|
OutputWriteValue(1, axis_op[1]);
|
|
|
|
|
|
|
|
|
|
|
|
|