Merged edge into dev-304, leaving old code commented out. SUccessful build, but has not yet been validated against test suite

This commit is contained in:
Alden Hart
2018-03-26 10:38:57 -04:00
11 changed files with 514 additions and 76 deletions

View File

@@ -29,7 +29,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<Motate::kSocket1_StepPinNumber,
Motate::kSocket1_DirPinNumber,
Motate::kSocket1_EnablePinNumber,
@@ -37,7 +37,7 @@ StepDirStepper<Motate::kSocket1_StepPinNumber,
Motate::kSocket1_Microstep_1PinNumber,
Motate::kSocket1_Microstep_2PinNumber,
Motate::kSocket1_VrefPinNumber>
motor_1{};
motor_1{M1_ENABLE_POLARITY};
StepDirStepper<Motate::kSocket2_StepPinNumber,
Motate::kSocket2_DirPinNumber,
@@ -46,7 +46,7 @@ StepDirStepper<Motate::kSocket2_StepPinNumber,
Motate::kSocket2_Microstep_1PinNumber,
Motate::kSocket2_Microstep_2PinNumber,
Motate::kSocket2_VrefPinNumber>
motor_2{};
motor_2{M2_ENABLE_POLARITY};
StepDirStepper<Motate::kSocket3_StepPinNumber,
Motate::kSocket3_DirPinNumber,
@@ -55,7 +55,7 @@ StepDirStepper<Motate::kSocket3_StepPinNumber,
Motate::kSocket3_Microstep_1PinNumber,
Motate::kSocket3_Microstep_2PinNumber,
Motate::kSocket3_VrefPinNumber>
motor_3{};
motor_3{M3_ENABLE_POLARITY};
StepDirStepper<Motate::kSocket4_StepPinNumber,
Motate::kSocket4_DirPinNumber,
@@ -64,7 +64,7 @@ StepDirStepper<Motate::kSocket4_StepPinNumber,
Motate::kSocket4_Microstep_1PinNumber,
Motate::kSocket4_Microstep_2PinNumber,
Motate::kSocket4_VrefPinNumber>
motor_4{};
motor_4{M4_ENABLE_POLARITY};
// StepDirStepper<
// Motate::kSocket5_StepPinNumber,
@@ -73,7 +73,8 @@ StepDirStepper<Motate::kSocket4_StepPinNumber,
// Motate::kSocket5_Microstep_0PinNumber,
// Motate::kSocket5_Microstep_1PinNumber,
// Motate::kSocket5_Microstep_2PinNumber,
// Motate::kSocket5_VrefPinNumber> motor_5 {};
// Motate::kSocket5_VrefPinNumber>
// motor_5 {M5_ENABLE_POLARITY};
// StepDirStepper<
// Motate::kSocket6_StepPinNumber,
@@ -82,7 +83,8 @@ StepDirStepper<Motate::kSocket4_StepPinNumber,
// Motate::kSocket6_Microstep_0PinNumber,
// Motate::kSocket6_Microstep_1PinNumber,
// Motate::kSocket6_Microstep_2PinNumber,
// Motate::kSocket6_VrefPinNumber> motor_6 {};
// Motate::kSocket6_VrefPinNumber>
// motor_6 {M6_ENABLE_POLARITY};
Stepper* Motors[MOTORS] = {&motor_1, &motor_2, &motor_3, &motor_4};

View File

