|
|
@ -639,7 +639,7 @@ void PosCtl::CalibModeStart(int x, int y) {
|
|
|
|
calib_pos.y = y;
|
|
|
|
calib_pos.y = y;
|
|
|
|
|
|
|
|
|
|
|
|
calib_mode = POSCTL_CALIB_MODE_DELTA;
|
|
|
|
calib_mode = POSCTL_CALIB_MODE_DELTA;
|
|
|
|
OutputWriteStop();
|
|
|
|
if (OutputWriteStop()) mode = POSCTL_MODE_OFF;
|
|
|
|
gettimeofday (&calib_timestamp, NULL);
|
|
|
|
gettimeofday (&calib_timestamp, NULL);
|
|
|
|
gdk_threads_add_idle(cb_thread_posctl, NULL);
|
|
|
|
gdk_threads_add_idle(cb_thread_posctl, NULL);
|
|
|
|
};
|
|
|
|
};
|
|
|
@ -671,7 +671,7 @@ void PosCtl::CalibModeDelta(int x, int y) {
|
|
|
|
calib_pos.x = x;
|
|
|
|
calib_pos.x = x;
|
|
|
|
calib_pos.y = y;
|
|
|
|
calib_pos.y = y;
|
|
|
|
|
|
|
|
|
|
|
|
OutputWriteValue(0, CALIB_MAXSPEED);
|
|
|
|
if (OutputWriteValue(0, CALIB_MAXSPEED)) mode = POSCTL_MODE_OFF;
|
|
|
|
|
|
|
|
|
|
|
|
gettimeofday (&calib_timestamp, NULL);
|
|
|
|
gettimeofday (&calib_timestamp, NULL);
|
|
|
|
gdk_threads_add_idle(cb_thread_posctl, NULL);
|
|
|
|
gdk_threads_add_idle(cb_thread_posctl, NULL);
|
|
|
@ -696,13 +696,13 @@ void PosCtl::CalibModeAxis(int x, int y) {
|
|
|
|
if (calib_mode == POSCTL_CALIB_MODE_AXIS1) {
|
|
|
|
if (calib_mode == POSCTL_CALIB_MODE_AXIS1) {
|
|
|
|
calib_axis1_v = fp - calib_rot_v;
|
|
|
|
calib_axis1_v = fp - calib_rot_v;
|
|
|
|
calc_vec2anglelen(&calib_axis1_v, &calib_axis1);
|
|
|
|
calc_vec2anglelen(&calib_axis1_v, &calib_axis1);
|
|
|
|
OutputWriteStop();
|
|
|
|
if (OutputWriteStop()) mode = POSCTL_MODE_OFF;
|
|
|
|
OutputWriteValue(1, CALIB_MAXSPEED);
|
|
|
|
else if (OutputWriteValue(1, CALIB_MAXSPEED)) mode = POSCTL_MODE_OFF;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
else if (calib_mode == POSCTL_CALIB_MODE_AXIS2) {
|
|
|
|
else if (calib_mode == POSCTL_CALIB_MODE_AXIS2) {
|
|
|
|
calib_axis2_v = fp - calib_rot_v;
|
|
|
|
calib_axis2_v = fp - calib_rot_v;
|
|
|
|
calc_vec2anglelen(&calib_axis2_v, &calib_axis2);
|
|
|
|
calc_vec2anglelen(&calib_axis2_v, &calib_axis2);
|
|
|
|
OutputWriteStart();
|
|
|
|
if (OutputWriteStart()) mode = POSCTL_MODE_OFF;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
calib_mode++;
|
|
|
|
calib_mode++;
|
|
|
@ -774,8 +774,8 @@ void PosCtl::Loop (int posx, int posy) {
|
|
|
|
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]);
|
|
|
|
|
|
|
|
|
|
|
|
OutputWriteValue(0, axis_op[0]);
|
|
|
|
if (OutputWriteValue(0, axis_op[0]) || OutputWriteValue(1, axis_op[1]))
|
|
|
|
OutputWriteValue(1, axis_op[1]);
|
|
|
|
mode = POSCTL_MODE_OFF;
|
|
|
|
|
|
|
|
|
|
|
|
axis_history_add(0, axis_pv[0], axis_op[0]);
|
|
|
|
axis_history_add(0, axis_pv[0], axis_op[0]);
|
|
|
|
axis_history_add(1, axis_pv[1], axis_op[1]);
|
|
|
|
axis_history_add(1, axis_pv[1], axis_op[1]);
|
|
|
|