From b9a403a2490c9fb1e56baffd2c9eefe0f4e2fa6a Mon Sep 17 00:00:00 2001 From: Rob Giseburt Date: Fri, 10 Apr 2020 16:34:23 -0500 Subject: [PATCH] Added pressure event state machine, added event/error counters --- g2core/config_app.cpp | 6 ++ g2core/kinematics.cpp | 22 ++++++ g2core/kinematics.h | 4 ++ g2core/kinematics_pressure.h | 103 ++++++++++++++++++++++------ g2core/settings/settings_minimill.h | 2 +- 5 files changed, 115 insertions(+), 22 deletions(-) diff --git a/g2core/config_app.cpp b/g2core/config_app.cpp index 658c9beb..8950de5a 100644 --- a/g2core/config_app.cpp +++ b/g2core/config_app.cpp @@ -1015,10 +1015,16 @@ constexpr cfgItem_t sys_config_items_2[] = { { "kn","knp", _f0, 4, tx_print_nul, kn_get_p_factor, kn_set_p_factor, nullptr, 0 }, { "kn","kni", _f0, 4, tx_print_nul, kn_get_i_factor, kn_set_i_factor, nullptr, 0 }, { "kn","knd", _f0, 4, tx_print_nul, kn_get_d_factor, kn_set_d_factor, nullptr, 0 }, + { "kn","knp", _f0, 4, tx_print_nul, kn_get_p_factor, kn_set_p_factor, nullptr, 0 }, + { "kn","kni", _f0, 4, tx_print_nul, kn_get_i_factor, kn_set_i_factor, nullptr, 0 }, + { "kn","knd", _f0, 4, tx_print_nul, kn_get_d_factor, kn_set_d_factor, nullptr, 0 }, { "kn","knev", _f0, 4, tx_print_nul, kn_get_e_value, set_nul, nullptr, 0 }, { "kn","kniv", _f0, 4, tx_print_nul, kn_get_i_value, set_nul, nullptr, 0 }, { "kn","kndv", _f0, 4, tx_print_nul, kn_get_d_value, set_nul, nullptr, 0 }, { "kn","knbf", _f0, 4, tx_print_nul, kn_get_backoff_pressure, kn_set_backoff_pressure, nullptr, 0 }, + { "kn","knec", _i0, 0, tx_print_nul, kn_get_ec_value, set_nul, nullptr, 0 }, + { "kn","knuoc", _i0, 0, tx_print_nul, kn_get_uoc_value, set_nul, nullptr, 0 }, + { "kn","knumc", _i0, 0, tx_print_nul, kn_get_umc_value, set_nul, nullptr, 0 }, #endif diff --git a/g2core/kinematics.cpp b/g2core/kinematics.cpp index dc3294d1..f86714b0 100644 --- a/g2core/kinematics.cpp +++ b/g2core/kinematics.cpp @@ -244,6 +244,28 @@ stat_t kn_get_d_value(nvObj_t *nv) return (STAT_OK); }; +stat_t kn_get_uoc_value(nvObj_t *nv) +{ + nv->valuetype = TYPE_INTEGER; + nv->value_int = pressure_kinematics.unable_to_obtian_error_counter; + + return (STAT_OK); +}; +stat_t kn_get_umc_value(nvObj_t *nv) +{ + nv->valuetype = TYPE_INTEGER; + nv->value_int = pressure_kinematics.unable_to_maintian_error_counter; + + return (STAT_OK); +}; +stat_t kn_get_ec_value(nvObj_t *nv) +{ + nv->valuetype = TYPE_INTEGER; + nv->value_int = pressure_kinematics.event_counter; + + return (STAT_OK); +}; + stat_t kn_get_p_factor(nvObj_t *nv) { nv->valuetype = TYPE_FLOAT; diff --git a/g2core/kinematics.h b/g2core/kinematics.h index 550d0953..72395569 100644 --- a/g2core/kinematics.h +++ b/g2core/kinematics.h @@ -115,6 +115,10 @@ stat_t kn_get_e_value(nvObj_t *nv); stat_t kn_get_i_value(nvObj_t *nv); stat_t kn_get_d_value(nvObj_t *nv); +stat_t kn_get_uoc_value(nvObj_t *nv); +stat_t kn_get_umc_value(nvObj_t *nv); +stat_t kn_get_ec_value(nvObj_t *nv); + stat_t kn_get_p_factor(nvObj_t *nv); stat_t kn_set_p_factor(nvObj_t *nv); stat_t kn_get_i_factor(nvObj_t *nv); diff --git a/g2core/kinematics_pressure.h b/g2core/kinematics_pressure.h index 3cd514c2..1d64abf7 100644 --- a/g2core/kinematics_pressure.h +++ b/g2core/kinematics_pressure.h @@ -57,11 +57,11 @@ struct PressureKinematics : KinematicsBase { double sensor_zero_target = 0.0; - double sensor_proportional_factor = 250; + double sensor_proportional_factor = 200; double sensor_integral_store = 0; - double sensor_inetgral_factor = 0.005; + double sensor_inetgral_factor = 0.01; double sensor_error_store = 0; - double sensor_derivative_factor = 50; + double sensor_derivative_factor = 500; double sensor_derivative_store = 0; double derivative_contribution = 1.0/10.0; @@ -293,6 +293,67 @@ struct PressureKinematics : KinematicsBase { // nothing to do yet } + // control and handle state machine changes + + void change_state_to_start() { + // this means the EPM timer expired - this shoutd be on an Idle -> Start transition + // we'll also accept a Release -> Start transition + // if not, we have an error to deal with + + if ((PressureState::Idle != pressure_state) && (PressureState::Release != pressure_state)) { + // we were unable to hold or obtain pressure - we need to move to release + change_state_to_release(); + } else { + pressure_state = PressureState::Start; + + sensor_zero_target = pressure_target; + } + + // wipe out the PID integral, as in all the cases we reverse directions + sensor_integral_store = 0; + + // restart the timer + inter_event_timer.set(seconds_between_events * 1000.0); + event_counter++; + } + + void change_state_to_hold() { + // this should only go into hold if called from Start + if (PressureState::Start == pressure_state) { + pressure_state = PressureState::Hold; + hold_pressure_timer.set(seconds_to_hold_event*1000.0); + } + + + // Other possibilities: + // * Already in Hold: Still holding + // * In Release: pressure still high from Hold state + // * In Idle: Negative pressue used to Release is still in effect + } + + void change_state_to_release() { + // we have an error of some sort + if (PressureState::Start == pressure_state) { + unable_to_obtian_error_counter++; + } + else if (PressureState::Hold == pressure_state) { + if (!hold_pressure_timer.isPast()) { + unable_to_maintian_error_counter++; + } + } + + pressure_state = PressureState::Release; + sensor_zero_target = reverse_target_pressure; + } + + void change_state_to_idle() { + if (PressureState::Release != pressure_state) { + // shouldn't happen? + } + + pressure_state = PressureState::Idle; + } + // if we requested a move, return true, otherwise false bool last_segment_was_idle = false; bool over_pressure = false; @@ -335,10 +396,7 @@ struct PressureKinematics : KinematicsBase { */ if (!inter_event_timer.isSet() || inter_event_timer.isPast()) { - if () - sensor_zero_target = pressure_target; - sensor_integral_store = 0; - inter_event_timer.set(seconds_between_events * 1000.0); + change_state_to_start(); } // check inputs to be sure we aren't anchored @@ -357,30 +415,29 @@ struct PressureKinematics : KinematicsBase { bool over_pressure_detected = (raw_sensor_value[joint] > (sensor_zero_target*0.95)); if ((PressureState::Hold == pressure_state) && hold_pressure_timer.isPast()) { - hold_pressure_timer.clear(); - sensor_zero_target = reverse_target_pressure; - pressure_state = PressureState::Release; - } else if (over_pressure_detected && !hold_pressure_timer.isSet()) { - over_pressure = true; - hold_pressure_timer.set(seconds_to_hold_event*1000.0); + change_state_to_release(); + } else if (over_pressure_detected) { + change_state_to_hold(); // detects if already in hold, or not in Start } - // bool switch_state = false; // ignore switches if (switch_state && !last_switch_state[joint]) { - if (sensor_zero_target < 0) { + if (PressureState::Release == pressure_state) { // stop the motion - sensor_zero_target = 0.1; + // sensor_zero_target = 0.1; sensor_integral_store = 0; + + change_state_to_idle(); } joint_min_limit[joint] = joint_position[joint] + cm->a[AXIS_X].travel_min; } else if (!switch_state) { if (last_switch_state[joint]) { + // just left the switch, record how far we can go joint_max_limit[joint] = joint_position[joint] + cm->a[AXIS_X].travel_max; } else { - if ((sensor_zero_target > 0) && (joint_position[joint] < joint_min_limit[joint])) { - sensor_zero_target = reverse_target_pressure; - sensor_integral_store = 0; + // check to make sure we haven't gone too far + if (joint_position[joint] > joint_max_limit[joint]) { + change_state_to_release(); } } } @@ -439,17 +496,21 @@ struct PressureKinematics : KinematicsBase { // now that everything is done adjusting joint_vel[joint], we can recompute joint_accel[joint] joint_accel[joint] = (joint_vel[joint] - old_joint_vel) / segment_time - jmax * segment_time * 0.5; - // stop driving past the switch or too far out + // we check if we'll violate min or max position with the next position - last-change to stop driving into the wall auto proposed_position = joint_position[joint] + ((old_joint_vel + joint_vel[joint]) * 0.5 * segment_time); + + // if the switch is closed, we may still have some room to stop cleanly if (((switch_state) && (proposed_position < joint_min_limit[joint]) && (joint_vel[joint] < 0)) || ((proposed_position > joint_max_limit[joint]) && (joint_vel[joint] > 0))) { if (joint_vel[joint] > 0) { - sensor_zero_target = reverse_target_pressure; + // we moved too far, give up on this pass + change_state_to_release(); } // prevent the integral from winding up positive, pushing past the switch sensor_integral_store = 0; + joint_vel[joint] = joint_vel[joint] * 0.5; // drop the velocity hard -- we should probably do this more intelligently // this is a lie - we've certainly violated jerk, so don't punish the acceleration counter diff --git a/g2core/settings/settings_minimill.h b/g2core/settings/settings_minimill.h index 14847a2b..8acc94a0 100644 --- a/g2core/settings/settings_minimill.h +++ b/g2core/settings/settings_minimill.h @@ -127,7 +127,7 @@ M100 ({th2mxp:1500}) ; laser max pulses per mm #define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE #define STATUS_REPORT_MIN_MS 100 // milliseconds - enforces a viable minimum #define STATUS_REPORT_INTERVAL_MS 250 // milliseconds - set $SV=0 to disable -#define STATUS_REPORT_DEFAULTS "knfc", "stat", "knft", "prs1", "feed", "knev", "kniv", "kndv" +#define STATUS_REPORT_DEFAULTS "knfc", "stat", "knft", "prs1", "feed", "knev", "kniv", "kndv", "knec", "knuoc", "knumc" // #define STATUS_REPORT_DEFAULTS "posx", "posy", "posz", // "unit", "stat", "coor", "momo", "dist", // "home", "vel", "plan", "line", "path",