@@ -29,7 +29,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<Motate::kSocket1_StepPinNumber,
Motate::kSocket1_DirPinNumber,
Motate::kSocket1_EnablePinNumber,
@@ -37,7 +37,7 @@ StepDirStepper<Motate::kSocket1_StepPinNumber,
Motate::kSocket1_Microstep_1PinNumber,
Motate::kSocket1_Microstep_2PinNumber,
Motate::kSocket1_VrefPinNumber>
motor_1{};
motor_1{M1_ENABLE_POLARITY};
StepDirStepper<Motate::kSocket2_StepPinNumber,
Motate::kSocket2_DirPinNumber,
@@ -46,7 +46,7 @@ StepDirStepper<Motate::kSocket2_StepPinNumber,
Motate::kSocket2_Microstep_1PinNumber,
Motate::kSocket2_Microstep_2PinNumber,
Motate::kSocket2_VrefPinNumber>
motor_2{};
motor_2{M2_ENABLE_POLARITY};
StepDirStepper<Motate::kSocket3_StepPinNumber,
Motate::kSocket3_DirPinNumber,
@@ -55,7 +55,7 @@ StepDirStepper<Motate::kSocket3_StepPinNumber,
Motate::kSocket3_Microstep_1PinNumber,
Motate::kSocket3_Microstep_2PinNumber,
Motate::kSocket3_VrefPinNumber>
motor_3{};
motor_3{M3_ENABLE_POLARITY};
StepDirStepper<Motate::kSocket4_StepPinNumber,
Motate::kSocket4_DirPinNumber,
@@ -64,7 +64,7 @@ StepDirStepper<Motate::kSocket4_StepPinNumber,
Motate::kSocket4_Microstep_1PinNumber,
Motate::kSocket4_Microstep_2PinNumber,
Motate::kSocket4_VrefPinNumber>
motor_4{};
motor_4{M4_ENABLE_POLARITY};
// StepDirStepper<
// Motate::kSocket5_StepPinNumber,
@@ -73,7 +73,8 @@ StepDirStepper<Motate::kSocket4_StepPinNumber,
// Motate::kSocket5_Microstep_0PinNumber,
// Motate::kSocket5_Microstep_1PinNumber,
// Motate::kSocket5_Microstep_2PinNumber,
// Motate::kSocket5_VrefPinNumber> motor_5 {};
// Motate::kSocket5_VrefPinNumber>
// motor_5 {M5_ENABLE_POLARITY};
// StepDirStepper<
// Motate::kSocket6_StepPinNumber,
@@ -82,7 +83,8 @@ StepDirStepper<Motate::kSocket4_StepPinNumber,
// Motate::kSocket6_Microstep_0PinNumber,
// Motate::kSocket6_Microstep_1PinNumber,
// Motate::kSocket6_Microstep_2PinNumber,
// Motate::kSocket6_VrefPinNumber> motor_6 {};
// Motate::kSocket6_VrefPinNumber>
// motor_6 {M6_ENABLE_POLARITY};
Stepper* Motors[MOTORS] = {&motor_1, &motor_2, &motor_3, &motor_4};

View File

@@ -29,7 +29,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<Motate::kSocket1_StepPinNumber,
Motate::kSocket1_DirPinNumber,
Motate::kSocket1_EnablePinNumber,
@@ -37,7 +37,7 @@ StepDirStepper<Motate::kSocket1_StepPinNumber,
Motate::kSocket1_Microstep_1PinNumber,
Motate::kSocket1_Microstep_2PinNumber,
Motate::kSocket1_VrefPinNumber>
motor_1{};
motor_1{M1_ENABLE_POLARITY};
StepDirStepper<Motate::kSocket2_StepPinNumber,
Motate::kSocket2_DirPinNumber,
@@ -46,7 +46,7 @@ StepDirStepper<Motate::kSocket2_StepPinNumber,
Motate::kSocket2_Microstep_1PinNumber,
Motate::kSocket2_Microstep_2PinNumber,
Motate::kSocket2_VrefPinNumber>
motor_2{};
motor_2{M2_ENABLE_POLARITY};
StepDirHobbyServo<Motate::kServo1_PinNumber> motor_3;

View File

