diff --git a/detect.h b/detect.h index 9d20ccf..68806b6 100644 --- a/detect.h +++ b/detect.h @@ -72,13 +72,15 @@ class PosCtl { double calib_rot_angle; double calib_rot_len; - double calib_axis1_angle; - double calib_axis1_lenmi; - double calib_axis1_lenma; + double calib_axis1min_angle; + double calib_axis1min_len; + double calib_axis1max_angle; + double calib_axis1max_len; - double calib_axis2_angle; - double calib_axis2_lenmi; - double calib_axis2_lenma; + double calib_axis2min_angle; + double calib_axis2min_len; + double calib_axis2max_angle; + double calib_axis2max_len; double out[2]; @@ -96,8 +98,8 @@ class PosCtl { void Loop (int posx, int posy); int OutputWriteValue (int axis, double value); - int OutputWriteStop (int axis); - int OutputWriteStart (int axis); + int OutputWriteStop (); + int OutputWriteStart (); }; diff --git a/posctl.cc b/posctl.cc index 449daa4..b6f9902 100644 --- a/posctl.cc +++ b/posctl.cc @@ -136,22 +136,25 @@ void cb_posctl_angles_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer // 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_angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1angle")); - GtkWidget *entry_axis1_lenmi = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1lenmi")); - GtkWidget *entry_axis1_lenma = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1lenma")); - GtkWidget *entry_axis2_angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2angle")); - GtkWidget *entry_axis2_lenmi = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2lenmi")); - GtkWidget *entry_axis2_lenma = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2lenma")); + 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 a1_angle = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_angle))) * (M_PI / 180); - float a1_lenmi = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_lenmi))); - float a1_lenma = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis1_lenma))); - float a2_angle = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_angle))) * (M_PI / 180); - float a2_lenmi = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_lenmi))); - float a2_lenma = atof(gtk_entry_get_text(GTK_ENTRY(entry_axis2_lenma))); -// float lenmax = max(max(max(a1_lenmi, a1_lenma),max(a2_lenmi, a2_lenma)), rotlen); + 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")); @@ -185,15 +188,15 @@ void cb_posctl_angles_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer lpos[0].x = sin(rotangle) * rotlen; lpos[0].y = cos(rotangle) * rotlen; - lpos[1].x = lpos[0].x + sin(a1_angle) * a1_lenmi; - lpos[1].y = lpos[0].y + cos(a1_angle) * a1_lenmi; - lpos[2].x = lpos[0].x + sin(a1_angle) * a1_lenma; - lpos[2].y = lpos[0].y + cos(a1_angle) * a1_lenma; + 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(a2_angle) * a2_lenmi; - lpos[3].y = lpos[0].y + cos(a2_angle) * a2_lenmi; - lpos[4].x = lpos[0].x + sin(a2_angle) * a2_lenma; - lpos[4].y = lpos[0].y + cos(a2_angle) * a2_lenma; + 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++) { @@ -305,13 +308,15 @@ void posctl_gui_update() { 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_a1angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1angle")); - GtkWidget *e_cal_a1lenmi = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1lenmi")); - GtkWidget *e_cal_a1lenma = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a1lenma")); + 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_a2angle = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2angle")); - GtkWidget *e_cal_a2lenmi = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2lenmi")); - GtkWidget *e_cal_a2lenma = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_a2lenma")); + 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")); @@ -369,19 +374,23 @@ void posctl_gui_update() { 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_axis1_angle); - gtk_entry_set_text (GTK_ENTRY(e_cal_a1angle), txt); - strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis1_lenmi); - gtk_entry_set_text (GTK_ENTRY(e_cal_a1lenmi), txt); - strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis1_lenma); - gtk_entry_set_text (GTK_ENTRY(e_cal_a1lenma), txt); - - strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2_angle); - gtk_entry_set_text (GTK_ENTRY(e_cal_a2angle), txt); - strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2_lenmi); - gtk_entry_set_text (GTK_ENTRY(e_cal_a2lenmi), txt); - strfromd (txt, sizeof(txt-1), (const char *)"%f", posctl.calib_axis2_lenma); - gtk_entry_set_text (GTK_ENTRY(e_cal_a2lenma), 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]); @@ -433,12 +442,14 @@ PosCtl::PosCtl() { calib_mode = POSCTL_CALIB_MODE_OFF; calib_rot_len = 0.0; calib_rot_angle = 0.0; - calib_axis1_angle = 0.0; - calib_axis1_lenmi = 0.0; - calib_axis1_lenma = 0.0; - calib_axis2_angle = 0.0; - calib_axis2_lenmi = 0.0; - calib_axis2_lenma = 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 = ""; }; @@ -508,6 +519,7 @@ void PosCtl::CalibModeStart(int x, int y) { calib_pos.y = y; calib_mode = POSCTL_CALIB_MODE_DELTA; + OutputWriteStop(); gettimeofday (&calib_timestamp, NULL); gdk_threads_add_idle(cb_thread_posctl, NULL); }; @@ -516,6 +528,7 @@ void PosCtl::CalibModeStart(int x, int y) { 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); @@ -524,6 +537,9 @@ void PosCtl::CalibModeDelta(int x, int y) { 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; @@ -533,7 +549,8 @@ void PosCtl::CalibModeDelta(int x, int y) { calib_rot_len = sqrt(fp.x * fp.x + fp.y * fp.y); UnLockMutex(); - calib_mode = POSCTL_CALIB_MODE_AXIS1_MA; + calib_mode = POSCTL_CALIB_MODE_AXIS1_MI; + OutputWriteValue(0, a1min); gettimeofday (&calib_timestamp, NULL); gdk_threads_add_idle(cb_thread_posctl, NULL); } @@ -543,6 +560,7 @@ void PosCtl::CalibModeDelta(int x, int y) { 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); @@ -550,6 +568,22 @@ void PosCtl::CalibModeAxis(int x, int y) { 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); @@ -643,7 +677,7 @@ int PosCtl::OutputWriteValue (int axis, double value) { std::string s = setlocale(LC_ALL, NULL); setlocale (LC_ALL, "C"); - snprintf(outbuf, 254, ":R%c%c%08.4f#\n", (axis == 0 ? 'A' : 'R'), + snprintf(outbuf, 254, ":R%c%c%08.4f#\n", (axis == 0 ? 'D' : 'R'), (value < 0 ? '-' : '+'), value); outbuf[254] = 0; @@ -665,17 +699,34 @@ int PosCtl::OutputWriteValue (int axis, double value) { return 0; }; -int PosCtl::OutputWriteStop (int axis) { - printf ("%s:%d %s Axis %d\n", __FILE__, __LINE__, __FUNCTION__, axis); +int PosCtl::OutputWriteStop () { + char outbuf[255]; + ssize_t len; + + printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); + snprintf (outbuf, 255, ":Q#, RT9#\n"); if (device_fd <= 0) if (OutputOpen() != 0) return -1; + + 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 *)"OutputWriteValue", + (char *) "%s:%d could not write data to device:%s Error:%s\n", __FILE__, __LINE__, + device.c_str(), strerror(errno)); + } + + return 0; } -int PosCtl::OutputWriteStart (int axis) { - printf ("%s:%d %s Axis %d\n", __FILE__, __LINE__, __FUNCTION__, axis); +int PosCtl::OutputWriteStart () { + printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); + + OutputWriteValue(0, 1.0); + OutputWriteValue(1, 1.0); - if (device_fd <= 0) if (OutputOpen() != 0) return -1; return 0; } diff --git a/simpleskycam.ui b/simpleskycam.ui index 6ef3980..17934ee 100644 --- a/simpleskycam.ui +++ b/simpleskycam.ui @@ -762,7 +762,7 @@ - + True False @@ -771,7 +771,7 @@ 4 4 - + True False True @@ -801,7 +801,7 @@ - + True False True @@ -819,7 +819,7 @@ True False - Angle + Min: Angle 1 @@ -830,7 +830,7 @@ True False - Len Min + Len 2 @@ -885,11 +885,10 @@ 2 1 - 2 - + True False True @@ -904,7 +903,7 @@ - + True False True @@ -922,7 +921,7 @@ True False - Len Max + Max: Angle 3 @@ -930,35 +929,80 @@ - + True False True False 6 - 15 + 30 3 - 2 + 3 - + True False True False 6 - 30 + 15 + + 4 + 2 + + + + + True + False + True + False + 6 + 15 + 3 + 2 + + + + + True + False + True + False + 6 + 15 + + + 4 3 + + + True + False + Len + + + 4 + 0 + + + + + + + +