static int8_t motor_update() {
uint8_t steps = 0;
/* Motion along axes X and Y takes precedence over motion along axis Z, when
* no submersion is requested on the latter. Also, only motion along axes X
* and Y may be combined with one another. */
if((new_pos.x != cur_pos.x || new_pos.y != cur_pos.y)
&& new_pos.z <= cur_pos.z) {
/* Relative offset from #cur_pos on axes X and Y. */
int16_t rel_x = (int16_t)new_pos.x - (int16_t)cur_pos.x;
int16_t rel_y = (int16_t)new_pos.y - (int16_t)cur_pos.y;
/* Enable lines for PWM propagation to motor Y and set its speed. */
if(rel_y) {
setup_axis(AXIS_Y, rel_y); /* @c rel_y is used only for its sign. */
}
/* Enable lines for PWM propagation to motor X and set its speed. Motor
* X may only be set-up after motor Y, as noted in #setup_axis(). */
if(rel_x) {
setup_axis(AXIS_X, rel_x); /* @c rel_x is used only for its sign. */
}
rel_x = abs(rel_x);
rel_y = abs(rel_y);
if(rel_x > 0 && rel_y > 0) {
/* Request the common amount of steps between @c rel_x and
* @c rel_y. */
steps = rel_x >= rel_y ? GRID_TO_STEP(rel_y)
: GRID_TO_STEP(rel_x);
} else {
/* If one of @c rel_x and @c rel_y is @c 0, then calculate the
* amount of steps solely based on the other (non-zero) value. */
steps = rel_x > 0 ? GRID_TO_STEP(rel_x)
: GRID_TO_STEP(rel_y);
}
/* Configure motion along axis Z. */
} else if(new_pos.z != cur_pos.z) {
int16_t rel_z = (int16_t)new_pos.z - cur_pos.z;
setup_axis(AXIS_Z, rel_z); /* @c rel_z is used only for its sign. */
steps = GRID_TO_STEP(abs(rel_z));
motor_status |= _BV(MTR_IS_Z);
} else {
/* Already at #new_pos. */
motor_status &= ~_BV(MTR_IS_Z);
return -1;
}
setup_lock(steps);
motor_start();
return 0;
}
void motor_reset() {
/* Begin with resetting axis Z if no reset is already in progress. It is
* imperative to first retract on axis Z separately from the others. Once
* that has been dealt with, axes X and Y may follow. */
if(bit_is_clear(motor_status, MTR_RESET)) {
motor_stop();
motor_status |= _BV(MTR_RESET) | _BV(MTR_IS_Z);
setup_axis(AXIS_Z, MTR_INC);
LOCK_DISABLE();
motor_start();
/* Axis Z has been reset. Now, axes X and Y may follow. */
} else if(bit_is_set(motor_status, MTR_RESET_Z_DONE)) {
/* Remove flag denoting axis Z is resetting. */
motor_status &= ~(_BV(MTR_IS_Z) | _BV(MTR_RESET_Z_DONE));
/* Reset axes X and Y. */
setup_axis(AXIS_Y, MTR_DEC);
setup_axis(AXIS_X, MTR_DEC);
LOCK_DISABLE(); /* Manually enable PWM propagation. */
motor_start();
/* All resetting stages have been completed. Reset #cur_pos and flags. */
} else if(bit_is_set(motor_status, MTR_RESET_X_DONE)
&& bit_is_set(motor_status, MTR_RESET_Y_DONE)) {
cur_pos.x = 0;
cur_pos.y = 0;
cur_pos.z = max_pos.z - 1;
motor_stop();
LOCK_ENABLE();
/* If this resetting cycle was initiated as a response to a limit being
* engaged while under normal motor operation, retry reaching #new_pos
* anew. */
if(bit_is_set(motor_status, MTR_LIMIT)) {
if(motor_update()) {
motor_stop();
MTR_CALL(cur_pos, MTR_EVT_OK);
}
} else {
new_pos = cur_pos;
MTR_CALL(cur_pos, MTR_EVT_OK);
}
motor_status &= ~(_BV(MTR_RESET) | _BV(MTR_LIMIT)
| _BV(MTR_RESET_X_DONE) | _BV(MTR_RESET_Y_DONE));
motor_status |= _BV(MTR_IS_RST_FRESH);
/* Reset is in progress. */
} else {
}
}
/* new concave face */
void new_concave_face (struct surface *this_srf, struct probe *prb)
{
int k;
double probe_center[3];
double vertex1_coor[3], vertex2_coor[3], vertex3_coor[3], vertex4_coor[3];
double circle1_axis[3], circle2_axis[3], circle3_axis[3], circle4_axis[3];
struct circle *circle1, *circle2, *circle3, *circle4;
struct vertex *vertex1, *vertex2, *vertex3, *vertex4;
struct arc *arc1, *arc2, *arc3, *arc4;
struct sphere *atom1, *atom2, *atom3, *atom4;
struct face *concave_fac;
struct pair *torus1, *torus2, *torus3, *torus4;
double dot1, dot2, dot3, dot4;
char message[MAX_STRING];
struct cept *ex;
if (prb -> natom > 3) {
sprintf (message, "%8ld probe square concave face", prb -> number);
informd (message);
}
else {
sprintf (message, "%8ld probe triangular concave face", prb -> number);
informd2 (message);
}
atom1 = prb -> atm[0];
atom2 = prb -> atm[1];
atom3 = prb -> atm[2];
atom4 = prb -> atm[3];
torus1 = prb -> pairs[0];
torus2 = prb -> pairs[1];
torus3 = prb -> pairs[2];
torus4 = prb -> pairs[3];
for (k = 0; k < 3; k++)
probe_center[k] = prb -> center[k];
/* compute vertex coordinates */
for (k = 0; k < 3; k++) {
vertex1_coor[k] = (atom1 -> radius * probe_center[k] +
this_srf -> probe_radius * atom1 -> center[k]) /
(atom1 -> radius + this_srf -> probe_radius);
vertex2_coor[k] = (atom2 -> radius * probe_center[k] +
this_srf -> probe_radius * atom2 -> center[k]) /
(atom2 -> radius + this_srf -> probe_radius);
vertex3_coor[k] = (atom3 -> radius * probe_center[k] +
this_srf -> probe_radius * atom3 -> center[k]) /
(atom3 -> radius + this_srf -> probe_radius);
if (atom4 != NULL) {
vertex4_coor[k] = (atom4 -> radius * probe_center[k] +
this_srf -> probe_radius * atom4 -> center[k]) /
(atom4 -> radius + this_srf -> probe_radius);
}
}
/* set up vertices */
vertex1 = new_vertex (vertex1_coor, (struct sphere *) atom1, prb, NULL, NULL);
if (vertex1 == NULL) return;
link_vertex (this_srf, vertex1);
vertex2 = new_vertex (vertex2_coor, (struct sphere *) atom2, prb, NULL, NULL);
if (vertex2 == NULL) return;
link_vertex (this_srf, vertex2);
vertex3 = new_vertex (vertex3_coor, (struct sphere *) atom3, prb, NULL, NULL);
if (vertex3 == NULL) return;
link_vertex (this_srf, vertex3);
if (atom4 != NULL) {
vertex4 = new_vertex (vertex4_coor, (struct sphere *) atom4, prb, NULL, NULL);
if (vertex4 == NULL) return;
link_vertex (this_srf, vertex4);
}
else vertex4 = NULL;
/* calculate axes and set up circles */
setup_axis (probe_center, prb -> atm[0] -> center,
prb -> atm[1] -> center, circle1_axis);
setup_axis (probe_center, prb -> atm[1] -> center,
prb -> atm[2] -> center, circle2_axis);
if (atom4 == NULL) {
setup_axis (probe_center, prb -> atm[2] -> center,
prb -> atm[0] -> center, circle3_axis);
}
else if (atom4 != NULL) {
setup_axis (probe_center, prb -> atm[2] -> center,
prb -> atm[3] -> center, circle3_axis);
setup_axis (probe_center, prb -> atm[3] -> center,
prb -> atm[0] -> center, circle4_axis);
}
circle1 = new_circle (probe_center, this_srf -> probe_radius, circle1_axis);
if (circle1 == NULL) return;
link_circle (this_srf, circle1);
circle1 -> theta = 0.0;
circle1 -> subtype = GREAT_SUBTYPE;
circle2 = new_circle (probe_center, this_srf -> probe_radius, circle2_axis);
if (circle2 == NULL) return;
link_circle (this_srf, circle2);
circle2 -> theta = 0.0;
circle2 -> subtype = GREAT_SUBTYPE;
circle3 = new_circle (probe_center, this_srf -> probe_radius, circle3_axis);
if (circle3 == NULL) return;
link_circle (this_srf, circle3);
circle3 -> theta = 0.0;
circle3 -> subtype = GREAT_SUBTYPE;
if (atom4 != NULL) {
circle4 = new_circle (probe_center, this_srf -> probe_radius, circle4_axis);
if (circle4 == NULL) return;
link_circle (this_srf, circle4);
circle4 -> theta = 0.0;
circle4 -> subtype = GREAT_SUBTYPE;
}
else circle4 = NULL;
/* set up arcs */
arc1 = new_arc (circle1, vertex1, vertex2, CONCAVE, 0, (double) 0.0, 0L, 0L, 0L);
if (arc1 == NULL) return;
this_srf -> n_arc++;
arc2 = new_arc (circle2, vertex2, vertex3, CONCAVE, 0, (double) 0.0, 0L, 0L, 0L);
if (arc2 == NULL) return;
this_srf -> n_arc++;
if (circle4 == NULL) {
arc3 = new_arc (circle3, vertex3, vertex1, CONCAVE, 0, (double) 0.0, 0L, 0L, 0L);
if (arc3 == NULL) return;
this_srf -> n_arc++;
arc4 = NULL;
}
else if (circle4 != NULL) {
arc3 = new_arc (circle3, vertex3, vertex4, CONCAVE, 0, (double) 0.0, 0L, 0L, 0L);
if (arc3 == NULL) return;
this_srf -> n_arc++;
arc4 = new_arc (circle4, vertex4, vertex1, CONCAVE, 0, (double) 0.0, 0L, 0L, 0L);
if (arc4 == NULL) return;
this_srf -> n_arc++;
}
/* add arcs to tori */
arc1 -> next = torus1 -> first_arc;
torus1 -> first_arc = arc1;
sprintf (message, "%8ld %8ld torus: add arc",
torus1 -> sph[0] -> number, torus1 -> sph[1] -> number);
informd2(message);
arc2 -> next = torus2 -> first_arc;
torus2 -> first_arc = arc2;
sprintf (message, "%8ld %8ld torus: add arc",
torus2 -> sph[0] -> number, torus2 -> sph[1] -> number);
informd2(message);
arc3 -> next = torus3 -> first_arc;
torus3 -> first_arc = arc3;
sprintf (message, "%8ld %8ld torus: add arc",
torus3 -> sph[0] -> number, torus3 -> sph[1] -> number);
informd2(message);
if (torus4 != NULL) {
arc4 -> next = torus4 -> first_arc;
torus4 -> first_arc = arc4;
sprintf (message, "%8ld %8ld torus: add arc",
torus4 -> sph[0] -> number, torus4 -> sph[1] -> number);
informd(message);
}
/* new concave face */
concave_fac = new_face (NULL, CONCAVE);
if (concave_fac == NULL) return;
concave_fac -> ptr.prb = prb;
concave_fac -> chi = 1;
link_face (this_srf, concave_fac);
add2face (concave_fac, arc1, arc2, arc3, arc4);
/* back pointer for cusp corrections */
prb -> fac = concave_fac;
arc1 -> fac = concave_fac;
arc2 -> fac = concave_fac;
arc3 -> fac = concave_fac;
if (arc4 != NULL) {
arc4 -> fac = concave_fac;
}
}