@@ -29,7 +29,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<Motate::kSocket1_StepPinNumber,
Motate::kSocket1_DirPinNumber,
Motate::kSocket1_EnablePinNumber,
@@ -37,7 +37,7 @@ StepDirStepper<Motate::kSocket1_StepPinNumber,
Motate::kSocket1_Microstep_1PinNumber,
Motate::kSocket1_Microstep_2PinNumber,
Motate::kSocket1_VrefPinNumber>
motor_1{};
motor_1{M1_ENABLE_POLARITY};
StepDirStepper<Motate::kSocket2_StepPinNumber,
Motate::kSocket2_DirPinNumber,
@@ -46,7 +46,7 @@ StepDirStepper<Motate::kSocket2_StepPinNumber,
Motate::kSocket2_Microstep_1PinNumber,
Motate::kSocket2_Microstep_2PinNumber,
Motate::kSocket2_VrefPinNumber>
motor_2{};
motor_2{M2_ENABLE_POLARITY};
StepDirStepper<Motate::kSocket3_StepPinNumber,
Motate::kSocket3_DirPinNumber,
@@ -55,7 +55,7 @@ StepDirStepper<Motate::kSocket3_StepPinNumber,
Motate::kSocket3_Microstep_1PinNumber,
Motate::kSocket3_Microstep_2PinNumber,
Motate::kSocket3_VrefPinNumber>
motor_3{};
motor_3{M3_ENABLE_POLARITY};
StepDirStepper<Motate::kSocket4_StepPinNumber,
Motate::kSocket4_DirPinNumber,
@@ -64,7 +64,7 @@ StepDirStepper<Motate::kSocket4_StepPinNumber,
Motate::kSocket4_Microstep_1PinNumber,
Motate::kSocket4_Microstep_2PinNumber,
Motate::kSocket4_VrefPinNumber>
motor_4{};
motor_4{M4_ENABLE_POLARITY};
// StepDirStepper<
// Motate::kSocket5_StepPinNumber,
@@ -73,7 +73,8 @@ StepDirStepper<Motate::kSocket4_StepPinNumber,
// Motate::kSocket5_Microstep_0PinNumber,
// Motate::kSocket5_Microstep_1PinNumber,
// Motate::kSocket5_Microstep_2PinNumber,
// Motate::kSocket5_VrefPinNumber> motor_5 {};
// Motate::kSocket5_VrefPinNumber>
// motor_5 {M5_ENABLE_POLARITY};
// StepDirStepper<
// Motate::kSocket6_StepPinNumber,
@@ -82,7 +83,8 @@ StepDirStepper<Motate::kSocket4_StepPinNumber,
// Motate::kSocket6_Microstep_0PinNumber,
// Motate::kSocket6_Microstep_1PinNumber,
// Motate::kSocket6_Microstep_2PinNumber,
// Motate::kSocket6_VrefPinNumber> motor_6 {};
// Motate::kSocket6_VrefPinNumber>
// motor_6 {M6_ENABLE_POLARITY};
Stepper* Motors[MOTORS] = {&motor_1, &motor_2, &motor_3, &motor_4};

View File

@@ -29,7 +29,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<Motate::kSocket1_StepPinNumber,
Motate::kSocket1_DirPinNumber,
Motate::kSocket1_EnablePinNumber,
@@ -37,7 +37,7 @@ StepDirStepper<Motate::kSocket1_StepPinNumber,
Motate::kSocket1_Microstep_1PinNumber,
Motate::kSocket1_Microstep_2PinNumber,
Motate::kSocket1_VrefPinNumber>
motor_1{};
motor_1{M1_ENABLE_POLARITY};
StepDirStepper<Motate::kSocket2_StepPinNumber,
Motate::kSocket2_DirPinNumber,
@@ -46,7 +46,7 @@ StepDirStepper<Motate::kSocket2_StepPinNumber,
Motate::kSocket2_Microstep_1PinNumber,
Motate::kSocket2_Microstep_2PinNumber,
Motate::kSocket2_VrefPinNumber>
motor_2{};
motor_2{M2_ENABLE_POLARITY};
StepDirStepper<Motate::kSocket3_StepPinNumber,
Motate::kSocket3_DirPinNumber,
@@ -55,7 +55,7 @@ StepDirStepper<Motate::kSocket3_StepPinNumber,
Motate::kSocket3_Microstep_1PinNumber,
Motate::kSocket3_Microstep_2PinNumber,
Motate::kSocket3_VrefPinNumber>
motor_3{};
motor_3{M3_ENABLE_POLARITY};
StepDirStepper<Motate::kSocket4_StepPinNumber,
Motate::kSocket4_DirPinNumber,
@@ -64,7 +64,7 @@ StepDirStepper<Motate::kSocket4_StepPinNumber,
Motate::kSocket4_Microstep_1PinNumber,
Motate::kSocket4_Microstep_2PinNumber,
Motate::kSocket4_VrefPinNumber>
motor_4{};
motor_4{M4_ENABLE_POLARITY};
// StepDirStepper<
// Motate::kSocket5_StepPinNumber,
@@ -73,7 +73,8 @@ StepDirStepper<Motate::kSocket4_StepPinNumber,
// Motate::kSocket5_Microstep_0PinNumber,
// Motate::kSocket5_Microstep_1PinNumber,
// Motate::kSocket5_Microstep_2PinNumber,
// Motate::kSocket5_VrefPinNumber> motor_5 {};
// Motate::kSocket5_VrefPinNumber>
// motor_5 {M5_ENABLE_POLARITY};
// StepDirStepper<
// Motate::kSocket6_StepPinNumber,
@@ -82,7 +83,8 @@ StepDirStepper<Motate::kSocket4_StepPinNumber,
// Motate::kSocket6_Microstep_0PinNumber,
// Motate::kSocket6_Microstep_1PinNumber,
// Motate::kSocket6_Microstep_2PinNumber,
// Motate::kSocket6_VrefPinNumber> motor_6 {};
// Motate::kSocket6_VrefPinNumber>
// motor_6 {M6_ENABLE_POLARITY};
Stepper* Motors[MOTORS] = {&motor_1, &motor_2, &motor_3, &motor_4};

