diff --git a/g2core/board/ArduinoDue/board_stepper.cpp b/g2core/board/ArduinoDue/board_stepper.cpp index 0583e4e7..2ebd0f4a 100755 --- a/g2core/board/ArduinoDue/board_stepper.cpp +++ b/g2core/board/ArduinoDue/board_stepper.cpp @@ -28,7 +28,7 @@ #include "board_stepper.h" -// These are identical to board_stepper.h, except for the word "extern" +// These are identical to board_stepper.h, except for the word "extern", and they have the initialization parameters StepDirStepper - motor_1{}; + motor_1{M1_ENABLE_POLARITY}; StepDirStepper - motor_2{}; + motor_2{M2_ENABLE_POLARITY}; StepDirStepper - motor_3{}; + motor_3{M3_ENABLE_POLARITY}; StepDirStepper - motor_4{}; + motor_4{M4_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket5_StepPinNumber, @@ -72,7 +72,8 @@ StepDirStepper motor_5 {}; +// Motate::kSocket5_VrefPinNumber> +// motor_5 {M5_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket6_StepPinNumber, @@ -81,7 +82,8 @@ StepDirStepper motor_6 {}; +// Motate::kSocket6_VrefPinNumber> +// motor_6 {M6_ENABLE_POLARITY}; Stepper* Motors[MOTORS] = {&motor_1, &motor_2, &motor_3, &motor_4}; diff --git a/g2core/board/G2v9/board_stepper.cpp b/g2core/board/G2v9/board_stepper.cpp index 0583e4e7..2ebd0f4a 100755 --- a/g2core/board/G2v9/board_stepper.cpp +++ b/g2core/board/G2v9/board_stepper.cpp @@ -28,7 +28,7 @@ #include "board_stepper.h" -// These are identical to board_stepper.h, except for the word "extern" +// These are identical to board_stepper.h, except for the word "extern", and they have the initialization parameters StepDirStepper - motor_1{}; + motor_1{M1_ENABLE_POLARITY}; StepDirStepper - motor_2{}; + motor_2{M2_ENABLE_POLARITY}; StepDirStepper - motor_3{}; + motor_3{M3_ENABLE_POLARITY}; StepDirStepper - motor_4{}; + motor_4{M4_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket5_StepPinNumber, @@ -72,7 +72,8 @@ StepDirStepper motor_5 {}; +// Motate::kSocket5_VrefPinNumber> +// motor_5 {M5_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket6_StepPinNumber, @@ -81,7 +82,8 @@ StepDirStepper motor_6 {}; +// Motate::kSocket6_VrefPinNumber> +// motor_6 {M6_ENABLE_POLARITY}; Stepper* Motors[MOTORS] = {&motor_1, &motor_2, &motor_3, &motor_4}; diff --git a/g2core/board/gquadratic/board_stepper.cpp b/g2core/board/gquadratic/board_stepper.cpp index bc927558..b43c3269 100755 --- a/g2core/board/gquadratic/board_stepper.cpp +++ b/g2core/board/gquadratic/board_stepper.cpp @@ -28,7 +28,7 @@ #include "board_stepper.h" -// These are identical to board_stepper.h, except for the word "extern" +// These are identical to board_stepper.h, except for the word "extern", and they have the initialization parameters StepDirStepper - motor_1{}; + motor_1{M1_ENABLE_POLARITY}; StepDirStepper - motor_2{}; + motor_2{M2_ENABLE_POLARITY}; StepDirHobbyServo motor_3; diff --git a/g2core/board/printrboardg2/board_stepper.cpp b/g2core/board/printrboardg2/board_stepper.cpp index 0583e4e7..2ebd0f4a 100755 --- a/g2core/board/printrboardg2/board_stepper.cpp +++ b/g2core/board/printrboardg2/board_stepper.cpp @@ -28,7 +28,7 @@ #include "board_stepper.h" -// These are identical to board_stepper.h, except for the word "extern" +// These are identical to board_stepper.h, except for the word "extern", and they have the initialization parameters StepDirStepper - motor_1{}; + motor_1{M1_ENABLE_POLARITY}; StepDirStepper - motor_2{}; + motor_2{M2_ENABLE_POLARITY}; StepDirStepper - motor_3{}; + motor_3{M3_ENABLE_POLARITY}; StepDirStepper - motor_4{}; + motor_4{M4_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket5_StepPinNumber, @@ -72,7 +72,8 @@ StepDirStepper motor_5 {}; +// Motate::kSocket5_VrefPinNumber> +// motor_5 {M5_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket6_StepPinNumber, @@ -81,7 +82,8 @@ StepDirStepper motor_6 {}; +// Motate::kSocket6_VrefPinNumber> +// motor_6 {M6_ENABLE_POLARITY}; Stepper* Motors[MOTORS] = {&motor_1, &motor_2, &motor_3, &motor_4}; diff --git a/g2core/board/sbv300/board_stepper.cpp b/g2core/board/sbv300/board_stepper.cpp index 0583e4e7..2ebd0f4a 100755 --- a/g2core/board/sbv300/board_stepper.cpp +++ b/g2core/board/sbv300/board_stepper.cpp @@ -28,7 +28,7 @@ #include "board_stepper.h" -// These are identical to board_stepper.h, except for the word "extern" +// These are identical to board_stepper.h, except for the word "extern", and they have the initialization parameters StepDirStepper - motor_1{}; + motor_1{M1_ENABLE_POLARITY}; StepDirStepper - motor_2{}; + motor_2{M2_ENABLE_POLARITY}; StepDirStepper - motor_3{}; + motor_3{M3_ENABLE_POLARITY}; StepDirStepper - motor_4{}; + motor_4{M4_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket5_StepPinNumber, @@ -72,7 +72,8 @@ StepDirStepper motor_5 {}; +// Motate::kSocket5_VrefPinNumber> +// motor_5 {M5_ENABLE_POLARITY}; // StepDirStepper< // Motate::kSocket6_StepPinNumber, @@ -81,7 +82,8 @@ StepDirStepper motor_6 {}; +// Motate::kSocket6_VrefPinNumber> +// motor_6 {M6_ENABLE_POLARITY}; Stepper* Motors[MOTORS] = {&motor_1, &motor_2, &motor_3, &motor_4}; diff --git a/g2core/config_app.cpp b/g2core/config_app.cpp index 01e0cc3e..b7d2c79a 100644 --- a/g2core/config_app.cpp +++ b/g2core/config_app.cpp @@ -165,14 +165,14 @@ const cfgItem_t cfgArray[] = { { "hom","homb",_f0, 0, cm_print_hom, get_ui8, set_01, (float *)&cm.homed[AXIS_B], false }, // B homed { "hom","homc",_f0, 0, cm_print_hom, get_ui8, set_01, (float *)&cm.homed[AXIS_C], false }, // C homed - { "prb","prbe",_f0, 0, tx_print_nul, get_ui8, set_ro, (float *)&cm.probe_state[0], 0 }, // probing state + { "prb","prbe",_f0, 0, tx_print_nul, get_ui8, set_ro, (float *)&cm.probe_state[0], 0 }, // probing state { "prb","prbx",_f0, 3, tx_print_nul, get_flt, set_ro, (float *)&cm.probe_results[0][AXIS_X], 0 }, { "prb","prby",_f0, 3, tx_print_nul, get_flt, set_ro, (float *)&cm.probe_results[0][AXIS_Y], 0 }, { "prb","prbz",_f0, 3, tx_print_nul, get_flt, set_ro, (float *)&cm.probe_results[0][AXIS_Z], 0 }, { "prb","prba",_f0, 3, tx_print_nul, get_flt, set_ro, (float *)&cm.probe_results[0][AXIS_A], 0 }, { "prb","prbb",_f0, 3, tx_print_nul, get_flt, set_ro, (float *)&cm.probe_results[0][AXIS_B], 0 }, { "prb","prbc",_f0, 3, tx_print_nul, get_flt, set_ro, (float *)&cm.probe_results[0][AXIS_C], 0 }, - { "prb","prbr",_f0, 0, tx_print_nul, cm_get_prbr, cm_get_prbr, nullptr, 0 }, // enable probe report. Init in cm_init + { "prb","prbr",_f0, 0, tx_print_nul, cm_get_prbr, cm_get_prbr, nullptr, 0 }, // enable probe report. Init in cm_init { "jog","jogx",_f0, 0, tx_print_nul, get_nul, cm_run_jogx, (float *)&cm.jogging_dest, 0}, { "jog","jogy",_f0, 0, tx_print_nul, get_nul, cm_run_jogy, (float *)&cm.jogging_dest, 0}, @@ -197,75 +197,81 @@ const cfgItem_t cfgArray[] = { #endif // Motor parameters - { "1","1ma",_fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_1].motor_map, M1_MOTOR_MAP }, - { "1","1sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_1].step_angle, M1_STEP_ANGLE }, - { "1","1tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_1].travel_rev, M1_TRAVEL_PER_REV }, - { "1","1mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_1].microsteps, M1_MICROSTEPS }, - { "1","1su",_fipi,5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_1].steps_per_unit, M1_STEPS_PER_UNIT }, - { "1","1po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_1].polarity, M1_POLARITY }, - { "1","1pm",_fip, 0, st_print_pm, st_get_pm,st_set_pm, (float *)&cs.null, M1_POWER_MODE }, - { "1","1pl",_fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_1].power_level, M1_POWER_LEVEL }, -// { "1","1pi",_fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_1].power_idle, M1_POWER_IDLE }, -// { "1","1mt",_fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_1].motor_timeout, M1_MOTOR_TIMEOUT }, + { "1","1ma",_fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_1].motor_map, M1_MOTOR_MAP }, + { "1","1sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_1].step_angle, M1_STEP_ANGLE }, + { "1","1tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_1].travel_rev, M1_TRAVEL_PER_REV }, + { "1","1mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_1].microsteps, M1_MICROSTEPS }, + { "1","1su",_fipi,5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_1].steps_per_unit, M1_STEPS_PER_UNIT }, + { "1","1po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_1].polarity, M1_POLARITY }, + { "1","1ep",_fip, 0, st_print_ep, st_get_ep, st_set_ep, (float *)&cs.null, M1_ENABLE_POLARITY }, + { "1","1pm",_fip, 0, st_print_pm, st_get_pm, st_set_pm, (float *)&cs.null, M1_POWER_MODE }, + { "1","1pl",_fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_1].power_level, M1_POWER_LEVEL }, +// { "1","1pi",_fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_1].power_idle, M1_POWER_IDLE }, +// { "1","1mt",_fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_1].motor_timeout, M1_MOTOR_TIMEOUT }, #if (MOTORS >= 2) - { "2","2ma",_fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_2].motor_map, M2_MOTOR_MAP }, - { "2","2sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_2].step_angle, M2_STEP_ANGLE }, - { "2","2tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_2].travel_rev, M2_TRAVEL_PER_REV }, - { "2","2mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_2].microsteps, M2_MICROSTEPS }, - { "2","2su",_fipi,5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_2].steps_per_unit, M2_STEPS_PER_UNIT }, - { "2","2po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_2].polarity, M2_POLARITY }, - { "2","2pm",_fip, 0, st_print_pm, st_get_pm,st_set_pm, (float *)&cs.null, M2_POWER_MODE }, - { "2","2pl",_fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_2].power_level, M2_POWER_LEVEL}, -// { "2","2pi",_fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_2].power_idle, M2_POWER_IDLE }, -// { "2","2mt",_fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_2].motor_timeout, M2_MOTOR_TIMEOUT }, + { "2","2ma",_fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_2].motor_map, M2_MOTOR_MAP }, + { "2","2sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_2].step_angle, M2_STEP_ANGLE }, + { "2","2tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_2].travel_rev, M2_TRAVEL_PER_REV }, + { "2","2mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_2].microsteps, M2_MICROSTEPS }, + { "2","2su",_fipi,5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_2].steps_per_unit, M2_STEPS_PER_UNIT }, + { "2","2po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_2].polarity, M2_POLARITY }, + { "2","2ep",_fip, 0, st_print_ep, st_get_ep, st_set_ep, (float *)&cs.null, M2_ENABLE_POLARITY }, + { "2","2pm",_fip, 0, st_print_pm, st_get_pm, st_set_pm, (float *)&cs.null, M2_POWER_MODE }, + { "2","2pl",_fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_2].power_level, M2_POWER_LEVEL}, +// { "2","2pi",_fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_2].power_idle, M2_POWER_IDLE }, +// { "2","2mt",_fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_2].motor_timeout, M2_MOTOR_TIMEOUT }, #endif #if (MOTORS >= 3) - { "3","3ma",_fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_3].motor_map, M3_MOTOR_MAP }, - { "3","3sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_3].step_angle, M3_STEP_ANGLE }, - { "3","3tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_3].travel_rev, M3_TRAVEL_PER_REV }, - { "3","3mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_3].microsteps, M3_MICROSTEPS }, - { "3","3su",_fipi,5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_3].steps_per_unit, M3_STEPS_PER_UNIT }, - { "3","3po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_3].polarity, M3_POLARITY }, - { "3","3pm",_fip, 0, st_print_pm, st_get_pm,st_set_pm, (float *)&cs.null, M3_POWER_MODE }, - { "3","3pl",_fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_3].power_level, M3_POWER_LEVEL }, -// { "3","3pi",_fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_3].power_idle, M3_POWER_IDLE }, -// { "3","3mt",_fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_3].motor_timeout, M3_MOTOR_TIMEOUT }, + { "3","3ma",_fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_3].motor_map, M3_MOTOR_MAP }, + { "3","3sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_3].step_angle, M3_STEP_ANGLE }, + { "3","3tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_3].travel_rev, M3_TRAVEL_PER_REV }, + { "3","3mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_3].microsteps, M3_MICROSTEPS }, + { "3","3su",_fipi,5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_3].steps_per_unit, M3_STEPS_PER_UNIT }, + { "3","3po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_3].polarity, M3_POLARITY }, + { "3","3ep",_fip, 0, st_print_ep, st_get_ep, st_set_ep, (float *)&cs.null, M3_ENABLE_POLARITY }, + { "3","3pm",_fip, 0, st_print_pm, st_get_pm, st_set_pm, (float *)&cs.null, M3_POWER_MODE }, + { "3","3pl",_fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_3].power_level, M3_POWER_LEVEL }, +// { "3","3pi",_fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_3].power_idle, M3_POWER_IDLE }, +// { "3","3mt",_fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_3].motor_timeout, M3_MOTOR_TIMEOUT }, #endif #if (MOTORS >= 4) - { "4","4ma",_fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_4].motor_map, M4_MOTOR_MAP }, - { "4","4sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_4].step_angle, M4_STEP_ANGLE }, - { "4","4tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_4].travel_rev, M4_TRAVEL_PER_REV }, - { "4","4mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_4].microsteps, M4_MICROSTEPS }, - { "4","4su",_fipi,5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_4].steps_per_unit, M4_STEPS_PER_UNIT }, - { "4","4po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_4].polarity, M4_POLARITY }, - { "4","4pm",_fip, 0, st_print_pm, st_get_pm,st_set_pm, (float *)&cs.null, M4_POWER_MODE }, - { "4","4pl",_fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_4].power_level, M4_POWER_LEVEL }, -// { "4","4pi",_fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_4].power_idle, M4_POWER_IDLE }, -// { "4","4mt",_fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_4].motor_timeout, M4_MOTOR_TIMEOUT }, + { "4","4ma",_fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_4].motor_map, M4_MOTOR_MAP }, + { "4","4sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_4].step_angle, M4_STEP_ANGLE }, + { "4","4tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_4].travel_rev, M4_TRAVEL_PER_REV }, + { "4","4mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_4].microsteps, M4_MICROSTEPS }, + { "4","4su",_fipi,5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_4].steps_per_unit, M4_STEPS_PER_UNIT }, + { "4","4po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_4].polarity, M4_POLARITY }, + { "4","4ep",_fip, 0, st_print_ep, st_get_ep, st_set_ep, (float *)&cs.null, M4_ENABLE_POLARITY }, + { "4","4pm",_fip, 0, st_print_pm, st_get_pm, st_set_pm, (float *)&cs.null, M4_POWER_MODE }, + { "4","4pl",_fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_4].power_level, M4_POWER_LEVEL }, +// { "4","4pi",_fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_4].power_idle, M4_POWER_IDLE }, +// { "4","4mt",_fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_4].motor_timeout, M4_MOTOR_TIMEOUT }, #endif #if (MOTORS >= 5) - { "5","5ma",_fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_5].motor_map, M5_MOTOR_MAP }, - { "5","5sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_5].step_angle, M5_STEP_ANGLE }, - { "5","5tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_5].travel_rev, M5_TRAVEL_PER_REV }, - { "5","5mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_5].microsteps, M5_MICROSTEPS }, - { "5","5su",_fipi,5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_5].steps_per_unit, M5_STEPS_PER_UNIT }, - { "5","5po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_5].polarity, M5_POLARITY }, - { "5","5pm",_fip, 0, st_print_pm, st_get_pm,st_set_pm, (float *)&cs.null, M5_POWER_MODE }, - { "5","5pl",_fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_5].power_level, M5_POWER_LEVEL }, -// { "5","5pi",_fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_5].power_idle, M5_POWER_IDLE }, -// { "5","5mt",_fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_5].motor_timeout, M5_MOTOR_TIMEOUT }, + { "5","5ma",_fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_5].motor_map, M5_MOTOR_MAP }, + { "5","5sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_5].step_angle, M5_STEP_ANGLE }, + { "5","5tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_5].travel_rev, M5_TRAVEL_PER_REV }, + { "5","5mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_5].microsteps, M5_MICROSTEPS }, + { "5","5su",_fipi,5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_5].steps_per_unit, M5_STEPS_PER_UNIT }, + { "5","5po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_5].polarity, M5_POLARITY }, + { "5","5ep",_fip, 0, st_print_ep, st_get_ep, st_set_ep, (float *)&cs.null, M5_ENABLE_POLARITY }, + { "5","5pm",_fip, 0, st_print_pm, st_get_pm, st_set_pm, (float *)&cs.null, M5_POWER_MODE }, + { "5","5pl",_fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_5].power_level, M5_POWER_LEVEL }, +// { "5","5pi",_fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_5].power_idle, M5_POWER_IDLE }, +// { "5","5mt",_fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_5].motor_timeout, M5_MOTOR_TIMEOUT }, #endif #if (MOTORS >= 6) - { "6","6ma",_fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_6].motor_map, M6_MOTOR_MAP }, - { "6","6sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_6].step_angle, M6_STEP_ANGLE }, - { "6","6tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_6].travel_rev, M6_TRAVEL_PER_REV }, - { "6","6mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_6].microsteps, M6_MICROSTEPS }, - { "6","6su",_fipi,5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_6].steps_per_unit, M6_STEPS_PER_UNIT }, - { "6","6po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_6].polarity, M6_POLARITY }, - { "6","6pm",_fip, 0, st_print_pm, st_get_pm,st_set_pm, (float *)&cs.null, M6_POWER_MODE }, - { "6","6pl",_fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_6].power_level, M6_POWER_LEVEL }, -// { "6","6pi",_fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_6].power_idle, M6_POWER_IDLE }, -// { "6","6mt",_fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_6].motor_timeout, M6_MOTOR_TIMEOUT }, + { "6","6ma",_fip, 0, st_print_ma, get_ui8, st_set_ma, (float *)&st_cfg.mot[MOTOR_6].motor_map, M6_MOTOR_MAP }, + { "6","6sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_6].step_angle, M6_STEP_ANGLE }, + { "6","6tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_6].travel_rev, M6_TRAVEL_PER_REV }, + { "6","6mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_6].microsteps, M6_MICROSTEPS }, + { "6","6su",_fipi,5, st_print_su, st_get_su,st_set_su, (float *)&st_cfg.mot[MOTOR_6].steps_per_unit, M6_STEPS_PER_UNIT }, + { "6","6po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_6].polarity, M6_POLARITY }, + { "6","6ep",_fip, 0, st_print_ep, st_get_ep, st_set_ep, (float *)&cs.null, M6_ENABLE_POLARITY }, + { "6","6pm",_fip, 0, st_print_pm, st_get_pm, st_set_pm, (float *)&cs.null, M6_POWER_MODE }, + { "6","6pl",_fip, 3, st_print_pl, get_flt, st_set_pl, (float *)&st_cfg.mot[MOTOR_6].power_level, M6_POWER_LEVEL }, +// { "6","6pi",_fip, 3, st_print_pi, get_flt, st_set_pi, (float *)&st_cfg.mot[MOTOR_6].power_idle, M6_POWER_IDLE }, +// { "6","6mt",_fip, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.mot[MOTOR_6].motor_timeout, M6_MOTOR_TIMEOUT }, #endif // Axis parameters { "x","xam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_X].axis_mode, X_AXIS_MODE }, @@ -421,13 +427,13 @@ const cfgItem_t cfgArray[] = { { "in","in9", _f0, 0, io_print_in, io_get_input, set_ro, (float *)&cs.null, 0 }, #endif #if (D_IN_CHANNELS >= 10) - { "in","in10", _f0, 0, io_print_in, io_get_input, set_ro, (float *)&cs.null, 0 }, + { "in","in10", _f0,0, io_print_in, io_get_input, set_ro, (float *)&cs.null, 0 }, #endif #if (D_IN_CHANNELS >= 11) - { "in","in11", _f0, 0, io_print_in, io_get_input, set_ro, (float *)&cs.null, 0 }, + { "in","in11", _f0,0, io_print_in, io_get_input, set_ro, (float *)&cs.null, 0 }, #endif #if (D_IN_CHANNELS >= 12) - { "in","in12", _f0, 0, io_print_in, io_get_input, set_ro, (float *)&cs.null, 0 }, + { "in","in12", _f0,0, io_print_in, io_get_input, set_ro, (float *)&cs.null, 0 }, #endif // digital output configs @@ -849,7 +855,7 @@ const cfgItem_t cfgArray[] = { { "sys","mto", _fipn,3, cm_print_mto, get_flt,cm_set_mto,(float *)&cm.gmx.mto_factor, TRAVERSE_OVERRIDE_FACTOR}, // Power management - { "sys","mt", _fipn,2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.motor_power_timeout, MOTOR_POWER_TIMEOUT}, + { "sys","mt", _fipn,2, st_print_mt, get_flt, st_set_mt,(float *)&st_cfg.motor_power_timeout, MOTOR_POWER_TIMEOUT}, { "", "me", _f0, 0, st_print_me, st_set_me, st_set_me,(float *)&cs.null, 0 }, // SET to enable motors (null value sets to maintain compatability) { "", "md", _f0, 0, st_print_md, st_set_md, st_set_md,(float *)&cs.null, 0 }, // SET to disable motors (null value sets to maintain compatability) @@ -1060,10 +1066,10 @@ const cfgItem_t cfgArray[] = { // - Optional DIAGNOSTIC_PARAMETERS // - Uber groups (count these separately) - { "","sys",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // system group - { "","p1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // PWM 1 group + { "","sys",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // system group + { "","p1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // PWM 1 group // 2 - { "","1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // motor groups + { "","1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // motor groups { "","2", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, { "","3", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, { "","4", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, @@ -1074,7 +1080,7 @@ const cfgItem_t cfgArray[] = { { "","6", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, #endif // +4 = 6 - { "","x", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // axis groups + { "","x", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // axis groups { "","y", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, { "","z", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, { "","a", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, @@ -1107,15 +1113,15 @@ const cfgItem_t cfgArray[] = { { "","do12", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, { "","do13", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // +14 = 36 - { "","g54",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // coord offset groups + { "","g54",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // coord offset groups { "","g55",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, { "","g56",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, { "","g57",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, { "","g58",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, { "","g59",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","g92",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // origin offsets - { "","g28",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // g28 home position - { "","g30",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // g30 home position + { "","g92",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // origin offsets + { "","g28",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // g28 home position + { "","g30",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // g30 home position // +9 = 45 { "","tof",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tool offsets { "","tt1",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets @@ -1135,21 +1141,21 @@ const cfgItem_t cfgArray[] = { { "","tt15",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets { "","tt16",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets // +17 = 62 - { "","mpo",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // machine position group - { "","pos",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // work position group - { "","ofs",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // work offset group - { "","hom",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // axis homing state group - { "","prb",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // probing state group + { "","mpo",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // machine position group + { "","pos",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // work position group + { "","ofs",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // work offset group + { "","hom",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // axis homing state group + { "","prb",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // probing state group { "","pwr",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // motor power enagled group - { "","jog",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // axis jogging state group - { "","jid",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // job ID group + { "","jog",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // axis jogging state group + { "","jid",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // job ID group // +8 = 70 - { "","he1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // heater 1 group - { "","he2", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // heater 2 group - { "","he3", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // heater 3 group - { "","pid1",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // PID 1 group - { "","pid2",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // PID 2 group - { "","pid3",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // PID 3 group + { "","he1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // heater 1 group + { "","he2", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // heater 2 group + { "","he3", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // heater 3 group + { "","pid1",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // PID 1 group + { "","pid2",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // PID 2 group + { "","pid3",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // PID 3 group // +6 = 76 #ifdef __USER_DATA @@ -1226,7 +1232,7 @@ bool nv_index_lt_groups(index_t index) { return ((index <= NV_INDEX_START_GROUPS /***** APPLICATION SPECIFIC CONFIGS AND EXTENSIONS TO GENERIC FUNCTIONS *****/ /* - * set_flu() - set floating point number with G20/G21 units conversion + * set_flu() - set floating point number with G20/G21 units conversion * set_flup() - set positive floating point number with G20/G21 units conversion * set_fltp() - set positive floating point number with no units conversion * @@ -1240,10 +1246,10 @@ bool nv_index_lt_groups(index_t index) { return ((index <= NV_INDEX_START_GROUPS stat_t set_flu(nvObj_t *nv) { - if (cm_get_units_mode(MODEL) == INCHES) { // if in inches... - nv->value *= MM_PER_INCH; // convert to canonical millimeter units + if (cm_get_units_mode(MODEL) == INCHES) { // if in inches... + nv->value *= MM_PER_INCH; // convert to canonical millimeter units } - *((float *)GET_TABLE_WORD(target)) = nv->value; // write value as millimeters or degrees + *((float *)GET_TABLE_WORD(target)) = nv->value; // write value as millimeters or degrees nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; return(STAT_OK); @@ -1288,18 +1294,18 @@ void preprocess_float(nvObj_t *nv) ///+++ transform these checks into NaN or INF strings with an error return? // We may need one of two types of units conversion, but only if in inches mode - if (cm_get_units_mode(MODEL) == INCHES) { + if (cm_get_units_mode(MODEL) == INCHES) { cmAxisType type = cm_get_axis_type(nv->index); // linear, rotary or global if (cfgArray[nv->index].flags & F_CONVERT) { // standard units conversion if ((type == AXIS_TYPE_LINEAR) || (type == AXIS_TYPE_SYSTEM)) { - nv->value *= INCHES_PER_MM; - } + nv->value *= INCHES_PER_MM; + } } else if (cfgArray[nv->index].flags & F_ICONVERT) {// inverse units conversion if ((type == AXIS_TYPE_LINEAR) || (type == AXIS_TYPE_SYSTEM)) { nv->value *= MM_PER_INCH; - } - } - } + } + } + } nv->precision = GET_TABLE_WORD(precision); nv->valuetype = TYPE_FLOAT; } diff --git a/g2core/device/step_dir_driver/step_dir_driver.cpp b/g2core/device/step_dir_driver/step_dir_driver.cpp deleted file mode 100644 index b6b06db9..00000000 --- a/g2core/device/step_dir_driver/step_dir_driver.cpp +++ /dev/null @@ -1,27 +0,0 @@ -/* - * step_dir_driver.cpp - control over a Step/Direction/Enable stepper motor driver - * This file is part of the G2 project - * - * Copyright (c) 2016 Alden S. Hart, Jr. - * Copyright (c) 2016 Robert Giseburt - * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ diff --git a/g2core/device/step_dir_driver/step_dir_driver.h b/g2core/device/step_dir_driver/step_dir_driver.h index da3cfc1e..e088aa3b 100644 --- a/g2core/device/step_dir_driver/step_dir_driver.h +++ b/g2core/device/step_dir_driver/step_dir_driver.h @@ -37,6 +37,7 @@ using Motate::pin_number; using Motate::OutputPin; using Motate::PWMOutputPin; using Motate::kStartHigh; +using Motate::kStartLow; using Motate::kNormal; using Motate::Timeout; @@ -55,14 +56,21 @@ struct StepDirStepper final : Stepper { OutputPin _step; uint8_t _step_downcount; OutputPin _dir; - OutputPin _enable{kStartHigh}; + OutputPin _enable; OutputPin _ms0; OutputPin _ms1; OutputPin _ms2; PWMOutputPin _vref; + ioMode _enable_polarity; // 0=active HIGH, 1=active LOW + // sets default pwm freq for all motor vrefs (commented line below also sets HiZ) - StepDirStepper(const uint32_t frequency = 250000) : Stepper{}, _vref{kNormal, frequency} {}; + StepDirStepper(ioMode enable_polarity = IO_ACTIVE_LOW, const uint32_t frequency = 250000) : + Stepper{enable_polarity}, + _enable{enable_polarity==IO_ACTIVE_LOW?kStartHigh:kStartLow}, + _vref{kNormal, frequency}, + _enable_polarity{enable_polarity} + {}; /* Optional override of init */ @@ -114,15 +122,29 @@ struct StepDirStepper final : Stepper { }; void _enableImpl() override { +// if (!_enable.isNull()) { +// _enable.clear(); +// } if (!_enable.isNull()) { - _enable.clear(); + if (_enable_polarity == IO_ACTIVE_HIGH) { + _enable.set(); + } else { + _enable.clear(); + } } }; void _disableImpl() override { +// if (!_enable.isNull()) { +// _enable.set(); +// } if (!_enable.isNull()) { + if (_enable_polarity == IO_ACTIVE_HIGH) { + _enable.clear(); + } else { _enable.set(); } + } }; void stepStart() override { _step.set(); }; @@ -144,6 +166,19 @@ struct StepDirStepper final : Stepper { _vref = new_pl; } }; + + ioMode getEnablePolarity() override + { + return _enable_polarity; + }; + + void setEnablePolarity(ioMode new_mp) override + { + _enable_polarity = new_mp; + // this is a misnomer, but handles the logic we need for asserting the newly adjusted enable line correctly + motionStopped(); + }; + }; #endif // STEPP_DIR_DRIVER_H_ONCE diff --git a/g2core/settings/settings_default.h b/g2core/settings/settings_default.h index 68841479..8a227349 100644 --- a/g2core/settings/settings_default.h +++ b/g2core/settings/settings_default.h @@ -226,6 +226,9 @@ #ifndef M1_POLARITY #define M1_POLARITY 0 // {1po: 0=normal direction, 1=inverted direction #endif +#ifndef M1_ENABLE_POLARITY +#define M1_ENABLE_POLARITY IO_ACTIVE_LOW // {1ep: 0=active LOW, 1=active HIGH +#endif #ifndef M1_POWER_MODE #define M1_POWER_MODE MOTOR_DISABLED // {1pm: MOTOR_DISABLED, MOTOR_ALWAYS_POWERED, MOTOR_POWERED_IN_CYCLE, MOTOR_POWERED_ONLY_WHEN_MOVING #endif @@ -252,6 +255,9 @@ #ifndef M2_POLARITY #define M2_POLARITY 0 #endif +#ifndef M2_ENABLE_POLARITY +#define M2_ENABLE_POLARITY IO_ACTIVE_LOW +#endif #ifndef M2_POWER_MODE #define M2_POWER_MODE MOTOR_DISABLED #endif @@ -276,7 +282,10 @@ #define M3_STEPS_PER_UNIT 0 #endif #ifndef M3_POLARITY -#define M3_POLARITY 1 +#define M3_POLARITY 0 +#endif +#ifndef M3_ENABLE_POLARITY +#define M3_ENABLE_POLARITY IO_ACTIVE_LOW #endif #ifndef M3_POWER_MODE #define M3_POWER_MODE MOTOR_DISABLED @@ -304,6 +313,9 @@ #ifndef M4_POLARITY #define M4_POLARITY 0 #endif +#ifndef M4_ENABLE_POLARITY +#define M4_ENABLE_POLARITY IO_ACTIVE_LOW +#endif #ifndef M4_POWER_MODE #define M4_POWER_MODE MOTOR_DISABLED #endif @@ -330,6 +342,9 @@ #ifndef M5_POLARITY #define M5_POLARITY 0 #endif +#ifndef M5_ENABLE_POLARITY +#define M5_ENABLE_POLARITY IO_ACTIVE_LOW +#endif #ifndef M5_POWER_MODE #define M5_POWER_MODE MOTOR_DISABLED #endif @@ -356,6 +371,9 @@ #ifndef M6_POLARITY #define M6_POLARITY 0 #endif +#ifndef M6_ENABLE_POLARITY +#define M6_ENABLE_POLARITY IO_ACTIVE_LOW +#endif #ifndef M6_POWER_MODE #define M6_POWER_MODE MOTOR_DISABLED #endif diff --git a/g2core/stepper.cpp b/g2core/stepper.cpp index 5c2a6032..8961735b 100644 --- a/g2core/stepper.cpp +++ b/g2core/stepper.cpp @@ -336,7 +336,7 @@ void dda_timer_type::interrupt() } #endif - // Process end of segment. + // Process end of segment. // One more interrupt will occur to turn of any pulses set in this pass. if (--st_run.dda_ticks_downcount == 0) { _load_move(); // load the next move at the current interrupt level @@ -451,7 +451,7 @@ static void _load_move() // ...start motor power timeouts // for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) { - // Motors[motor]->motionStopped(); + // Motors[motor]->motionStopped(); // } // loop unrolled version motor_1.motionStopped(); // ...start motor power timeouts @@ -626,7 +626,7 @@ static void _load_move() // handle synchronous commands } else if (st_pre.block_type == BLOCK_TYPE_COMMAND) { mp_runtime_command(st_pre.bf); - + } // else null - which is okay in many cases // all other cases drop to here (e.g. Null moves after Mcodes skip to here) @@ -926,7 +926,7 @@ stat_t st_set_su(nvObj_t *nv) // motor steps per unit (direct) if (cm_get_axis_type(nv->index) == AXIS_TYPE_LINEAR) { nv->value *= INCHES_PER_MM; } - } + } set_flt(nv); st_cfg.mot[m].units_per_step = 1.0/st_cfg.mot[m].steps_per_unit; @@ -936,20 +936,45 @@ stat_t st_set_su(nvObj_t *nv) // motor steps per unit (direct) return(STAT_OK); } +stat_t st_set_ep(nvObj_t *nv) // set motor enable polarity +{ + if (nv->value < IO_ACTIVE_LOW) { return (STAT_INPUT_LESS_THAN_MIN_VALUE); } + if (nv->value > IO_ACTIVE_HIGH) { return (STAT_INPUT_EXCEEDS_MAX_VALUE); } + + uint8_t motor = _get_motor(nv->index); + if (motor > MOTORS) { return STAT_INPUT_VALUE_RANGE_ERROR; }; + + Motors[motor]->setEnablePolarity((ioMode)nv->value); + return (STAT_OK); +} + +stat_t st_get_ep(nvObj_t *nv) // get motor enable polarity +{ + if (nv->value < IO_ACTIVE_LOW) { return (STAT_INPUT_LESS_THAN_MIN_VALUE); } + if (nv->value > IO_ACTIVE_HIGH) { return (STAT_INPUT_EXCEEDS_MAX_VALUE); } + + uint8_t motor = _get_motor(nv->index); + if (motor > MOTORS) { return STAT_INPUT_VALUE_RANGE_ERROR; }; + + nv->value = (float)Motors[motor]->getEnablePolarity(); + nv->valuetype = TYPE_INT; + return (STAT_OK); +} + stat_t st_set_pm(nvObj_t *nv) // set motor power mode { if (nv->value < 0) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_LESS_THAN_MIN_VALUE); } - if (nv->value >= MOTOR_POWER_MODE_MAX_VALUE) { + if (nv->value >= MOTOR_POWER_MODE_MAX_VALUE) { nv->valuetype = TYPE_NULL; - return (STAT_INPUT_EXCEEDS_MAX_VALUE); + return (STAT_INPUT_EXCEEDS_MAX_VALUE); } uint8_t motor = _get_motor(nv->index); if (motor > MOTORS) { nv->valuetype = TYPE_NULL; - return STAT_INPUT_VALUE_RANGE_ERROR; + return STAT_INPUT_VALUE_RANGE_ERROR; }; // We do this *here* in order for this to take effect immediately. @@ -963,7 +988,7 @@ stat_t st_get_pm(nvObj_t *nv) // get motor power mode uint8_t motor = _get_motor(nv->index); if (motor > MOTORS) { nv->valuetype = TYPE_NULL; - return STAT_INPUT_VALUE_RANGE_ERROR; + return STAT_INPUT_VALUE_RANGE_ERROR; }; nv->value = (float)Motors[motor]->getPowerMode(); @@ -982,7 +1007,7 @@ stat_t st_set_pl(nvObj_t *nv) // motor power level { if (nv->value < (float)0.0) { nv->valuetype = TYPE_NULL; - return (STAT_INPUT_LESS_THAN_MIN_VALUE); + return (STAT_INPUT_LESS_THAN_MIN_VALUE); } if (nv->value > (float)1.0) { nv->valuetype = TYPE_NULL; @@ -1044,7 +1069,7 @@ stat_t st_set_mt(nvObj_t *nv) // Make sure this function is not part of initialization --> f00 // nv->value is seconds of timeout -stat_t st_set_me(nvObj_t *nv) +stat_t st_set_me(nvObj_t *nv) { for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) { Motors[motor]->enable(nv->value); // nv->value is the timeout or 0 for default @@ -1054,7 +1079,7 @@ stat_t st_set_me(nvObj_t *nv) // Make sure this function is not part of initialization --> f00 // nv-value is motor to disable, or 0 for all motors -stat_t st_set_md(nvObj_t *nv) +stat_t st_set_md(nvObj_t *nv) { if (nv->value < 0) { nv->valuetype = TYPE_NULL; @@ -1063,7 +1088,7 @@ stat_t st_set_md(nvObj_t *nv) if (nv->value > MOTORS) { nv->valuetype = TYPE_NULL; return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } + } // de-energize all motors if ((uint8_t)nv->value == 0) { // 0 means all motors for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) { @@ -1097,6 +1122,7 @@ static const char fmt_0tr[] = "[%s%s] m%s travel per revolution%10.4f%s\n"; static const char fmt_0mi[] = "[%s%s] m%s microsteps%16d [1,2,4,8,16,32]\n"; static const char fmt_0su[] = "[%s%s] m%s steps per unit %17.5f steps per%s\n"; static const char fmt_0po[] = "[%s%s] m%s polarity%18d [0=normal,1=reverse]\n"; +static const char fmt_0ep[] = "[%s%s] m%s enable polarity%11d [0=active HIGH,1=ractive LOW]\n"; static const char fmt_0pm[] = "[%s%s] m%s power management%10d [0=disabled,1=always on,2=in cycle,3=when moving]\n"; static const char fmt_0pl[] = "[%s%s] m%s motor power level%13.3f [0.000=minimum, 1.000=maximum]\n"; static const char fmt_pwr[] = "[%s%s] Motor %c power level:%12.3f\n"; @@ -1135,6 +1161,7 @@ void st_print_tr(nvObj_t *nv) { _print_motor_flt_units(nv, fmt_0tr, cm_get_units void st_print_mi(nvObj_t *nv) { _print_motor_int(nv, fmt_0mi);} void st_print_su(nvObj_t *nv) { _print_motor_flt_units(nv, fmt_0su, cm_get_units_mode(MODEL));} void st_print_po(nvObj_t *nv) { _print_motor_int(nv, fmt_0po);} +void st_print_ep(nvObj_t *nv) { _print_motor_int(nv, fmt_0ep);} void st_print_pm(nvObj_t *nv) { _print_motor_int(nv, fmt_0pm);} void st_print_pl(nvObj_t *nv) { _print_motor_flt(nv, fmt_0pl);} void st_print_pwr(nvObj_t *nv){ _print_motor_pwr(nv, fmt_pwr);} diff --git a/g2core/stepper.h b/g2core/stepper.h index 5734ef0e..011e41fb 100644 --- a/g2core/stepper.h +++ b/g2core/stepper.h @@ -255,6 +255,7 @@ #define STEPPER_H_ONCE #include "planner.h" // planner.h must precede stepper.h for moveType typedef +#include "gpio.h" // for IO_ACTIVE_HIGH/IO_ACTIVE_LOW /********************************* * Stepper configs and constants * @@ -336,7 +337,7 @@ typedef enum { * There are 5 main structures involved in stepper operations; * * data structure: found in: runs primarily at: - * mpBuffer planning buffers (bf) planner.c main loop + * mpBuffer planning buffers (bf) planner.c main loop * mrRuntimeSingleton (mr) planner.c MED ISR * stConfig (st_cfg) stepper.c write=bkgd, read=ISRs * stPrepSingleton (st_pre) stepper.c MED ISR @@ -430,17 +431,15 @@ extern stPrepSingleton_t st_pre; // only used by config_app diagnosti /**** Stepper (base object) ****/ struct Stepper { +protected: Timeout _motor_disable_timeout; // this is the timeout object that will let us know when time is u uint32_t _motor_disable_timeout_ms; // the number of ms that the timeout is reset to stPowerState _power_state; // state machine for managing motor power stPowerMode _power_mode; // See stPowerMode for values /* stepper default values */ - - // sets default pwm freq for all motor vrefs (commented line below also sets HiZ) - Stepper(const uint32_t frequency = 500000) - { - }; +public: + Stepper(ioMode enable_polarity = IO_ACTIVE_LOW) {}; /* Functions that handle all motor functions (calls virtuals if needed) */ @@ -459,6 +458,16 @@ struct Stepper { } }; + virtual ioMode getEnablePolarity() + { + return IO_ACTIVE_LOW; // we have to say something here + }; + + virtual void setEnablePolarity(ioMode new_mp) + { + // do nothing + }; + virtual stPowerMode getPowerMode() { return _power_mode; @@ -471,7 +480,7 @@ struct Stepper { } if (_power_state == MOTOR_IDLE) { return (0.0); - } + } return (st_cfg.mot[motor].power_level); }; @@ -479,7 +488,7 @@ struct Stepper { // { // return (_power_mode == MOTOR_DISABLED); // }; - + // turn on motor in all cases unless it's disabled // NOTE: in the future the default assigned timeout will be the motor's default value void enable(float timeout = st_cfg.motor_power_timeout) @@ -506,7 +515,7 @@ struct Stepper { _motor_disable_timeout.clear(); _power_state = MOTOR_IDLE; // or MOTOR_OFF }; - + // turn off motor is only powered when moving void motionStopped() { if (_power_mode == MOTOR_POWERED_IN_CYCLE) { @@ -518,7 +527,7 @@ struct Stepper { } } }; - + virtual void periodicCheck(bool have_actually_stopped) // can be overridden { if (have_actually_stopped && _power_state == MOTOR_RUNNING) { @@ -585,6 +594,8 @@ stat_t st_set_tr(nvObj_t *nv); stat_t st_set_mi(nvObj_t *nv); stat_t st_get_su(nvObj_t *nv); stat_t st_set_su(nvObj_t *nv); +stat_t st_set_ep(nvObj_t *nv); +stat_t st_get_ep(nvObj_t *nv); stat_t st_set_pm(nvObj_t *nv); stat_t st_get_pm(nvObj_t *nv); stat_t st_set_pl(nvObj_t *nv); @@ -602,6 +613,7 @@ stat_t st_set_me(nvObj_t *nv); void st_print_mi(nvObj_t *nv); void st_print_su(nvObj_t *nv); void st_print_po(nvObj_t *nv); + void st_print_ep(nvObj_t *nv); void st_print_pm(nvObj_t *nv); void st_print_pl(nvObj_t *nv); void st_print_pwr(nvObj_t *nv); @@ -617,6 +629,7 @@ stat_t st_set_me(nvObj_t *nv); #define st_print_mi tx_print_stub #define st_print_su tx_print_stub #define st_print_po tx_print_stub + #define st_print_ep tx_print_stub #define st_print_pm tx_print_stub #define st_print_pl tx_print_stub #define st_print_pwr tx_print_stub