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

Added pressure event state machine, added event/error counters

Browse Source
This commit is contained in:
Rob Giseburt
2020-04-10 16:34:23 -05:00
parent ec208f6ed2
commit b9a403a249
5 changed files with 115 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
g2core/config_app.cpp
+6
View File
@@ -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
g2core/kinematics.cpp
+22
View File
@@ -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;
g2core/kinematics.h
+4
View File
@@ -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);
g2core/kinematics_pressure.h
+82 -21
View File
@@ -57,11 +57,11 @@ struct PressureKinematics : KinematicsBase<axes, motors> {
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<axes, motors> {
// 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<axes, motors> {
*/
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<axes, motors> {
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<axes, motors> {
// 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
g2core/settings/settings_minimill.h
+1 -1
View File
@@ -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",