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

trying 2 step calibration and justify of the output values, but not working yet

Browse Source
master
Steffen Pohle 2 years ago
parent d216628ac8
commit 539e49b28f
8 changed files with 1919 additions and 1181 deletions
Show Stats Download Patch File Download Diff File

17
configuration.cc
Unescape View File

@ -15,6 +15,7 @@ extern PosCtl posctl;
Configuration::Configuration() {
debugpath = NULL;
readdumppath = NULL;
destpath = "./";
};
@ -99,6 +100,7 @@ void Configuration::SaveConfig(std::string filename) {
__FILE__, __LINE__, i, amin, amax, akp, aki, akd);
jp.AddObject("posctl_axis" + to_string(i), jaxis);
}
jp.AddObject("posctl_filter", posctl.GetFilter());
jp.AddObject("posctl_device", posctl.GetDevice());
posctl.UnLockMutex();
@ -107,6 +109,10 @@ void Configuration::SaveConfig(std::string filename) {
jp.AddObject("histogram_log", histogram_log);
jp.AddObject("debayer_mode", debayer_mode);
//
// save destination path
jp.AddObject("destination_path", destpath);
//
// save button config
for (i = 0; i < BTN_PRESET_MAX; i++) {
@ -218,7 +224,7 @@ void Configuration::LoadConfig(std::string filename) {
//
// load posctl
double amin, amax, akp, aki, akd;
double amin, amax, akp, aki, akd, f;
JSONParse jaxis;
posctl.LockMutex();
@ -236,6 +242,10 @@ void Configuration::LoadConfig(std::string filename) {
}
if (jp.GetValue("posctl_device", &vstr))
posctl.SetDevice(vstr);
if (jp.GetValueDouble("posctl_filter", &f))
posctl.SetFilter(f);
jp.AddObject("posctl_filter", posctl.GetFilter());
posctl.UnLockMutex();
jp.GetValueInt("histogram_log", &i);
@ -244,6 +254,11 @@ void Configuration::LoadConfig(std::string filename) {
jp.GetValueInt("debayer_mode", &i);
SetDebayerMode(i);
if (jp.GetValue("destination_path", &destpath)) {
GtkWidget *entry = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "input_entry_destpath"));
gtk_entry_set_text(GTK_ENTRY(entry), destpath.c_str());
}
//
// start streaming if a device was selected.
//

5
configuration.h
Unescape View File

@ -19,7 +19,7 @@ class Configuration {
private:
int histogram_log = 1; // logarithmic or linear scale
int calibration_showdata = 0; // needed for debugging calibration
std::string destpath; // destination path
JSONParse config;
std::string GetDefaultFileName();
list<VideoDevCtrl> presetbtn[BTN_PRESET_MAX];
@ -39,6 +39,9 @@ public:
void SetDebayerMode(int trueorfalse);
void SetDestPath (std::string dest) { destpath = dest; };
std::string GetDestPath() { return destpath; };
void SetHistogramLog(int trueorfalse);
int GetHistogramLog() { return histogram_log; };

21
detect.h
Unescape View File

@ -49,7 +49,9 @@ enum {
enum {
POSCTL_MODE_OFF = 0,
POSCTL_MODE_CALIB,
POSCTL_MODE_CONTROL
POSCTL_MODE_CONTROL,
POSCTL_MODE_2STEPINIT,
POSCTL_MODE_2STEP
};
enum {
@ -70,6 +72,14 @@ enum {
};
struct PosCtl_2Step_Data {
double pv[2];
double op[2];
double npv[2];
double ppv[2];
double nop[2];
double pop[2];
};
struct PosCtl_ThreadData {
position_2d c[2];
@ -96,10 +106,15 @@ class PosCtl {
int calib_mode;
position_2d calib_pos;
position_f_2d pos;
double npv[2];
double ppv[2];
double nop[2];
double pop[2];
double posfilter;
struct PosCtl_ThreadData threaddata; // will be copied to gtk thread
void NotifyGtk();
void NotifyGtk2Step();
void CalibModeStart(int x, int y);
void CalibModeDelta(int x, int y);
@ -135,11 +150,15 @@ class PosCtl {
void Stop ();
void StartCalibration ();
void StartControl ();
void Start2Step ();
void Stop2Step ();
void Reset2Step ();
int GetMode () { return mode; };
void SetAxisParam (int axis, double min, double max, double k, double i, double d);
void GetAxisParam (int axis, double *min, double *max, double *k, double *i, double *d);
void SetDevice(std::string d);
void SetFilter(double f) { posfilter = f; };
double GetFilter() { return posfilter; };
std::string GetDevice() { return device; };
void Loop (int posx, int posy, double dt);

35
gui.cc
Unescape View File

@ -360,6 +360,40 @@ void cb_detect_show_window (GtkWidget *widget, gpointer data) {
};
/*
* setup destination path
*/
void cb_input_btnsetdestpath (GtkWidget *widget, gpointer data) {
GtkBuilder *builder = (GtkBuilder *) data;
GtkWindow *window = GTK_WINDOW (gtk_builder_get_object (builder, "windows-input"));
GtkWidget *dialog;
GtkFileChooserAction action = GTK_FILE_CHOOSER_ACTION_SELECT_FOLDER;
gint res;
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
dialog = gtk_file_chooser_dialog_new ("Open Folder",
window,
action,
"_Cancel",
GTK_RESPONSE_CANCEL,
"_Open",
GTK_RESPONSE_ACCEPT,
NULL);
res = gtk_dialog_run (GTK_DIALOG (dialog));
if (res == GTK_RESPONSE_ACCEPT) {
char *filename;
GtkFileChooser *chooser = GTK_FILE_CHOOSER (dialog);
filename = gtk_file_chooser_get_filename (chooser);
config.SetDestPath(filename);
GtkWidget *entry = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "input_entry_destpath"));
gtk_entry_set_text(GTK_ENTRY(entry), filename);
g_free (filename);
}
gtk_widget_destroy (dialog);
};
void cb_input_show_window (GtkWidget *widget, gpointer data) {
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
@ -420,3 +454,4 @@ void draw_printf(cairo_t *cr, int x, int y, float border, char *fmt,...) {
draw_text (cr, x, y, border, buffer);
};

4
gui.h
Unescape View File

@ -155,7 +155,7 @@ G_MODULE_EXPORT void cb_input_show_window (GtkWidget *widget, gpointer data);
G_MODULE_EXPORT void cb_input_btncapture (GtkWidget *widget, gpointer data);
G_MODULE_EXPORT void cb_input_btnsnapshot (GtkWidget *widget, gpointer data);
G_MODULE_EXPORT void cb_input_btnscale (GtkWidget *widget, gpointer data);
G_MODULE_EXPORT void cb_input_btnsetdestpath (GtkWidget *widget, gpointer data);
//
// filter detect or image data
@ -202,6 +202,8 @@ 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);
G_MODULE_EXPORT void cb_posctl_btn2stepstartstop (GtkWidget *widget, gpointer data);
G_MODULE_EXPORT void cb_posctl_btn2stepreset (GtkWidget *widget, gpointer data);
//

329
posctl.cc
Unescape View File

@ -127,6 +127,30 @@ void cb_posctl_btnsetdest (GtkWidget *widget, gpointer data) {
};
/*
* calibrate adjust control output..
*/
void cb_posctl_btn2stepstartstop (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_2stepctl"));
if (posctl.GetMode() == POSCTL_MODE_2STEP || posctl.GetMode() == POSCTL_MODE_2STEPINIT) {
posctl.Stop2Step();
gtk_button_set_label(GTK_BUTTON(btn), "Start");
}
else {
posctl.Start2Step();
gtk_button_set_label(GTK_BUTTON(btn), "Stop");
}
};
void cb_posctl_btn2stepreset (GtkWidget *widget, gpointer data) {
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
posctl.Reset2Step();
}
void cb_posctl_btn_axismove (GtkWidget *widget, gpointer data) {
GtkWidget *btn_ramin = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_btn_ra_min"));
@ -261,43 +285,49 @@ void cb_posctl_angles_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer
cairo_set_font_size (cr, 11);
// 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 + 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);
if (!isnan(rotangle) && !isnan(rotlen)) {
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 + 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);
}
// axis1
lpos.x = sin(ra_angle) * ra_len * 4.0;
lpos.y = cos(ra_angle) * ra_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);
if (!isnan(ra_angle) && !isnan(ra_len)) {
lpos.x = sin(ra_angle) * ra_len * 4.0;
lpos.y = cos(ra_angle) * ra_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);
}
// axis2
lpos.x = sin(dec_angle) * dec_len * 4.0;
lpos.y = cos(dec_angle) * dec_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);
if (!isnan(dec_angle) && !isnan(dec_len)) {
lpos.x = sin(dec_angle) * dec_len * 4.0;
lpos.y = cos(dec_angle) * dec_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
@ -320,18 +350,24 @@ void cb_posctl_angles_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer
cairo_stroke(cr);
// draw distance to axis
lpos.x = sindeg(posctl.calib_axis1.a) * posctl.axis_pv[AXIS_DEC] * pscale;
lpos.y = cosdeg(posctl.calib_axis1.a) * posctl.axis_pv[AXIS_DEC] * 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);
if (!isnan(posctl.calib_axis1.a) && !isnan(posctl.axis_pv[AXIS_DEC])) {
lpos.x = sindeg(posctl.calib_axis1.a+90.0) * posctl.axis_pv[AXIS_DEC] * pscale;
lpos.y = cosdeg(posctl.calib_axis1.a+90.0) * posctl.axis_pv[AXIS_DEC] * pscale;
cairo_set_source_rgb(cr, 0.5, 1.0, 0.5);
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[AXIS_RA] * pscale;
lpos.y = cosdeg(posctl.calib_axis2.a) * posctl.axis_pv[AXIS_RA] * 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);
if (!isnan(posctl.calib_axis2.a) && !isnan(posctl.axis_pv[AXIS_RA])) {
lpos.x = sindeg(posctl.calib_axis2.a+90.0) * posctl.axis_pv[AXIS_RA] * pscale;
lpos.y = cosdeg(posctl.calib_axis2.a+90.0) * posctl.axis_pv[AXIS_RA] * pscale;
cairo_set_source_rgb(cr, 0.5, 0.5, 1.0);
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);
}
};
@ -453,6 +489,43 @@ void cb_posctl_axis_draw(GtkWidget *area, cairo_t *cr, int w, int h, gpointer da
};
/*
* add result to the table
*/
void posctl_2step_gui_update( double axis_pv[2], double axis_op[2],
double npv[2], double nop[2], double ppv[2], double pop[2]) {
GtkWidget *lramin = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_2s_ramin"));
GtkWidget *lramax = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_2s_ramax"));
GtkWidget *ldecmin = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_2s_decmin"));
GtkWidget *ldecmax = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_2s_decmax"));
GtkWidget *lraout = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_2s_raout"));
GtkWidget *ldecout = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_2s_decout"));
char txt[255];
// min
snprintf (txt, 255, "%2.5f [%2.5f]", npv[AXIS_RA], nop[AXIS_RA]);
gtk_label_set_text(GTK_LABEL(lramin), txt);
snprintf (txt, 255, "%2.5f [%2.5f]", npv[AXIS_DEC], nop[AXIS_DEC]);
gtk_label_set_text(GTK_LABEL(ldecmin), txt);
// max
snprintf (txt, 255, "%2.5f [%2.5f]", ppv[AXIS_RA], pop[AXIS_RA]);
gtk_label_set_text(GTK_LABEL(lramax), txt);
snprintf (txt, 255, "%2.5f [%2.5f]", ppv[AXIS_DEC], pop[AXIS_DEC]);
gtk_label_set_text(GTK_LABEL(ldecmax), txt);
// output
snprintf (txt, 255, "%2.5f [%2.5f]", axis_pv[AXIS_RA], axis_op[AXIS_RA]);
gtk_label_set_text(GTK_LABEL(lraout), txt);
snprintf (txt, 255, "%2.5f [%2.5f]", axis_pv[AXIS_DEC], axis_op[AXIS_DEC]);
gtk_label_set_text(GTK_LABEL(ldecout), txt);
cb_posctl_entryanglelen(NULL, NULL);
}
/*
* posctl gui update
*/
@ -502,6 +575,8 @@ void posctl_gui_update() {
GtkWidget *dec_out = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_out_dec"));
GtkWidget *dec_pv = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_lb_d_dec"));
GtkWidget *pos_filter = GTK_WIDGET(gtk_builder_get_object (GTK_BUILDER(_builder_), "posctl_entry_inputfilter"));
posctl.LockMutex();
int m = posctl.GetMode();
@ -589,12 +664,32 @@ void posctl_gui_update() {
strfromd (txt, sizeof(txt-1), (char *)"%f", kd);
gtk_entry_set_text_nofocus (dec_kd, txt);
strfromd (txt, sizeof(txt-1), (char *)"%f", posctl.GetFilter());
gtk_entry_set_text (GTK_ENTRY(pos_filter), 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();
}
/*
* callback from the detect thread.
* the gtk/gui updates must and will be processed in a separate gui thread
*/
gboolean cb_thread_posctl_2step (gpointer data) {
struct PosCtl_2Step_Data *d = (struct PosCtl_2Step_Data *) data;
posctl_gui_update();
if (data) {
posctl_2step_gui_update(d->pv, d->op, d->npv, d->nop, d->ppv, d->pop);
free (data);
}
return false;
};
/*
* callback from the detect thread.
@ -654,7 +749,7 @@ gboolean cb_thread_posctl (gpointer data) {
}
}
return false;
return false;
};
@ -681,8 +776,32 @@ PosCtl::PosCtl() {
*/
void PosCtl::Stop() {
printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
LockMutex();
mode = POSCTL_MODE_OFF;
NotifyGtk();
for (int i = 0; i < 2; i++) {
npv[i] = NAN;
ppv[i] = NAN;
nop[i] = NAN;
pop[i] = NAN;
}
UnLockMutex();
}
void PosCtl::NotifyGtk2Step() {
struct PosCtl_2Step_Data *data = (struct PosCtl_2Step_Data*) malloc (sizeof(struct PosCtl_2Step_Data));
for (int i = 0; i < 2; i++) {
data->pv[i] = axis_pv[i];
data->op[i] = axis_op[i];
data->npv[i] = npv[i];
data->ppv[i] = ppv[i];
data->nop[i] = nop[i];
data->pop[i] = pop[i];
}
gdk_threads_add_idle(cb_thread_posctl_2step, (gpointer) data);
}
@ -923,9 +1042,13 @@ double PosCtl::filteredvalue(double old_value, double new_value, double dt, doub
* Loop, if new data is aviable{
*/
void PosCtl::Loop (int pos_x, int pos_y, double dt) {
position_f_2d p;
pos.x = PosCtl::filteredvalue(pos.x, pos_x, dt, posfilter);
pos.y = PosCtl::filteredvalue(pos.y, pos_y, dt, posfilter);
p.x = pos.x; p.y = pos.y;
axis_pv[AXIS_RA] = calib_axis2_v.perpendicular(p, target_pos);
axis_pv[AXIS_DEC] = calib_axis1_v.perpendicular(p, target_pos);
#ifdef DEBUG_POSCTL
static int lastmode = -1;
@ -974,17 +1097,11 @@ void PosCtl::Loop (int pos_x, int pos_y, double dt) {
// dist_axis1 --> will control pid_axis2
// dist_axis2 --> will control pis_axis1
else if (mode == POSCTL_MODE_CONTROL) {
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 = pos.x; p.y = pos.y;
axis_pv[AXIS_RA] = calib_axis2_v.perpendicular(p, target_pos);
axis_pv[AXIS_DEC] = calib_axis1_v.perpendicular(p, target_pos);
axis_op[AXIS_RA] = pid_axis[AXIS_RA].Update(0.0, axis_pv[AXIS_RA]);
axis_op[AXIS_DEC] = pid_axis[AXIS_DEC].Update(0.0, axis_pv[AXIS_DEC]);
@ -1008,6 +1125,14 @@ void PosCtl::Loop (int pos_x, int pos_y, double dt) {
NotifyGtk();
}
else if (mode == POSCTL_MODE_2STEP || mode == POSCTL_MODE_2STEPINIT) {
LockMutex();
NotifyGtk2Step();
UnLockMutex();
}
else {
LockMutex();
target_pos.x = pos.x;
@ -1024,6 +1149,108 @@ void PosCtl::Loop (int pos_x, int pos_y, double dt) {
};
/*
*
*/
void PosCtl::Reset2Step() {
LockMutex();
mode = POSCTL_MODE_OFF;
for (int i = 0; i < 2; i++) {
npv[i] = NAN;
ppv[i] = NAN;
nop[i] = NAN;
pop[i] = NAN;
}
posctl_2step_gui_update(axis_pv, axis_op, npv, nop, ppv, pop);
UnLockMutex();
NotifyGtk();
};
/*
* set current pos as target
* if npv and ppv == NAN set min pos for both axes
* min and max values will be read from the PID objects
*/
void PosCtl::Start2Step() {
int found1 = 0, found2 = 0, i;
LockMutex();
for (i = 0, found2 = 0, found1 = 0; i < 2; i++) {
if (isnan(npv[i]) && isnan(ppv[i])) found1 = 1;
if (isnan(npv[i]) || isnan(ppv[i])) found2 = 1;
}
if (found1) {
mode = POSCTL_MODE_2STEPINIT;
for (i = 0; i < 2; i++) {
axis_op[i] = pid_axis[1-i].GetMin();
OutputWriteValue(i, axis_op[i]);
}
}
else if (found2) {
for (i = 0; i < 2; i++) {
axis_op[i] = pid_axis[1-i].GetMax();
OutputWriteValue(i, axis_op[i]);
}
mode = POSCTL_MODE_2STEPINIT;
}
else mode = POSCTL_MODE_2STEP;
target_pos = pos;
printf ("%s:%d %s axis_op: (%f %f)\n", __FILE__, __LINE__, __FUNCTION__, axis_op[0], axis_op[1]);
posctl_2step_gui_update(axis_pv, axis_op, npv, nop, ppv, pop);
UnLockMutex();
};
/*
* with each cycle, caclulate an output between p and n values.
*/
void PosCtl::Stop2Step() {
double axisnew_op[2] = { NAN, NAN };
int i;
LockMutex();
//
// check if we got better values
for (int i = 0; i < 2; i++) {
if (axis_pv[i] < 0.0 && (isnan(npv[i]) || axis_pv[i] > npv[i])) {
npv[i] = axis_pv[i];
nop[i] = axis_op[i];
}
if (axis_pv[i] > 0.0 && (isnan(ppv[i]) || axis_pv[i] < ppv[i])) {
ppv[i] = axis_pv[i];
pop[i] = axis_op[i];
}
}
// need to calculate new step
if (mode == POSCTL_MODE_2STEP) {
for (i = 0; i < 2; i++) {
axisnew_op[i] = (nop[i] + pop[i]) / 2.0;
OutputWriteValue(i, axisnew_op[i]);
}
}
printf ("%s:%d %s axis \t_op:\t%f\t%f \t_pv:\t%f\t%f \t new:\t%f\t%f\n", __FILE__, __LINE__, __FUNCTION__,
axis_op[0], axis_op[1], axis_pv[0], axis_pv[1], axisnew_op[0], axisnew_op[1]);
mode = POSCTL_MODE_OFF;
posctl_2step_gui_update(axis_pv, axis_op, npv, nop, ppv, pop);
for (i = 0; i < 2; i++) axis_op[i] = axisnew_op[i];
UnLockMutex();
};
void PosCtl::SetDevice (std::string d) {
printf ("%s:%d %s new device:%s\n", __FILE__, __LINE__, __FUNCTION__, d.c_str());
@ -1129,7 +1356,6 @@ int PosCtl::ReadTTY (char * inbuf, int length) {
}
else {
inbuf[len] = 0;
//printf ("%s:%d %s receive: '%s'\n", __FILE__, __LINE__, __FUNCTION__, inbuf);
}
#else
ssize_t len;
@ -1168,7 +1394,6 @@ int PosCtl::ReadTTY (char * inbuf, int length) {
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_type == POSCTL_DEVTYPE_SIM) {
simulation.AxisSetValue(axis, value);

4
simpleskycam.h
Unescape View File

@ -9,8 +9,8 @@
extern void errorexit(char *fmt, ...);
extern void calc_vec2anglelen(position_f_2d *p, vector_2d *v);
#define sindeg(_angle_) sin((M_PI * _angle_ / 180.0))
#define cosdeg(_angle_) cos((M_PI * _angle_ / 180.0))
#define sindeg(_angle_) sin((M_PI * (_angle_) / 180.0))
#define cosdeg(_angle_) cos((M_PI * (_angle_) / 180.0))
#endif

2685
simpleskycam.ui
View File

File diff suppressed because it is too large Load Diff
Write Preview
Loading…
Cancel
Save