View File

@@ -137,6 +137,7 @@ const cfgItem_t cfgArray[] = {
{ "", "_leds",_i0, 0, tx_print_nul, _get_leds,_set_leds, nullptr, 0 }, // TEMPORARY - change LEDs
#endif
// <<<<<<< HEAD
{ "mpo","mpox",_f0, 5, cm_print_mpo, cm_get_mpo, set_ro, nullptr, 0 }, // X machine position
{ "mpo","mpoy",_f0, 5, cm_print_mpo, cm_get_mpo, set_ro, nullptr, 0 }, // Y machine position
{ "mpo","mpoz",_f0, 5, cm_print_mpo, cm_get_mpo, set_ro, nullptr, 0 }, // Z machine position
@@ -201,6 +202,56 @@ const cfgItem_t cfgArray[] = {
{ "pwr","pwr1",_f0, 3, st_print_pwr, st_get_pwr, set_ro, nullptr, 0}, // motor power readouts
{ "pwr","pwr2",_f0, 3, st_print_pwr, st_get_pwr, set_ro, nullptr, 0},
/* =======
{ "mpo","mpox",_f0, 3, cm_print_mpo, cm_get_mpo, set_ro, (float *)&cs.null, 0 }, // X machine position
{ "mpo","mpoy",_f0, 3, cm_print_mpo, cm_get_mpo, set_ro, (float *)&cs.null, 0 }, // Y machine position
{ "mpo","mpoz",_f0, 3, cm_print_mpo, cm_get_mpo, set_ro, (float *)&cs.null, 0 }, // Z machine position
{ "mpo","mpoa",_f0, 3, cm_print_mpo, cm_get_mpo, set_ro, (float *)&cs.null, 0 }, // A machine position
{ "mpo","mpob",_f0, 3, cm_print_mpo, cm_get_mpo, set_ro, (float *)&cs.null, 0 }, // B machine position
{ "mpo","mpoc",_f0, 3, cm_print_mpo, cm_get_mpo, set_ro, (float *)&cs.null, 0 }, // C machine position
{ "pos","posx",_f0, 3, cm_print_pos, cm_get_pos, set_ro, (float *)&cs.null, 0 }, // X work position
{ "pos","posy",_f0, 3, cm_print_pos, cm_get_pos, set_ro, (float *)&cs.null, 0 }, // Y work position
{ "pos","posz",_f0, 3, cm_print_pos, cm_get_pos, set_ro, (float *)&cs.null, 0 }, // Z work position
{ "pos","posa",_f0, 3, cm_print_pos, cm_get_pos, set_ro, (float *)&cs.null, 0 }, // A work position
{ "pos","posb",_f0, 3, cm_print_pos, cm_get_pos, set_ro, (float *)&cs.null, 0 }, // B work position
{ "pos","posc",_f0, 3, cm_print_pos, cm_get_pos, set_ro, (float *)&cs.null, 0 }, // C work position
{ "ofs","ofsx",_f0, 3, cm_print_ofs, cm_get_ofs, set_ro, (float *)&cs.null, 0 }, // X work offset
{ "ofs","ofsy",_f0, 3, cm_print_ofs, cm_get_ofs, set_ro, (float *)&cs.null, 0 }, // Y work offset
{ "ofs","ofsz",_f0, 3, cm_print_ofs, cm_get_ofs, set_ro, (float *)&cs.null, 0 }, // Z work offset
{ "ofs","ofsa",_f0, 3, cm_print_ofs, cm_get_ofs, set_ro, (float *)&cs.null, 0 }, // A work offset
{ "ofs","ofsb",_f0, 3, cm_print_ofs, cm_get_ofs, set_ro, (float *)&cs.null, 0 }, // B work offset
{ "ofs","ofsc",_f0, 3, cm_print_ofs, cm_get_ofs, set_ro, (float *)&cs.null, 0 }, // C work offset
{ "hom","home",_f0, 0, cm_print_home,cm_get_home,set_01,(float *)&cm.homing_state, 0 }, // homing state, invoke homing cycle
{ "hom","homx",_f0, 0, cm_print_hom, get_ui8, set_01, (float *)&cm.homed[AXIS_X], false }, // X homed - Homing status group
{ "hom","homy",_f0, 0, cm_print_hom, get_ui8, set_01, (float *)&cm.homed[AXIS_Y], false }, // Y homed
{ "hom","homz",_f0, 0, cm_print_hom, get_ui8, set_01, (float *)&cm.homed[AXIS_Z], false }, // Z homed
{ "hom","homa",_f0, 0, cm_print_hom, get_ui8, set_01, (float *)&cm.homed[AXIS_A], false }, // A homed
{ "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","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
{ "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},
{ "jog","jogz",_f0, 0, tx_print_nul, get_nul, cm_run_jogz, (float *)&cm.jogging_dest, 0},
{ "jog","joga",_f0, 0, tx_print_nul, get_nul, cm_run_joga, (float *)&cm.jogging_dest, 0},
// { "jog","jogb",_f0, 0, tx_print_nul, get_nul, cm_run_jogb, (float *)&cm.jogging_dest, 0},
// { "jog","jogc",_f0, 0, tx_print_nul, get_nul, cm_run_jogc, (float *)&cm.jogging_dest, 0},
{ "pwr","pwr1",_f0, 3, st_print_pwr, st_get_pwr, set_ro, (float *)&cs.null, 0}, // motor power readouts
{ "pwr","pwr2",_f0, 3, st_print_pwr, st_get_pwr, set_ro, (float *)&cs.null, 0},
>>>>>>> refs/heads/edge
*/
#if (MOTORS > 2)
{ "pwr","pwr3",_f0, 3, st_print_pwr, st_get_pwr, set_ro, nullptr, 0},
#endif
@@ -215,6 +266,7 @@ const cfgItem_t cfgArray[] = {
#endif
// Motor parameters
// <<<<<<< HEAD
{ "1","1ma",_iip, 0, st_print_ma, st_get_ma, st_set_ma, nullptr, M1_MOTOR_MAP },
{ "1","1sa",_fip, 3, st_print_sa, st_get_sa, st_set_sa, nullptr, M1_STEP_ANGLE },
{ "1","1tr",_fipc,5, st_print_tr, st_get_tr, st_set_tr, nullptr, M1_TRAVEL_PER_REV },
@@ -284,6 +336,85 @@ const cfgItem_t cfgArray[] = {
{ "6","6pl",_fip, 3, st_print_pl, st_get_pl, st_set_pl, nullptr, M6_POWER_LEVEL },
// { "6","6pi",_fip, 3, st_print_pi, st_get_pi, st_set_pi, (float *)&st_cfg.mot[MOTOR_6].power_idle, M6_POWER_IDLE },
// { "6","6mt",_fip, 2, st_print_mt, st_get_mt, st_set_mt, (float *)&st_cfg.mot[MOTOR_6].motor_timeout, M6_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","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","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","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","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","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 },
>>>>>>> refs/heads/edge
*/
#endif
// Axis parameters
@@ -482,6 +613,7 @@ const cfgItem_t cfgArray[] = {
{ "in","in9", _i0, 0, io_print_in, io_get_input, set_ro, nullptr, 0 },
#endif
#if (D_IN_CHANNELS >= 10)
// <<<<<<< HEAD
{ "in","in10", _i0, 0, io_print_in, io_get_input, set_ro, nullptr, 0 },
#endif
#if (D_IN_CHANNELS >= 11)
@@ -489,6 +621,17 @@ const cfgItem_t cfgArray[] = {
#endif
#if (D_IN_CHANNELS >= 12)
{ "in","in12", _i0, 0, io_print_in, io_get_input, set_ro, nullptr, 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 },
#endif
#if (D_IN_CHANNELS >= 12)
{ "in","in12", _f0,0, io_print_in, io_get_input, set_ro, (float *)&cs.null, 0 },
>>>>>>> refs/heads/edge
*/
#endif
// digital output configs
@@ -686,11 +829,37 @@ const cfgItem_t cfgArray[] = {
{ "g30","g30c",_fic, 5, cm_print_cpos, cm_get_g30, set_ro, nullptr, 0 },
// this is a 128bit UUID for identifying a previously committed job state
// <<<<<<< HEAD
{ "jid","jida",_d0, 0, tx_print_nul, get_data, set_data, (float *)&cfg.job_id[0], 0 },
{ "jid","jidb",_d0, 0, tx_print_nul, get_data, set_data, (float *)&cfg.job_id[1], 0 },
{ "jid","jidc",_d0, 0, tx_print_nul, get_data, set_data, (float *)&cfg.job_id[2], 0 },
{ "jid","jidd",_d0, 0, tx_print_nul, get_data, set_data, (float *)&cfg.job_id[3], 0 },
/* =======
{ "jid","jida",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cfg.job_id[0], 0},
{ "jid","jidb",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cfg.job_id[1], 0},
{ "jid","jidc",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cfg.job_id[2], 0},
{ "jid","jidd",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cfg.job_id[3], 0},
// General system parameters
{ "sys","jt", _fipn, 2, cm_print_jt, get_flt, cm_set_jt,(float *)&cm.junction_integration_time,JUNCTION_INTEGRATION_TIME },
{ "sys","ct", _fipnc,4, cm_print_ct, get_flt, set_flup, (float *)&cm.chordal_tolerance, CHORDAL_TOLERANCE },
{ "sys","sl", _fipn, 0, cm_print_sl, get_ui8, set_01, (float *)&cm.soft_limit_enable, SOFT_LIMIT_ENABLE },
{ "sys","lim", _fipn,0, cm_print_lim, get_ui8, set_01, (float *)&cm.limit_enable, HARD_LIMIT_ENABLE },
{ "sys","saf", _fipn,0, cm_print_saf, get_ui8, set_01, (float *)&cm.safety_interlock_enable, SAFETY_INTERLOCK_ENABLE },
{ "sys","m48e",_fipn,0, cm_print_m48e,get_ui8, set_01, (float *)&cm.gmx.m48_enable, 0 }, // M48/M49 feedrate & spindle override enable
{ "sys","mfoe",_fipn,0, cm_print_mfoe,get_ui8, set_01, (float *)&cm.gmx.mfo_enable, FEED_OVERRIDE_ENABLE},
{ "sys","mfo", _fipn,3, cm_print_mfo, get_flt,cm_set_mfo,(float *)&cm.gmx.mfo_factor, FEED_OVERRIDE_FACTOR},
{ "sys","mtoe",_fipn,0, cm_print_mtoe,get_ui8, set_01, (float *)&cm.gmx.mto_enable, TRAVERSE_OVERRIDE_ENABLE},
{ "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},
{ "", "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)
>>>>>>> refs/heads/edge
*/
// Spindle functions
{ "sp","spmo", _iip, 0, sp_print_spmo, sp_get_spmo, sp_set_spmo, nullptr, SPINDLE_MODE },
{ "sp","spph", _bip, 0, sp_print_spph, sp_get_spph, sp_set_spph, nullptr, SPINDLE_PAUSE_ON_HOLD },
@@ -1254,6 +1423,7 @@ const cfgItem_t cfgArray[] = {
// - Optional DIAGNOSTIC_PARAMETERS
// - Uber groups (count these separately)
// <<<<<<< HEAD
#define FIXED_GROUPS 4
{ "","sys",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // system group
{ "","p1", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // PWM 1 group
@@ -1282,6 +1452,17 @@ const cfgItem_t cfgArray[] = {
#if (MOTORS >= 4)
{ "","4", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 },
#endif
/* =======
{ "","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
{ "","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 },
>>>>>>> refs/heads/edge
*/
#if (MOTORS >= 5)
{ "","5", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 },
#endif
@@ -1289,6 +1470,7 @@ const cfgItem_t cfgArray[] = {
{ "","6", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 },
#endif
// <<<<<<< HEAD
#define DIGITAL_IN_GROUPS 10
{ "","in", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // input state
{ "","di1", _f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // input configs
@@ -1381,6 +1563,87 @@ const cfgItem_t cfgArray[] = {
{ "","pid2",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // PID 2 group
{ "","pid3",_f0, 0, tx_print_nul, get_grp, set_grp, nullptr, 0 }, // PID 3 group
/* =======
// +4 = 6
{ "","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 },
{ "","b", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","c", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
// +6 = 12
{ "","in", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // input state
{ "","di1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // input configs
{ "","di2", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","di3", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","di4", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","di5", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","di6", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","di7", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","di8", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","di9", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
// +10 = 22
{ "","out", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // output state
{ "","do1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // output configs
{ "","do2", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","do3", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","do4", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","do5", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","do6", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","do7", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","do8", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","do9", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","do10", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","do11", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 },
{ "","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
{ "","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
// +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
{ "","tt2",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets
{ "","tt3",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets
{ "","tt4",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets
{ "","tt5",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets
{ "","tt6",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets
{ "","tt7",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets
{ "","tt8",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets
{ "","tt9",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets
{ "","tt10",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets
{ "","tt11",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets
{ "","tt12",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets
{ "","tt13",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets
{ "","tt14",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // tt offsets
{ "","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
{ "","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
// +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
// +6 = 76
>>>>>>> refs/heads/edge
*/
#ifdef __USER_DATA
#define USER_DATA_GROUPS 4
@@ -1463,6 +1726,7 @@ bool nv_index_lt_groups(index_t index) { return ((index <= NV_INDEX_START_GROUPS
static void _convert(nvObj_t *nv, float conversion_factor)
{
// <<<<<<< HEAD
if (nv->valuetype != TYPE_FLOAT) { return; } // can be called non-destructively for any value type
if (isnan((double)nv->value_flt) || isinf((double)nv->value_flt)) { return; } // trap illegal float values
///+++ transform these checks into NaN or INF strings with an error return?
@@ -1478,6 +1742,13 @@ static void _convert(nvObj_t *nv, float conversion_factor)
}
}
}
/* =======
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
>>>>>>> refs/heads/edge
*/
nv->precision = GET_TABLE_WORD(precision);
nv->valuetype = TYPE_FLOAT;
}
@@ -1587,8 +1858,31 @@ stat_t set_int32(nvObj_t *nv, int32_t &value, int32_t low, int32_t high)
stat_t get_string(nvObj_t *nv, const char *str)
{
// <<<<<<< HEAD
nv->valuetype = TYPE_STRING;
return (nv_copy_string(nv, str));
/* =======
if (nv->valuetype != TYPE_FLOAT) { return; } // can be called non-destructively for any value type
if (isnan((double)nv->value) || isinf((double)nv->value)) { return; } // trap illegal float values
///+++ 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) {
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;
}
} 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;
>>>>>>> refs/heads/edge
*/
}
/*

View File

@@ -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 <http://www.gnu.org/licenses/>.
*
* 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.
*/

View File

@@ -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_num> _step;
uint8_t _step_downcount;
OutputPin<dir_num> _dir;
OutputPin<enable_num> _enable{kStartHigh};
OutputPin<enable_num> _enable;
OutputPin<ms0_num> _ms0;
OutputPin<ms1_num> _ms1;
OutputPin<ms2_num> _ms2;
PWMOutputPin<vref_num> _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 // STEP_DIR_DRIVER_H_ONCE

View File

@@ -239,6 +239,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
@@ -265,6 +268,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
@@ -289,7 +295,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
@@ -317,6 +326,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
@@ -343,6 +355,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
@@ -369,6 +384,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

View File

@@ -341,7 +341,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
@@ -441,8 +441,17 @@ static void _load_move()
return; // exit if the runtime is busy
}
// <<<<<<< HEAD
// If there are no moves to load start motor power timeouts
if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER) {
/* =======
// ...start motor power timeouts
// for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) {
// Motors[motor]->motionStopped();
// }
// loop unrolled version
>>>>>>> refs/heads/edge
*/
motor_1.motionStopped(); // ...start motor power timeouts
motor_2.motionStopped();
#if (MOTORS > 2)
@@ -611,7 +620,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)
@@ -935,9 +944,15 @@ stat_t st_set_su(nvObj_t *nv)
if (cm_get_axis_type(nv) == AXIS_TYPE_LINEAR) {
nv->value_flt *= INCHES_PER_MM;
}
// <<<<<<< HEAD
}
uint8_t m = _motor(nv->index);
st_cfg.mot[m].steps_per_unit = nv->value_flt;
/* =======
}
set_flt(nv);
>>>>>>> refs/heads/edge
*/
st_cfg.mot[m].units_per_step = 1.0/st_cfg.mot[m].steps_per_unit;
// Scale TR so all the other values make sense
@@ -947,9 +962,53 @@ stat_t st_set_su(nvObj_t *nv)
return(STAT_OK);
}
// <<<<<<< HEAD
// polarity
stat_t st_get_po(nvObj_t *nv) { return(get_integer(nv, st_cfg.mot[_motor(nv->index)].polarity)); }
stat_t st_set_po(nvObj_t *nv) { return(set_integer(nv, st_cfg.mot[_motor(nv->index)].polarity, 0, 1)); }
/* =======
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) {
nv->valuetype = TYPE_NULL;
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;
};
>>>>>>> refs/heads/edge
*/
// power management mode
stat_t st_get_pm(nvObj_t *nv)
@@ -961,10 +1020,22 @@ stat_t st_get_pm(nvObj_t *nv)
stat_t st_set_pm(nvObj_t *nv)
{
// <<<<<<< HEAD
// Test the value without setting it, then setPowerMode() now
// to both set and take effect immediately.
ritorno(set_integer(nv, (uint8_t &)cs.null, 0, MOTOR_POWER_MODE_MAX_VALUE ));
Motors[_motor(nv->index)]->setPowerMode((stPowerMode)nv->value_int);
/* =======
uint8_t motor = _get_motor(nv->index);
if (motor > MOTORS) {
nv->valuetype = TYPE_NULL;
return STAT_INPUT_VALUE_RANGE_ERROR;
};
nv->value = (float)Motors[motor]->getPowerMode();
nv->valuetype = TYPE_INT;
>>>>>>> refs/heads/edge
*/
return (STAT_OK);
}
@@ -979,11 +1050,30 @@ stat_t st_set_pm(nvObj_t *nv)
stat_t st_get_pl(nvObj_t *nv) { return(get_float(nv, st_cfg.mot[_motor(nv->index)].power_level)); }
stat_t st_set_pl(nvObj_t *nv)
{
// <<<<<<< HEAD
uint8_t m = _motor(nv->index);
ritorno(set_float_range(nv, st_cfg.mot[m].power_level, 0.0, 1.0));
st_cfg.mot[m].power_level_scaled = (nv->value_flt * POWER_LEVEL_SCALE_FACTOR);
st_run.mot[m].power_level_dynamic = (st_cfg.mot[m].power_level_scaled);
Motors[m]->setPowerLevel(st_cfg.mot[m].power_level_scaled);
/* =======
if (nv->value < (float)0.0) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_LESS_THAN_MIN_VALUE);
}
if (nv->value > (float)1.0) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_EXCEEDS_MAX_VALUE);
}
set_flt(nv); // set power_setting value in the motor config struct (st)
uint8_t motor = _get_motor(nv->index);
st_cfg.mot[motor].power_level_scaled = (nv->value * POWER_LEVEL_SCALE_FACTOR);
st_run.mot[motor].power_level_dynamic = (st_cfg.mot[motor].power_level_scaled);
Motors[motor]->setPowerLevel(st_cfg.mot[motor].power_level_scaled);
>>>>>>> refs/heads/edge
*/
return(STAT_OK);
}
@@ -1027,7 +1117,7 @@ stat_t st_set_mt(nvObj_t *nv) { return(set_float_range(nv, st_cfg.motor_power_ti
// 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_int); // nv->value is the timeout or 0 for default
@@ -1037,7 +1127,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_int < 0) {
nv->valuetype = TYPE_NULL;
@@ -1046,7 +1136,7 @@ stat_t st_set_md(nvObj_t *nv)
if (nv->value_int > MOTORS) {
nv->valuetype = TYPE_NULL;
return (STAT_INPUT_EXCEEDS_MAX_VALUE);
}
}
// de-energize all motors
if ((uint8_t)nv->value_int == 0) { // 0 means all motors
for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) {
@@ -1087,6 +1177,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";
@@ -1125,6 +1216,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);}

View File

@@ -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) {
@@ -588,8 +597,15 @@ stat_t st_get_mi(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);
// <<<<<<< HEAD
stat_t st_get_po(nvObj_t *nv);
stat_t st_set_po(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);
>>>>>>> refs/heads/edge
*/
stat_t st_get_pm(nvObj_t *nv);
stat_t st_set_pm(nvObj_t *nv);
stat_t st_get_pl(nvObj_t *nv);
@@ -611,6 +627,7 @@ stat_t st_get_dw(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);
@@ -626,6 +643,7 @@ stat_t st_get_dw(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