Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-02-06 11:11:57 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Passes all get/set regressions. Fixed type coercion on GETS (not supposed to happen); Failing digital inputs for some reason; re-organized cm_set_tram() to use the nv->value_int boolean value

Browse Source
This commit is contained in:
Alden Hart
2017-03-20 09:23:54 -04:00
parent dfbcc3f050
commit 2a7a65efa5
6 changed files with 87 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
g2core/alarm.cpp
View File

@@ -235,7 +235,6 @@ stat_t cm_panic(const stat_t status, const char *msg)
spindle_reset(); // stop spindle immediately and set speed to 0 RPM
coolant_reset(); // stop coolant immediately
temperature_reset(); // turn off heaters and fans
//+++++ cm_queue_flush(&cm1); // flush all queues and reset positions
cm1.machine_state = MACHINE_PANIC; // don't reset anything. Panics are not recoverable
cm2.machine_state = MACHINE_PANIC; // don't reset anything. Panics are not recoverable

173
g2core/canonical_machine.cpp
View File

@@ -540,104 +540,93 @@ stat_t cm_deferred_write_callback()
stat_t cm_set_tram(nvObj_t *nv)
{
// Ah yes. If you call {tram:true} or any non-zero it will tram. Anything falsey will clear the tram.
if ((nv->valuetype == TYPE_BOOLEAN) || (nv->valuetype == TYPE_INTEGER) || (nv->valuetype == TYPE_FLOAT)) {
bool do_set = !!(nv->value_int);
// if passed false/0, we will clear the rotation matrix
if (!do_set) {
canonical_machine_reset_rotation(cm);
return (STAT_OK);
}
// check to make sure we have three valid probes in a row
if ((cm->probe_state[0] == PROBE_SUCCEEDED) &&
(cm->probe_state[1] == PROBE_SUCCEEDED) &&
(cm->probe_state[2] == PROBE_SUCCEEDED))
{
// Step 1: Get the normal of the plane formed by the three probes. Naming:
// d0_{xyz} is the delta between point 0 and point 1
// d2_{xyz} is the delta between point 2 and point 1
// n_{xyz} is the unit normal
// Step 1a: get the deltas
float d0_x = cm->probe_results[0][0] - cm->probe_results[1][0];
float d0_y = cm->probe_results[0][1] - cm->probe_results[1][1];
float d0_z = cm->probe_results[0][2] - cm->probe_results[1][2];
float d2_x = cm->probe_results[2][0] - cm->probe_results[1][0];
float d2_y = cm->probe_results[2][1] - cm->probe_results[1][1];
float d2_z = cm->probe_results[2][2] - cm->probe_results[1][2];
// Step 1b: compute the combined magnitude
// since sqrt(a)*sqrt(b) = sqrt(a*b), we can save a sqrt in making the unit normal
float combined_magnitude_inv = 1.0/sqrt((d0_x*d0_x + d0_y*d0_y + d0_z*d0_z) *
(d2_x*d2_x + d2_y*d2_y + d2_z*d2_z));
// Step 1c: compute the cross product and normalize
float n_x = (d0_z*d2_y - d0_y*d2_z)*combined_magnitude_inv;
float n_y = (d0_x*d2_z - d0_z*d2_x)*combined_magnitude_inv;
float n_z = (d0_y*d2_x - d0_x*d2_y)*combined_magnitude_inv;
// Step 1d: flip the normal if it's negative
if (n_z < 0.0) {
n_x = -n_x;
n_y = -n_y;
n_z = -n_z;
}
// Step 2: make the quaternion for the rotation to {0,0,1}
float p = sqrt(n_x*n_x + n_y*n_y + n_z*n_z);
float m = sqrt(2.0)*sqrt(p*(p+n_z));
float q_w = (n_z + p) / m;
float q_x = -n_y / m;
float q_y = n_x / m;
//float q_z = 0; // already optimized out
// Step 3: compute the rotation matrix
float q_wx_2 = q_w * q_x * 2.0;
float q_wy_2 = q_w * q_y * 2.0;
float q_xx_2 = q_x * q_x * 2.0;
float q_xy_2 = q_x * q_y * 2.0;
float q_yy_2 = q_y * q_y * 2.0;
/*
matrix = {
{1 - q_yy_2, q_xy_2, q_wy_2, 0},
{q_xy_2, 1 - q_xx_2, -q_wx_2, 0},
{-q_wy_2, q_wx_2, 1 - q_xx_2 - q_yy_2, i},
{0, 0, 0, 1}
}
*/
cm->rotation_matrix[0][0] = 1 - q_yy_2;
cm->rotation_matrix[0][1] = q_xy_2;
cm->rotation_matrix[0][2] = q_wy_2;
cm->rotation_matrix[1][0] = q_xy_2;
cm->rotation_matrix[1][1] = 1 - q_xx_2;
cm->rotation_matrix[1][2] = -q_wx_2;
cm->rotation_matrix[2][0] = -q_wy_2;
cm->rotation_matrix[2][1] = q_wx_2;
cm->rotation_matrix[2][2] = 1 - q_xx_2 - q_yy_2;
// Step 4: compute the z-offset
cm->rotation_z_offset = (n_x*cm->probe_results[1][0] +
n_y*cm->probe_results[1][1]) /
n_z + cm->probe_results[1][2];
} else {
return (STAT_COMMAND_NOT_ACCEPTED);
}
} else {
return (STAT_INPUT_VALUE_RANGE_ERROR);
if (!nv->value_int) { // if false, reset the matrix and return
canonical_machine_reset_rotation(cm);
return (STAT_OK);
}
// check to make sure we have three valid probes in a row
if (!((cm->probe_state[0] == PROBE_SUCCEEDED) &&
(cm->probe_state[1] == PROBE_SUCCEEDED) &&
(cm->probe_state[2] == PROBE_SUCCEEDED))) {
return (STAT_COMMAND_NOT_ACCEPTED); // do not have 3 valid probes
}
// Step 1: Get the normal of the plane formed by the three probes. Naming:
// d0_{xyz} is the delta between point 0 and point 1
// d2_{xyz} is the delta between point 2 and point 1
// n_{xyz} is the unit normal
// Step 1a: get the deltas
float d0_x = cm->probe_results[0][0] - cm->probe_results[1][0];
float d0_y = cm->probe_results[0][1] - cm->probe_results[1][1];
float d0_z = cm->probe_results[0][2] - cm->probe_results[1][2];
float d2_x = cm->probe_results[2][0] - cm->probe_results[1][0];
float d2_y = cm->probe_results[2][1] - cm->probe_results[1][1];
float d2_z = cm->probe_results[2][2] - cm->probe_results[1][2];
// Step 1b: compute the combined magnitude
// since sqrt(a)*sqrt(b) = sqrt(a*b), we can save a sqrt in making the unit normal
float combined_magnitude_inv = 1.0/sqrt((d0_x*d0_x + d0_y*d0_y + d0_z*d0_z) *
(d2_x*d2_x + d2_y*d2_y + d2_z*d2_z));
// Step 1c: compute the cross product and normalize
float n_x = (d0_z*d2_y - d0_y*d2_z)*combined_magnitude_inv;
float n_y = (d0_x*d2_z - d0_z*d2_x)*combined_magnitude_inv;
float n_z = (d0_y*d2_x - d0_x*d2_y)*combined_magnitude_inv;
// Step 1d: flip the normal if it's negative
if (n_z < 0.0) {
n_x = -n_x;
n_y = -n_y;
n_z = -n_z;
}
// Step 2: make the quaternion for the rotation to {0,0,1}
float p = sqrt(n_x*n_x + n_y*n_y + n_z*n_z);
float m = sqrt(2.0)*sqrt(p*(p+n_z));
float q_w = (n_z + p) / m;
float q_x = -n_y / m;
float q_y = n_x / m;
//float q_z = 0; // already optimized out
// Step 3: compute the rotation matrix
float q_wx_2 = q_w * q_x * 2.0;
float q_wy_2 = q_w * q_y * 2.0;
float q_xx_2 = q_x * q_x * 2.0;
float q_xy_2 = q_x * q_y * 2.0;
float q_yy_2 = q_y * q_y * 2.0;
/*
matrix = {
{1 - q_yy_2, q_xy_2, q_wy_2, 0},
{q_xy_2, 1 - q_xx_2, -q_wx_2, 0},
{-q_wy_2, q_wx_2, 1 - q_xx_2 - q_yy_2, i},
{0, 0, 0, 1}
}
*/
cm->rotation_matrix[0][0] = 1 - q_yy_2;
cm->rotation_matrix[0][1] = q_xy_2;
cm->rotation_matrix[0][2] = q_wy_2;
cm->rotation_matrix[1][0] = q_xy_2;
cm->rotation_matrix[1][1] = 1 - q_xx_2;
cm->rotation_matrix[1][2] = -q_wx_2;
cm->rotation_matrix[2][0] = -q_wy_2;
cm->rotation_matrix[2][1] = q_wx_2;
cm->rotation_matrix[2][2] = 1 - q_xx_2 - q_yy_2;
// Step 4: compute the z-offset
cm->rotation_z_offset = (n_x*cm->probe_results[1][0] +
n_y*cm->probe_results[1][1]) /
n_z + cm->probe_results[1][2];
return (STAT_OK);
}
stat_t cm_get_tram(nvObj_t *nv)
{
nv->value_int = true;
nv->value_int = true; // believe it or not, the compiler likes this form best - most efficient code
if (fp_NOT_ZERO(cm->rotation_z_offset) ||
fp_NOT_ZERO(cm->rotation_matrix[0][1]) ||

7
g2core/config.cpp
View File

@@ -214,7 +214,6 @@ stat_t get_nul(nvObj_t *nv)
stat_t get_ui8(nvObj_t *nv)
{
// nv->value = (float)*((uint8_t *)GET_TABLE_WORD(target));
nv->value_int = *((uint8_t *)GET_TABLE_WORD(target));
nv->valuetype = TYPE_INTEGER;
return (STAT_OK);
@@ -222,7 +221,6 @@ stat_t get_ui8(nvObj_t *nv)
stat_t get_int8(nvObj_t *nv)
{
// nv->value = (float)*((int8_t *)GET_TABLE_WORD(target));
nv->value_int = *((int8_t *)GET_TABLE_WORD(target));
nv->valuetype = TYPE_INTEGER;
return (STAT_OK);
@@ -230,7 +228,6 @@ stat_t get_int8(nvObj_t *nv)
stat_t get_int32(nvObj_t *nv)
{
// nv->value = *((uint32_t *)GET_TABLE_WORD(target));
nv->value_int = *((uint32_t *)GET_TABLE_WORD(target));
nv->valuetype = TYPE_INTEGER;
return (STAT_OK);
@@ -340,7 +337,6 @@ stat_t set_0123(nvObj_t *nv)
stat_t set_int32(nvObj_t *nv)
{
// *((uint32_t *)GET_TABLE_WORD(target)) = (uint32_t)nv->value; +++++
*((uint32_t *)GET_TABLE_WORD(target)) = nv->value_int;
nv->valuetype = TYPE_INTEGER;
return(STAT_OK);
@@ -508,6 +504,9 @@ uint8_t nv_get_type(nvObj_t *nv)
*/
void nv_coerce_types(nvObj_t *nv)
{
if (nv->valuetype == TYPE_NULL) { // don't change type if it's a GET query
return;
}
valueType type = (valueType)(cfgArray[nv->index].flags & F_TYPE_MASK);
if (type == TYPE_INTEGER) {
nv->valuetype = TYPE_INTEGER; // will pay attention to the int value, not the float

1
g2core/cycle_probing.cpp
View File

@@ -213,7 +213,6 @@ static stat_t _probe_move(const float target[], const bool flags[])
pb.waiting_for_motion_complete = true; // set this BEFORE the motion starts
cm_straight_feed(target, flags, PROFILE_FAST); // NB: feed rate was set earlier, so it's OK
mp_queue_command(_motion_end_callback, nullptr, nullptr); // the last two arguments are ignored anyway
// st_request_forward_plan(); //+++++
return (STAT_EAGAIN);
}

2
g2core/json_parser.cpp
View File

@@ -169,7 +169,7 @@ static stat_t _json_parser_kernal(nvObj_t *nv, char *str)
if ((nv_index_is_group(nv->index)) && (nv_group_is_prefixed(nv->token))) {
strncpy(group, nv->token, GROUP_LEN); // record the group ID
}
nv_coerce_types(nv); // set final types following _get_nv_pair
nv_coerce_types(nv); // adjust types based on type fields in configApp table
if ((nv = nv->nx) == NULL) {
return (STAT_JSON_TOO_MANY_PAIRS); // Not supposed to encounter a NULL
}

4
g2core/text_parser.cpp
View File

@@ -133,8 +133,8 @@ static stat_t _text_parser_kernal(char *str, nvObj_t *nv)
return (STAT_UNRECOGNIZED_NAME);
}
strcpy(nv->group, cfgArray[nv->index].group); // capture the group string if there is one
nv_coerce_types(nv); // adjust types based on type fields in configApp table
nv_coerce_types(nv); // adjust types based on type fields in configApp table
// see if you need to strip the token - but only if in text mode
if ((cs.comm_request_mode == TEXT_MODE) && (nv_group_is_prefixed(nv->group))) {
wr = nv->token;