Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-09-23 21:38:54 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Moved MRs under the planner; changed the inits and mr references for this to work. Still have testing and cleanup to go.

Browse Source
This commit is contained in:
Alden Hart
2017-01-10 09:06:39 -05:00
parent ddf32df2fd
commit 6a541f51ca
9 changed files with 407 additions and 436 deletions
Download Patch File Download Diff File Expand all files Collapse all files
g2core/canonical_machine.cpp
+10 -10
View File
@@ -226,7 +226,7 @@ cmCombinedState cm_get_combined_state()
/* These getters and setters will work on any gm model with inputs:
* MODEL (GCodeState_t *)&cm->gm // absolute pointer from canonical machine gm model
* PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to
* RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
* RUNTIME (GCodeState_t *)&mr->gm // absolute pointer from runtime mm struct
* ACTIVE_MODEL cm->am // active model pointer is maintained by state management
*/
uint32_t cm_get_linenum(const GCodeState_t *gcode_state) { return gcode_state->linenum;}
@@ -342,10 +342,10 @@ float cm_get_active_coord_offset(const uint8_t axis)
/*
* cm_get_work_offset() - return a coord offset from the gcode_state
*
* MODEL (GCodeState_t *)&cm->gm // absolute pointer from canonical machine gm model
* MODEL (GCodeState_t *)&cm->gm // absolute pointer from canonical machine gm model
* PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to
* RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
* ACTIVE_MODEL cm->am // active model pointer is maintained by state management
* RUNTIME (GCodeState_t *)&mr->gm // absolute pointer from runtime mm struct
* ACTIVE_MODEL cm->am // active model pointer is maintained by state management
*/
float cm_get_work_offset(const GCodeState_t *gcode_state, const uint8_t axis)
@@ -356,10 +356,10 @@ float cm_get_work_offset(const GCodeState_t *gcode_state, const uint8_t axis)
/*
* cm_set_work_offsets() - capture coord offsets from the model into absolute values in the gcode_state
*
* MODEL (GCodeState_t *)&cm->gm // absolute pointer from canonical machine gm model
* MODEL (GCodeState_t *)&cm->gm // absolute pointer from canonical machine gm model
* PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to
* RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
* ACTIVE_MODEL cm->am // active model pointer is maintained by state management
* RUNTIME (GCodeState_t *)&mr->gm // absolute pointer from runtime mm struct
* ACTIVE_MODEL cm->am // active model pointer is maintained by state management
*/
void cm_set_work_offsets(GCodeState_t *gcode_state)
@@ -374,7 +374,7 @@ void cm_set_work_offsets(GCodeState_t *gcode_state)
*
* This function accepts as input:
* MODEL (GCodeState_t *)&cm->gm // absolute pointer from canonical machine gm model
* RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
* RUNTIME (GCodeState_t *)&mr->gm // absolute pointer from runtime mm struct
*
* NOTE: Only MODEL and RUNTIME are supported (no PLANNER or bf's)
* NOTE: Machine position is always returned in mm mode. No units conversion is performed
@@ -398,7 +398,7 @@ float cm_get_absolute_position(const GCodeState_t *gcode_state, const uint8_t ax
*
* This function accepts as input:
* MODEL (GCodeState_t *)&cm->gm // absolute pointer from canonical machine gm model
* RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
* RUNTIME (GCodeState_t *)&mr->gm // absolute pointer from runtime mm struct
*
* NOTE: Only MODEL and RUNTIME are supported (no PLANNER or bf's)
*/
@@ -444,7 +444,7 @@ void cm_finalize_move()
void cm_update_model_position_from_runtime()
{
copy_vector(cm->gmx.position, mr.gm.target);
copy_vector(cm->gmx.position, mr->gm.target);
}
/*
g2core/canonical_machine.h
+1 -1
View File
@@ -40,7 +40,7 @@
#define MODEL (GCodeState_t *)&cm->gm // absolute pointer from canonical machine gm model
#define PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to
#define RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
#define RUNTIME (GCodeState_t *)&mr->gm // absolute pointer from runtime mm struct
#define ACTIVE_MODEL cm->am // active model pointer is maintained by state management
#define _to_millimeters(a) ((cm->gm.units_mode == INCHES) ? (a * MM_PER_INCH) : a)
g2core/config_app.cpp
+42 -42
View File
@@ -941,67 +941,67 @@ const cfgItem_t cfgArray[] = {
{ "", "clc",_f0, 0, tx_print_nul, st_clc, st_clc, (float *)&cs.null, 0 }, // clear diagnostic step counters
// { "", "_dam",_f0, 0, tx_print_nul, cm_dam, cm_dam, (float *)&cs.null, 0 }, // dump active model
{ "_te","_tex",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.target[AXIS_X], 0 }, // X target endpoint
{ "_te","_tey",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.target[AXIS_Y], 0 },
{ "_te","_tez",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.target[AXIS_Z], 0 },
{ "_te","_tea",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.target[AXIS_A], 0 },
{ "_te","_teb",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.target[AXIS_B], 0 },
{ "_te","_tec",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.target[AXIS_C], 0 },
{ "_te","_tex",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->target[AXIS_X], 0 }, // X target endpoint
{ "_te","_tey",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->target[AXIS_Y], 0 },
{ "_te","_tez",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->target[AXIS_Z], 0 },
{ "_te","_tea",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->target[AXIS_A], 0 },
{ "_te","_teb",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->target[AXIS_B], 0 },
{ "_te","_tec",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->target[AXIS_C], 0 },
{ "_tr","_trx",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.gm.target[AXIS_X], 0 }, // X target runtime
{ "_tr","_try",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.gm.target[AXIS_Y], 0 },
{ "_tr","_trz",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.gm.target[AXIS_Z], 0 },
{ "_tr","_tra",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.gm.target[AXIS_A], 0 },
{ "_tr","_trb",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.gm.target[AXIS_B], 0 },
{ "_tr","_trc",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.gm.target[AXIS_C], 0 },
{ "_tr","_trx",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->gm.target[AXIS_X], 0 }, // X target runtime
{ "_tr","_try",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->gm.target[AXIS_Y], 0 },
{ "_tr","_trz",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->gm.target[AXIS_Z], 0 },
{ "_tr","_tra",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->gm.target[AXIS_A], 0 },
{ "_tr","_trb",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->gm.target[AXIS_B], 0 },
{ "_tr","_trc",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->gm.target[AXIS_C], 0 },
#if (MOTORS >= 1)
{ "_ts","_ts1",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.target_steps[MOTOR_1], 0 }, // Motor 1 target steps
{ "_ps","_ps1",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.position_steps[MOTOR_1], 0 }, // Motor 1 position steps
{ "_cs","_cs1",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.commanded_steps[MOTOR_1], 0 }, // Motor 1 commanded steps (delayed steps)
{ "_es","_es1",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.encoder_steps[MOTOR_1], 0 }, // Motor 1 encoder steps
{ "_ts","_ts1",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->target_steps[MOTOR_1], 0 }, // Motor 1 target steps
{ "_ps","_ps1",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->position_steps[MOTOR_1], 0 }, // Motor 1 position steps
{ "_cs","_cs1",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->commanded_steps[MOTOR_1], 0 }, // Motor 1 commanded steps (delayed steps)
{ "_es","_es1",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->encoder_steps[MOTOR_1], 0 }, // Motor 1 encoder steps
{ "_xs","_xs1",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&st_pre.mot[MOTOR_1].corrected_steps, 0 }, // Motor 1 correction steps applied
{ "_fe","_fe1",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.following_error[MOTOR_1], 0 }, // Motor 1 following error in steps
{ "_fe","_fe1",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->following_error[MOTOR_1], 0 }, // Motor 1 following error in steps
#endif
#if (MOTORS >= 2)
{ "_ts","_ts2",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.target_steps[MOTOR_2], 0 },
{ "_ps","_ps2",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.position_steps[MOTOR_2], 0 },
{ "_cs","_cs2",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.commanded_steps[MOTOR_2], 0 },
{ "_es","_es2",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.encoder_steps[MOTOR_2], 0 },
{ "_ts","_ts2",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->target_steps[MOTOR_2], 0 },
{ "_ps","_ps2",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->position_steps[MOTOR_2], 0 },
{ "_cs","_cs2",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->commanded_steps[MOTOR_2], 0 },
{ "_es","_es2",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->encoder_steps[MOTOR_2], 0 },
{ "_xs","_xs2",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&st_pre.mot[MOTOR_2].corrected_steps, 0 },
{ "_fe","_fe2",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.following_error[MOTOR_2], 0 },
{ "_fe","_fe2",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->following_error[MOTOR_2], 0 },
#endif
#if (MOTORS >= 3)
{ "_ts","_ts3",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.target_steps[MOTOR_3], 0 },
{ "_ps","_ps3",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.position_steps[MOTOR_3], 0 },
{ "_cs","_cs3",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.commanded_steps[MOTOR_3], 0 },
{ "_es","_es3",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.encoder_steps[MOTOR_3], 0 },
{ "_ts","_ts3",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->target_steps[MOTOR_3], 0 },
{ "_ps","_ps3",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->position_steps[MOTOR_3], 0 },
{ "_cs","_cs3",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->commanded_steps[MOTOR_3], 0 },
{ "_es","_es3",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->encoder_steps[MOTOR_3], 0 },
{ "_xs","_xs3",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&st_pre.mot[MOTOR_3].corrected_steps, 0 },
{ "_fe","_fe3",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.following_error[MOTOR_3], 0 },
{ "_fe","_fe3",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->following_error[MOTOR_3], 0 },
#endif
#if (MOTORS >= 4)
{ "_ts","_ts4",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.target_steps[MOTOR_4], 0 },
{ "_ps","_ps4",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.position_steps[MOTOR_4], 0 },
{ "_cs","_cs4",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.commanded_steps[MOTOR_4], 0 },
{ "_es","_es4",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.encoder_steps[MOTOR_4], 0 },
{ "_ts","_ts4",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->target_steps[MOTOR_4], 0 },
{ "_ps","_ps4",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->position_steps[MOTOR_4], 0 },
{ "_cs","_cs4",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->commanded_steps[MOTOR_4], 0 },
{ "_es","_es4",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->encoder_steps[MOTOR_4], 0 },
{ "_xs","_xs4",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&st_pre.mot[MOTOR_4].corrected_steps, 0 },
{ "_fe","_fe4",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.following_error[MOTOR_4], 0 },
{ "_fe","_fe4",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->following_error[MOTOR_4], 0 },
#endif
#if (MOTORS >= 5)
{ "_ts","_ts5",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.target_steps[MOTOR_5], 0 },
{ "_ps","_ps5",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.position_steps[MOTOR_5], 0 },
{ "_cs","_cs5",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.commanded_steps[MOTOR_5], 0 },
{ "_es","_es5",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.encoder_steps[MOTOR_5], 0 },
{ "_ts","_ts5",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->target_steps[MOTOR_5], 0 },
{ "_ps","_ps5",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->position_steps[MOTOR_5], 0 },
{ "_cs","_cs5",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->commanded_steps[MOTOR_5], 0 },
{ "_es","_es5",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->encoder_steps[MOTOR_5], 0 },
{ "_xs","_xs6",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&st_pre.mot[MOTOR_5].corrected_steps, 0 },
{ "_fe","_fe5",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.following_error[MOTOR_5], 0 },
{ "_fe","_fe5",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->following_error[MOTOR_5], 0 },
#endif
#if (MOTORS >= 6)
{ "_ts","_ts6",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.target_steps[MOTOR_6], 0 },
{ "_ps","_ps6",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.position_steps[MOTOR_6], 0 },
{ "_cs","_cs6",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.commanded_steps[MOTOR_6], 0 },
{ "_es","_es6",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.encoder_steps[MOTOR_6], 0 },
{ "_ts","_ts6",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->target_steps[MOTOR_6], 0 },
{ "_ps","_ps6",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->position_steps[MOTOR_6], 0 },
{ "_cs","_cs6",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->commanded_steps[MOTOR_6], 0 },
{ "_es","_es6",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->encoder_steps[MOTOR_6], 0 },
{ "_xs","_xs5",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&st_pre.mot[MOTOR_6].corrected_steps, 0 },
{ "_fe","_fe6",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr.following_error[MOTOR_6], 0 },
{ "_fe","_fe6",_f0, 2, tx_print_flt, get_flt, set_ro, (float *)&mr->following_error[MOTOR_6], 0 },
#endif
#endif // __DIAGNOSTIC_PARAMETERS
g2core/main.cpp
+16 -17
View File
@@ -94,35 +94,34 @@ extern OutputPin<kDebug3_PinNumber> debug_pin3;
void application_init_services(void)
{
hardware_init(); // system hardware setup - must be first
persistence_init(); // set up EEPROM or other NVM - must be second
xio_init(); // xtended io subsystem - must be third
hardware_init(); // system hardware setup - must be first
persistence_init(); // set up EEPROM or other NVM - must be second
xio_init(); // xtended io subsystem - must be third
}
void application_init_machine(void)
{
cm = &cm0; // set global canonical machine pointer to primary machine
cm = &cm0; // set global canonical machine pointer to primary machine
cm->machine_state = MACHINE_INITIALIZING;
stepper_init(); // stepper subsystem
encoder_init(); // virtual encoders
gpio_init(); // inputs and outputs
pwm_init(); // pulse width modulation drivers
runtime_init(); // runtime execution structure
planner_init(&mp0); //+++++ // motion planning subsystem
// planner_init(&mp1); //+++++ // motion planning subsystem
canonical_machine_init(&cm0); //+++++ cleanup required // canonical machine
stepper_init(); // stepper subsystem
encoder_init(); // virtual encoders
gpio_init(); // inputs and outputs
pwm_init(); // pulse width modulation drivers
planner_init(&mp0, mp0_pool, &mr0); //+++++ //primary motion planner
// planner_init(&mp1, mp1_pool, &mr1); //+++++ //secondary motion planner
canonical_machine_init(&cm0); //+++++ cleanup required // canonical machine
// canonical_machine_init(&cm1); //+++++ cleanup required // canonical machine
}
void application_init_startup(void)
{
// start the application
controller_init(); // should be first startup init (requires xio_init())
config_init(); // apply the config settings from persistence
canonical_machine_reset(&cm1); // +++++
// canonical_machine_reset(&cm0); // +++++
spindle_init(); // should be after PWM and canonical machine inits and config_init()
controller_init(); // should be first startup init (requires xio_init())
config_init(); // apply the config settings from persistence
canonical_machine_reset(&cm0); // +++++
// canonical_machine_reset(&cm1); // +++++
spindle_init(); // should be after PWM and canonical machine inits and config_init()
spindle_reset();
temperature_init();
gpio_reset();
g2core/plan_exec.cpp
+170 -170
View File
File diff suppressed because it is too large Load Diff
g2core/plan_line.cpp
+12 -12
View File
@@ -74,10 +74,10 @@ static void _set_bf_diagnostics(mpBuf_t* bf) {
* that were in effect at move planning time
*/
void mp_zero_segment_velocity() { mr.segment_velocity = 0; }
float mp_get_runtime_velocity(void) { return (mr.segment_velocity); }
float mp_get_runtime_absolute_position(uint8_t axis) { return (mr.position[axis]); }
void mp_set_runtime_work_offset(float offset[]) { copy_vector(mr.gm.work_offset, offset); }
void mp_zero_segment_velocity() { mr->segment_velocity = 0; }
float mp_get_runtime_velocity(void) { return (mr->segment_velocity); }
float mp_get_runtime_absolute_position(uint8_t axis) { return (mr->position[axis]); }
void mp_set_runtime_work_offset(float offset[]) { copy_vector(mr->gm.work_offset, offset); }
// We have to handle rotation - "rotate" by the transverse of the matrix to got "normal" coordinates
float mp_get_runtime_work_position(uint8_t axis) {
@@ -87,17 +87,17 @@ float mp_get_runtime_work_position(uint8_t axis) {
// target_rotated[2] = a z_1 + b z_2 + c z_3 + z_offset
if (axis == AXIS_X) {
return mr.position[0] * cm->rotation_matrix[0][0] + mr.position[1] * cm->rotation_matrix[1][0] +
mr.position[2] * cm->rotation_matrix[2][0] - mr.gm.work_offset[0];
return mr->position[0] * cm->rotation_matrix[0][0] + mr->position[1] * cm->rotation_matrix[1][0] +
mr->position[2] * cm->rotation_matrix[2][0] - mr->gm.work_offset[0];
} else if (axis == AXIS_Y) {
return mr.position[0] * cm->rotation_matrix[0][1] + mr.position[1] * cm->rotation_matrix[1][1] +
mr.position[2] * cm->rotation_matrix[2][1] - mr.gm.work_offset[1];
return mr->position[0] * cm->rotation_matrix[0][1] + mr->position[1] * cm->rotation_matrix[1][1] +
mr->position[2] * cm->rotation_matrix[2][1] - mr->gm.work_offset[1];
} else if (axis == AXIS_Z) {
return mr.position[0] * cm->rotation_matrix[0][2] + mr.position[1] * cm->rotation_matrix[1][2] +
mr.position[2] * cm->rotation_matrix[2][2] - cm->rotation_z_offset - mr.gm.work_offset[2];
return mr->position[0] * cm->rotation_matrix[0][2] + mr->position[1] * cm->rotation_matrix[1][2] +
mr->position[2] * cm->rotation_matrix[2][2] - cm->rotation_z_offset - mr->gm.work_offset[2];
} else {
// ABC, UVW, we don't rotate them
return (mr.position[axis] - mr.gm.work_offset[axis]);
return (mr->position[axis] - mr->gm.work_offset[axis]);
}
}
@@ -114,7 +114,7 @@ bool mp_get_runtime_busy()
return (false);
}
if ((st_runtime_isbusy() == true) ||
(mr.block_state == BLOCK_ACTIVE) ||
(mr->block_state == BLOCK_ACTIVE) ||
(mp_get_r()->buffer_state > MP_BUFFER_EMPTY)) {
return (true);
}
g2core/plan_zoid.cpp
+5 -5
View File
@@ -164,7 +164,7 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent
// Here we verify it moving forward, checking to make sure it still is true.
// If so, we plan the "ramp" as flat, body-only.
if (bf->hint == PERFECT_CRUISE) {
if ((!mp.entry_changed) && fp_EQ(entry_velocity, bf->cruise_vmax)) {
if ((!mp->entry_changed) && fp_EQ(entry_velocity, bf->cruise_vmax)) {
// We need to ensure that neither the entry or the exit velocities are
// <= the cruise velocity even though there is tolerance in fp_EQ comparison.
block->exit_velocity = entry_velocity;
@@ -186,7 +186,7 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent
// Quick test to ensure we haven't violated the hint
if (entry_velocity > block->exit_velocity) {
// We're in a deceleration.
if (mp.entry_changed) {
if (mp->entry_changed) {
// If entry_changed, then entry_velocity is lower than the hints expect.
// A deceleration will never become an acceleration (post-hinting).
// If it is marked as MIXED_DECELERATION, it means the entry was CRUISE_VMAX.
@@ -226,7 +226,7 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent
// Reset entry_changed. We won't likely be changing the next block's entry velocity.
mp.entry_changed = false;
mp->entry_changed = false;
// Since we are not generally decelerating, this is effectively all of forward planning that we need.
@@ -244,7 +244,7 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent
if (accel_velocity < block->exit_velocity) { // still accelerating
mp.entry_changed = true; // we are changing the *next* block's entry velocity
mp->entry_changed = true; // we are changing the *next* block's entry velocity
block->exit_velocity = accel_velocity;
block->cruise_velocity = accel_velocity;
@@ -260,7 +260,7 @@ void mp_calculate_ramps(mpBlockRuntimeBuf_t* block, mpBuf_t* bf, const float ent
return (_zoid_exit(bf, ZOID_EXIT_1a));
} else { // it's hit the cusp
mp.entry_changed = false; // we are NOT changing the next block's entry velocity
mp->entry_changed = false; // we are NOT changing the next block's entry velocity
block->cruise_velocity = bf->cruise_vmax;
g2core/planner.cpp
+49 -75
View File
@@ -65,19 +65,18 @@
#include "xio.h" //+++++ DIAGNOSTIC - only needed if xio_writeline() direct prints are used
// Allocate planner structures
//mpPlannerQueue_t mb; // buffer pool management
mpPlanner_t *mp; // currently active planner (global variable)
mpPlanner_t mp0; // primary planning context
mpPlanner_t mp1; // secondary planning context
mpPlanner_t *mp; // currently active planner (global variable)
mpPlanner_t mp0; // primary planning context
mpPlanner_t mp1; // secondary planning context
mpPlannerRuntime_t *mr; // context for planner block runtime
mpPlannerRuntime_t mr0; // runtime context for primary planner
mpPlannerRuntime_t mr1; // runtime context for secondary planner
mpBuf_t mp0_pool[PLANNER_BUFFER_POOL_SIZE]; // storage allocation for primary planner queue buffers
mpBuf_t mp1_pool[SECONDARY_BUFFER_POOL_SIZE]; // storage allocation for secondary planner queue buffers
//mpMotionPlannerHead_t *mp; // pointer to motion planner
//mpMotionPlannerHead_t mp0; // primary motion planner
//mpMotionPlannerHead_t mp1; // secondary motion planner
mpMotionRuntimeSingleton_t mr; // context for block runtime
#define JSON_COMMAND_BUFFER_SIZE 3
struct json_command_buffer_t {
@@ -152,7 +151,6 @@ void _init_planner_queue(mpPlanner_t *mpl, mpBuf_t *pool, uint8_t size)
{
mpBuf_t *pv, *nx;
uint8_t i, nx_i;
// mpQueue_t *b = &mb.q[q];
mpPlannerQueue_t *q = &(mpl->q);
memset(q, 0, sizeof(mpPlannerQueue_t)); // clear values, pointers and status
@@ -178,58 +176,46 @@ void _init_planner_queue(mpPlanner_t *mpl, mpBuf_t *pool, uint8_t size)
q->bf[size-1].nx = pool;
}
void planner_init(mpPlanner_t *mpl)
void planner_init(mpPlanner_t *mpl, mpBuf_t *pool, mpPlannerRuntime_t *mrl)
{
// If you know all memory has been zeroed by a hard reset you don't need these next 2 lines
memset(&mp, 0, sizeof(mp)); // clear all values, pointers and status
// memset(&mr, 0, sizeof(mr)); // clear all values, pointers and status
mp = &mp0; // set global pointer to the primary planner
mr = &mr0; // and primary runtime
mp = &mp0; // install the primary planner
_init_planner_queue(mp, mp0_pool, PLANNER_BUFFER_POOL_SIZE);
planner_init_assertions(mpl);
// init planner master structure
memset(mpl, 0, sizeof(mpPlanner_t));// clear all values, pointers and status
mpl->magic_start = MAGICNUM; // set boundary condition assertions
mpl->magic_end = MAGICNUM;
mpl->mfo_factor = 1.00;
// mr.bf[0].nx = &mr.bf[1]; // Handle the two "stub blocks" in the runtime structure.
// mr.bf[1].nx = &mr.bf[0];
// mr.r = &mr.bf[0];
// mr.p = &mr.bf[1];
// init planner queues
mpl->q.bf = pool; // assign puffer pool to queue manager structure
_init_planner_queue(mpl, pool, PLANNER_BUFFER_POOL_SIZE);
// init runtime structs
mpl->mr = mrl;
memset(mrl, 0, sizeof(mpPlannerRuntime_t)); // clear all values, pointers and status
mrl->bf[0].nx = &mrl->bf[1]; // Handle the two "stub blocks" in the runtime structure.
mrl->bf[1].nx = &mrl->bf[0];
mrl->r = &mrl->bf[0];
mrl->p = &mrl->bf[1];
mpl->mr->magic_start = MAGICNUM; // assertions
mpl->mr->magic_end = MAGICNUM;
}
void planner_reset(mpPlanner_t *mpl)
{
planner_init(mpl);
}
void runtime_init() {
memset(&mr, 0, sizeof(mr)); // clear all values, pointers and status
mr.bf[0].nx = &mr.bf[1]; // Handle the two "stub blocks" in the runtime structure.
mr.bf[1].nx = &mr.bf[0];
mr.r = &mr.bf[0];
mr.p = &mr.bf[1];
planner_init(mpl, mpl->q.bf, mpl->mr); // reset parent planner and linked Q and MR
}
/*
* planner_init_assertions()
* planner_test_assertions() - test assertions, PANIC if violation exists
*/
void planner_init_assertions(mpPlanner_t *mpl)
{
// Note: mb magic numbers set up by mp_init_buffers()
mpl->magic_start = MAGICNUM;
mpl->magic_end = MAGICNUM;
mr.magic_start = MAGICNUM;
mr.magic_end = MAGICNUM;
}
stat_t planner_test_assertions(mpPlanner_t *mpl)
{
if (
(BAD_MAGIC(mpl->magic_start)) || (BAD_MAGIC(mpl->magic_end)) ||
(BAD_MAGIC(mr.magic_start)) || (BAD_MAGIC(mr.magic_end))
(BAD_MAGIC(mpl->magic_start)) || (BAD_MAGIC(mpl->magic_end)) ||
(BAD_MAGIC(mpl->mr->magic_start)) || (BAD_MAGIC(mpl->mr->magic_end))
) {
return(cm_panic(STAT_PLANNER_ASSERTION_FAILURE, "planner_test_assertions()"));
}
@@ -256,7 +242,7 @@ void mp_halt_runtime()
/*
* mp_flush_planner() - flush all moves in the planner and all arcs
*
* Does not affect the move currently running in mr.
* Does not affect the move currently running in mr->
* Does not affect mm or gm model positions
* This function is designed to be called during a hold to reset the planner
* This function should not generally be called; call cm_queue_flush() instead
@@ -265,8 +251,9 @@ void mp_flush_planner(mpPlanner_t *mpl)
{
cm_abort_arc();
// mp_init_planner_buffers(mpl); //+++++
planner_init(mpl);
mr.block_state = BLOCK_INACTIVE; // invalidate mr buffer to prevent subsequent motion
// planner_init(mpl); //+++++
planner_reset(mpl);
mr->block_state = BLOCK_INACTIVE; // invalidate mr buffer to prevent subsequent motion
}
/*
@@ -282,9 +269,9 @@ void mp_flush_planner(mpPlanner_t *mpl)
* Keeping track of position is complicated by the fact that moves exist in several reference
* frames. The scheme to keep this straight is:
*
* - mm.position - start and end position for planning
* - mr.position - current position of runtime segment
* - mr.target - target position of runtime segment
* - mp->position - start and end position for planning
* - mr->position - current position of runtime segment
* - mr->target - target position of runtime segment
*
* The runtime keeps a lot more data, such as waypoints, step vectors, etc.
* See struct mpMoveRuntimeSingleton for details.
@@ -295,21 +282,21 @@ void mp_flush_planner(mpPlanner_t *mpl)
*/
void mp_set_planner_position(uint8_t axis, const float position) { mp->position[axis] = position; } //+++++
void mp_set_runtime_position(uint8_t axis, const float position) { mr.position[axis] = position; }
void mp_set_runtime_position(uint8_t axis, const float position) { mr->position[axis] = position; }
void mp_set_steps_to_runtime_position()
{
float step_position[MOTORS];
kn_inverse_kinematics(mr.position, step_position); // convert lengths to steps in floating point
kn_inverse_kinematics(mr->position, step_position); // convert lengths to steps in floating point
for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) {
mr.target_steps[motor] = step_position[motor];
mr.position_steps[motor] = step_position[motor];
mr.commanded_steps[motor] = step_position[motor];
mr->target_steps[motor] = step_position[motor];
mr->position_steps[motor] = step_position[motor];
mr->commanded_steps[motor] = step_position[motor];
en_set_encoder_steps(motor, step_position[motor]); // write steps to encoder register
mr.encoder_steps[motor] = en_read_encoder(motor);
mr->encoder_steps[motor] = en_read_encoder(motor);
// These must be zero:
mr.following_error[motor] = 0;
mr->following_error[motor] = 0;
st_pre.mot[motor].corrected_steps = 0;
}
}
@@ -485,12 +472,12 @@ static stat_t _exec_dwell(mpBuf_t *bf)
//++++ stubbed ++++
void mp_request_out_of_band_dwell(float seconds)
{
// mr.out_of_band_dwell_time = seconds;
// mr->out_of_band_dwell_time = seconds;
}
//++++ stubbed ++++
stat_t mp_exec_out_of_band_dwell(void)
{
// return _advance_dwell(mr.out_of_band_dwell_time);
// return _advance_dwell(mr->out_of_band_dwell_time);
return 0;
}
@@ -783,19 +770,6 @@ static inline void _clear_buffer(mpBuf_t *bf)
bf->reset(); // Call a reset method on the buffer object.
} // We'll need something else for C - like bring the method code back into this function.
/*
void mp_init_planner_buffers(void)
{
// _init_planner_queue(0, mb_pool0, PLANNER_BUFFER_POOL_SIZE);
// _init_planner_queue(1, mb_pool1, SECONDARY_BUFFER_POOL_SIZE);
// mr.bf[0].nx = &mr.bf[1]; // Now handle the two "stub blocks" in the runtime structure.
// mr.bf[1].nx = &mr.bf[0];
// mr.r = &mr.bf[0];
// mr.p = &mr.bf[1];
}
*/
/*
* These GET functions are defined here but we use the macros in planner.h instead
mpBuf_t * mp_get_prev_buffer(const mpBuf_t *bf) { return (bf->pv); }
g2core/planner.h
+102 -104
View File
@@ -309,68 +309,69 @@ typedef enum {
* - plan_zoid.cpp / mp_calculate_ramps()
* - plan_exec.cpp / mp_exec_aline()
*/
//**** Planner Queue Structures ****
typedef struct mpBuffer {
// *** CAUTION *** These two pointers are not reset by _clear_buffer()
struct mpBuffer *pv; // static pointer to previous buffer
struct mpBuffer *nx; // static pointer to next buffer
uint8_t buffer_number; //+++++ DIAGNOSTIC for easier debugging
struct mpBuffer *pv; // static pointer to previous buffer
struct mpBuffer *nx; // static pointer to next buffer
uint8_t buffer_number; //+++++ DIAGNOSTIC for easier debugging
stat_t (*bf_func)(struct mpBuffer *bf); // callback to buffer exec function
cm_exec_t cm_func; // callback to canonical machine execution function
cm_exec_t cm_func; // callback to canonical machine execution function
//+++++ DIAGNOSTICS for easier debugging
uint32_t linenum; // mirror of bf->gm.linenum
uint32_t linenum; // mirror of bf->gm.linenum
int iterations;
float block_time_ms;
float plannable_time_ms; // time in planner
float plannable_length; // length in planner
uint8_t meet_iterations; // iterations needed in _get_meet_velocity
float plannable_time_ms; // time in planner
float plannable_length; // length in planner
uint8_t meet_iterations; // iterations needed in _get_meet_velocity
//+++++ to here
bufferState buffer_state; // used to manage queuing/dequeuing
blockType block_type; // used to dispatch to run routine
blockState block_state; // move state machine sequence
blockHint hint; // hint the block for zoid and other planning operations. Must be accurate or NO_HINT
bufferState buffer_state; // used to manage queuing/dequeuing
blockType block_type; // used to dispatch to run routine
blockState block_state; // move state machine sequence
blockHint hint; // hint the block for zoid and other planning operations. Must be accurate or NO_HINT
// block parameters
float unit[AXES]; // unit vector for axis scaling & planning
bool axis_flags[AXES]; // set true for axes participating in the move & for command parameters
float unit[AXES]; // unit vector for axis scaling & planning
bool axis_flags[AXES]; // set true for axes participating in the move & for command parameters
bool plannable; // set true when this block can be used for planning
bool plannable; // set true when this block can be used for planning
float length; // total length of line or helix in mm
float block_time; // computed move time for entire block (move)
float override_factor; // feed rate or rapid override factor for this block ("override" is a reserved word)
float length; // total length of line or helix in mm
float block_time; // computed move time for entire block (move)
float override_factor; // feed rate or rapid override factor for this block ("override" is a reserved word)
// *** SEE NOTES ON THESE VARIABLES, in aline() ***
// We removed all entry_* values.
// To get the entry_* values, look at pv->exit_* or mr.exit_*
float cruise_velocity; // cruise velocity requested & achieved
float exit_velocity; // exit velocity requested for the move
// To get the entry_* values, look at pv->exit_* or mr->exit_*
float cruise_velocity; // cruise velocity requested & achieved
float exit_velocity; // exit velocity requested for the move
// is also the entry velocity of the *next* move
float cruise_vset; // cruise velocity requested for move - prior to overrides
float cruise_vmax; // cruise max velocity adjusted for overrides
float exit_vmax; // max exit velocity possible for this move
float cruise_vset; // cruise velocity requested for move - prior to overrides
float cruise_vmax; // cruise max velocity adjusted for overrides
float exit_vmax; // max exit velocity possible for this move
// is also the maximum entry velocity of the next move
float absolute_vmax; // fastest this block can move w/o exceeding constraints
float junction_vmax; // maximum the exit velocity can be to go through the junction
float absolute_vmax; // fastest this block can move w/o exceeding constraints
float junction_vmax; // maximum the exit velocity can be to go through the junction
// between the NEXT BLOCK AND THIS ONE
float jerk; // maximum linear jerk term for this move
float jerk_sq; // Jm^2 is used for planning (computed and cached)
float recip_jerk; // 1/Jm used for planning (computed and cached)
float sqrt_j; // sqrt(jM) used for planning (computed and cached)
float q_recip_2_sqrt_j; // (q/(2 sqrt(jM))) where q = (sqrt(10)/(3^(1/4))), used in length computations (computed and cached)
float jerk; // maximum linear jerk term for this move
float jerk_sq; // Jm^2 is used for planning (computed and cached)
float recip_jerk; // 1/Jm used for planning (computed and cached)
float sqrt_j; // sqrt(jM) used for planning (computed and cached)
float q_recip_2_sqrt_j; // (q/(2 sqrt(jM))) where q = (sqrt(10)/(3^(1/4))), used in length computations (computed and cached)
GCodeState_t gm; // Gcode model state - passed from model, used by planner and runtime
GCodeState_t gm; // Gcode model state - passed from model, used by planner and runtime
// clears the above structure
void reset() {
//memset((void *)(this), 0, sizeof(mpBuffer_to_clear)); // slower on the M3. Test for M7
bf_func = nullptr;
cm_func = nullptr;
@@ -409,85 +410,37 @@ typedef struct mpBuffer {
q_recip_2_sqrt_j = 0.0;
gm.reset();
}
} mpBuf_t;
typedef struct mpPlannerQueue { // control structure for queue
magic_t magic_start; // magic number to test memory integrity
mpBuf_t *r; // run buffer pointer
mpBuf_t *w; // write buffer pointer
uint8_t queue_size; // total number of buffers, zero-based (e.g. 47 not 48)
uint8_t buffers_available; // running count of available buffers in queue
mpBuf_t *bf; // pointer to buffer storage array
typedef struct mpPlannerQueue { // control structure for queue
magic_t magic_start; // magic number to test memory integrity
mpBuf_t *r; // run buffer pointer
mpBuf_t *w; // write buffer pointer
uint8_t queue_size; // total number of buffers, zero-based (e.g. 47 not 48)
uint8_t buffers_available; // running count of available buffers in queue
mpBuf_t *bf; // pointer to buffer pool (storage array)
magic_t magic_end;
} mpPlannerQueue_t;
/*
typedef struct mpBufferQueue { // one or more planner buffer queues
uint8_t active_q; // index of currently active queue
uint8_t return_q; // index of queue to return to
mpQueue_t q[2]; // number of queues
} mpBufferQueue_t;
*/
//**** Planner Runtime structures ****
typedef struct mpPlanner { // common variables for a planner context
magic_t magic_start; // magic number to test memory integrity
typedef struct mpBlockRuntimeBuf { // Data structure for just the parts of RunTime that we need to plan a BLOCK
struct mpBlockRuntimeBuf *nx; // singly-linked-list
//+++++ DIAGNOSTICS
float run_time_remaining_ms;
float plannable_time_ms;
// planner position
float position[AXES]; // final move position for planning purposes
// timing variables
float run_time_remaining; // time left in runtime (including running block)
float plannable_time; // time in planner that can actually be planned
// planner state variables
plannerState planner_state; // current state of planner
bool request_planning; // set true to request backplanning
bool backplanning; // true if planner is in a back-planning pass
bool mfo_active; // true if mfo override is in effect
bool ramp_active; // true when a ramp is occurring
bool entry_changed; // mark if exit_velocity changed to invalidate next block's hint
// feed overrides and ramp variables (these extend the variables in cm->gmx)
float mfo_factor; // runtime override factor
float ramp_target;
float ramp_dvdt;
// objects
Timeout block_timeout; // Timeout object for block planning
// planner pointers
mpBuf_t *p; // planner buffer pointer
mpBuf_t *c; // pointer to buffer immediately following critical region
mpBuf_t *planning_return; // buffer to return to once back-planning is complete
// queue manager
mpPlannerQueue_t q; // embed a planner buffer queue manager
magic_t magic_end;
} mpPlanner_t;
typedef struct mpBlockRuntimeBuf { // Data structure for just the parts of RunTime that we need to plan a BLOCK
struct mpBlockRuntimeBuf *nx; // singly-linked-list
float head_length; // copies of bf variables of same name
float head_length; // copies of bf variables of same name
float body_length;
float tail_length;
float head_time; // copies of bf variables of same name
float head_time; // copies of bf variables of same name
float body_time;
float tail_time;
float cruise_velocity; // velocity at the end of the head and the beginning of the tail
float exit_velocity; // velocity at the end of the move
float cruise_velocity; // velocity at the end of the head and the beginning of the tail
float exit_velocity; // velocity at the end of the move
} mpBlockRuntimeBuf_t;
typedef struct mpMotionRuntimeSingleton { // persistent runtime variables
// uint8_t (*run_move)(struct mpMoveRuntimeSingleton *m); // currently running move - left in for reference
typedef struct mpPlannerRuntime { // persistent runtime variables
// uint8_t (*run_move)(struct mpMoveRuntimeSingleton *m); // currently running move - left in for reference
magic_t magic_start; // magic number to test memory integrity
blockState block_state; // state of the overall move
moveSection section; // what section is the move in?
@@ -525,10 +478,55 @@ typedef struct mpMotionRuntimeSingleton { // persistent runtime variables
GCodeState_t gm; // gcode model state currently executing
magic_t magic_end;
} mpMotionRuntimeSingleton_t;
} mpPlannerRuntime_t;
//**** Master Planner Structure ***
typedef struct mpPlanner { // common variables for a planner context
magic_t magic_start; // magic number to test memory integrity
//+++++ DIAGNOSTICS
float run_time_remaining_ms;
float plannable_time_ms;
// planner position
float position[AXES]; // final move position for planning purposes
// timing variables
float run_time_remaining; // time left in runtime (including running block)
float plannable_time; // time in planner that can actually be planned
// planner state variables
plannerState planner_state; // current state of planner
bool request_planning; // set true to request backplanning
bool backplanning; // true if planner is in a back-planning pass
bool mfo_active; // true if mfo override is in effect
bool ramp_active; // true when a ramp is occurring
bool entry_changed; // mark if exit_velocity changed to invalidate next block's hint
// feed overrides and ramp variables (these extend the variables in cm->gmx)
float mfo_factor; // runtime override factor
float ramp_target;
float ramp_dvdt;
// objects
Timeout block_timeout; // Timeout object for block planning
// planner pointers
mpBuf_t *p; // planner buffer pointer
mpBuf_t *c; // pointer to buffer immediately following critical region
mpBuf_t *planning_return; // buffer to return to once back-planning is complete
// planner queue manager
mpPlannerQueue_t q; // embed a planner buffer queue manager
// planner runtime
mpPlannerRuntime_t *mr; // bind to mr associated with this planner
magic_t magic_end;
} mpPlanner_t;
// Reference global scope structures
//extern mpPlannerQueue_t mb; // planner buffer queue management
extern mpPlanner_t *mp; // currently active planner (global variable)
extern mpPlanner_t mp0; // primary planning context
@@ -536,7 +534,9 @@ extern mpPlanner_t mp1; // secondary planning co
extern mpBuf_t mp0_pool[PLANNER_BUFFER_POOL_SIZE]; // storage allocation for primary planner queue buffers
extern mpBuf_t mp1_pool[SECONDARY_BUFFER_POOL_SIZE]; // storage allocation for secondary planner queue buffers
extern mpMotionRuntimeSingleton_t mr; // context for block runtime
extern mpPlannerRuntime_t *mr; // context for block runtime
extern mpPlannerRuntime_t mr0; // runtime context for primary planner
extern mpPlannerRuntime_t mr1; // runtime context for secondary planner
/*
* Global Scope Functions
@@ -544,11 +544,9 @@ extern mpMotionRuntimeSingleton_t mr; // context for block runtime
//planner.cpp functions
void planner_init(mpPlanner_t *mpl);
void planner_init(mpPlanner_t *mpl, mpBuf_t *pool, mpPlannerRuntime_t *mrl);
void planner_reset(mpPlanner_t *mpl);
void planner_init_assertions(mpPlanner_t *mpl);
stat_t planner_test_assertions(mpPlanner_t *mpl);
void runtime_init(void);
void mp_halt_runtime(void);
void mp_flush_planner(mpPlanner_t *mpl);