From 039ae5f9567f3f0e2fd3b00c9d41547767694f31 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Wed, 23 Mar 2016 00:10:48 -0700 Subject: [PATCH] More efficient struct initialization --- src/axes.c | 2 +- src/canonical_machine.c | 270 ++++++++++++++++++---------------------- src/config.h | 112 ++++++++--------- src/motor.c | 80 ++++++------ src/switch.c | 29 +++-- 5 files changed, 228 insertions(+), 265 deletions(-) diff --git a/src/axes.c b/src/axes.c index d208e2e..eec0822 100644 --- a/src/axes.c +++ b/src/axes.c @@ -155,7 +155,7 @@ void set_zero_backoff(int axis, float value) { - +// TODO fix these uint8_t get_min_switch(int axis) { //return cm.a[axis].min_switch; return 0; diff --git a/src/canonical_machine.c b/src/canonical_machine.c index 1ee6a59..308efaa 100644 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -118,7 +118,127 @@ #include -cmSingleton_t cm; // canonical machine controller singleton +cmSingleton_t cm = { + .junction_acceleration = JUNCTION_ACCELERATION, + .chordal_tolerance = CHORDAL_TOLERANCE, + .soft_limit_enable = SOFT_LIMIT_ENABLE, + + .arc_segment_len = ARC_SEGMENT_LENGTH, + + .coord_system = GCODE_DEFAULT_COORD_SYSTEM, + .select_plane = GCODE_DEFAULT_PLANE, + .units_mode = GCODE_DEFAULT_UNITS, + .path_control = GCODE_DEFAULT_PATH_CONTROL, + .distance_mode = GCODE_DEFAULT_DISTANCE_MODE, + + // Offsets + .offset = { + {}, // ABSOLUTE_COORDS + + {G54_X_OFFSET, G54_Y_OFFSET, G54_Z_OFFSET, + G54_A_OFFSET, G54_B_OFFSET, G54_C_OFFSET}, + + {G55_X_OFFSET, G55_Y_OFFSET, G55_Z_OFFSET, + G55_A_OFFSET, G55_B_OFFSET, G55_C_OFFSET}, + + {G56_X_OFFSET, G56_Y_OFFSET, G56_Z_OFFSET, + G56_A_OFFSET, G56_B_OFFSET, G56_C_OFFSET}, + + {G57_X_OFFSET, G57_Y_OFFSET, G57_Z_OFFSET, + G57_A_OFFSET, G57_B_OFFSET, G57_C_OFFSET}, + + {G58_X_OFFSET, G58_Y_OFFSET, G58_Z_OFFSET, + G58_A_OFFSET, G58_B_OFFSET, G58_C_OFFSET}, + + {G59_X_OFFSET, G59_Y_OFFSET, G59_Z_OFFSET, + G59_A_OFFSET, G59_B_OFFSET, G59_C_OFFSET}, + }, + + // Axes + .a = { + { + .axis_mode = X_AXIS_MODE, + .velocity_max = X_VELOCITY_MAX, + .feedrate_max = X_FEEDRATE_MAX, + .travel_min = X_TRAVEL_MIN, + .travel_max = X_TRAVEL_MAX, + .jerk_max = X_JERK_MAX, + .jerk_homing = X_JERK_HOMING, + .junction_dev = X_JUNCTION_DEVIATION, + .search_velocity = X_SEARCH_VELOCITY, + .latch_velocity = X_LATCH_VELOCITY, + .latch_backoff = X_LATCH_BACKOFF, + .zero_backoff = X_ZERO_BACKOFF, + }, { + .axis_mode = Y_AXIS_MODE, + .velocity_max = Y_VELOCITY_MAX, + .feedrate_max = Y_FEEDRATE_MAX, + .travel_min = Y_TRAVEL_MIN, + .travel_max = Y_TRAVEL_MAX, + .jerk_max = Y_JERK_MAX, + .jerk_homing = Y_JERK_HOMING, + .junction_dev = Y_JUNCTION_DEVIATION, + .search_velocity = Y_SEARCH_VELOCITY, + .latch_velocity = Y_LATCH_VELOCITY, + .latch_backoff = Y_LATCH_BACKOFF, + .zero_backoff = Y_ZERO_BACKOFF, + }, { + .axis_mode = Z_AXIS_MODE, + .velocity_max = Z_VELOCITY_MAX, + .feedrate_max = Z_FEEDRATE_MAX, + .travel_min = Z_TRAVEL_MIN, + .travel_max = Z_TRAVEL_MAX, + .jerk_max = Z_JERK_MAX, + .jerk_homing = Z_JERK_HOMING, + .junction_dev = Z_JUNCTION_DEVIATION, + .search_velocity = Z_SEARCH_VELOCITY, + .latch_velocity = Z_LATCH_VELOCITY, + .latch_backoff = Z_LATCH_BACKOFF, + .zero_backoff = Z_ZERO_BACKOFF, + }, { + .axis_mode = A_AXIS_MODE, + .velocity_max = A_VELOCITY_MAX, + .feedrate_max = A_FEEDRATE_MAX, + .travel_min = A_TRAVEL_MIN, + .travel_max = A_TRAVEL_MAX, + .jerk_max = A_JERK_MAX, + .jerk_homing = A_JERK_HOMING, + .junction_dev = A_JUNCTION_DEVIATION, + .radius = A_RADIUS, + .search_velocity = A_SEARCH_VELOCITY, + .latch_velocity = A_LATCH_VELOCITY, + .latch_backoff = A_LATCH_BACKOFF, + .zero_backoff = A_ZERO_BACKOFF, + }, { + .axis_mode = B_AXIS_MODE, + .velocity_max = B_VELOCITY_MAX, + .feedrate_max = B_FEEDRATE_MAX, + .travel_min = B_TRAVEL_MIN, + .travel_max = B_TRAVEL_MAX, + .jerk_max = B_JERK_MAX, + .junction_dev = B_JUNCTION_DEVIATION, + .radius = B_RADIUS, + }, { + .axis_mode = C_AXIS_MODE, + .velocity_max = C_VELOCITY_MAX, + .feedrate_max = C_FEEDRATE_MAX, + .travel_min = C_TRAVEL_MIN, + .travel_max = C_TRAVEL_MAX, + .jerk_max = C_JERK_MAX, + .junction_dev = C_JUNCTION_DEVIATION, + .radius = C_RADIUS, + } + }, + + .combined_state = COMBINED_READY, + .machine_state = MACHINE_READY, + + // State + .gm = {.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE}, + .gmx = {.block_delete_switch = true}, + .gn = {}, + .gf = {}, +}; // Command execution callbacks from planner queue @@ -515,146 +635,12 @@ stat_t cm_test_soft_limits(float target[]) { /// Config init cfg_init() must have been run beforehand void canonical_machine_init() { - // If you can assume all memory has been zeroed by a hard reset you don't - // need this code: - memset(&cm.gm, 0, sizeof(GCodeState_t)); - memset(&cm.gn, 0, sizeof(GCodeInput_t)); - memset(&cm.gf, 0, sizeof(GCodeInput_t)); - ACTIVE_MODEL = MODEL; // setup initial Gcode model pointer - // axes defaults - cm.a[AXIS_X].axis_mode = X_AXIS_MODE; - cm.a[AXIS_X].velocity_max = X_VELOCITY_MAX; - cm.a[AXIS_X].feedrate_max = X_FEEDRATE_MAX; - cm.a[AXIS_X].travel_min = X_TRAVEL_MIN; - cm.a[AXIS_X].travel_max = X_TRAVEL_MAX; - cm.a[AXIS_X].jerk_max = X_JERK_MAX; - cm.a[AXIS_X].jerk_homing = X_JERK_HOMING; - cm.a[AXIS_X].junction_dev = X_JUNCTION_DEVIATION; - cm.a[AXIS_X].search_velocity = X_SEARCH_VELOCITY; - cm.a[AXIS_X].latch_velocity = X_LATCH_VELOCITY; - cm.a[AXIS_X].latch_backoff = X_LATCH_BACKOFF; - cm.a[AXIS_X].zero_backoff = X_ZERO_BACKOFF; - - cm.a[AXIS_Y].axis_mode = Y_AXIS_MODE; - cm.a[AXIS_Y].velocity_max = Y_VELOCITY_MAX; - cm.a[AXIS_Y].feedrate_max = Y_FEEDRATE_MAX; - cm.a[AXIS_Y].travel_min = Y_TRAVEL_MIN; - cm.a[AXIS_Y].travel_max = Y_TRAVEL_MAX; - cm.a[AXIS_Y].jerk_max = Y_JERK_MAX; - cm.a[AXIS_Y].jerk_homing = Y_JERK_HOMING; - cm.a[AXIS_Y].junction_dev = Y_JUNCTION_DEVIATION; - cm.a[AXIS_Y].search_velocity = Y_SEARCH_VELOCITY; - cm.a[AXIS_Y].latch_velocity = Y_LATCH_VELOCITY; - cm.a[AXIS_Y].latch_backoff = Y_LATCH_BACKOFF; - cm.a[AXIS_Y].zero_backoff = Y_ZERO_BACKOFF; - - cm.a[AXIS_Z].axis_mode = Z_AXIS_MODE; - cm.a[AXIS_Z].velocity_max = Z_VELOCITY_MAX; - cm.a[AXIS_Z].feedrate_max = Z_FEEDRATE_MAX; - cm.a[AXIS_Z].travel_min = Z_TRAVEL_MIN; - cm.a[AXIS_Z].travel_max = Z_TRAVEL_MAX; - cm.a[AXIS_Z].jerk_max = Z_JERK_MAX; - cm.a[AXIS_Z].jerk_homing = Z_JERK_HOMING; - cm.a[AXIS_Z].junction_dev = Z_JUNCTION_DEVIATION; - cm.a[AXIS_Z].search_velocity = Z_SEARCH_VELOCITY; - cm.a[AXIS_Z].latch_velocity = Z_LATCH_VELOCITY; - cm.a[AXIS_Z].latch_backoff = Z_LATCH_BACKOFF; - cm.a[AXIS_Z].zero_backoff = Z_ZERO_BACKOFF; - - cm.a[AXIS_A].axis_mode = A_AXIS_MODE; - cm.a[AXIS_A].velocity_max = A_VELOCITY_MAX; - cm.a[AXIS_A].feedrate_max = A_FEEDRATE_MAX; - cm.a[AXIS_A].travel_min = A_TRAVEL_MIN; - cm.a[AXIS_A].travel_max = A_TRAVEL_MAX; - cm.a[AXIS_A].jerk_max = A_JERK_MAX; - cm.a[AXIS_A].jerk_homing = A_JERK_HOMING; - cm.a[AXIS_A].junction_dev = A_JUNCTION_DEVIATION; - cm.a[AXIS_A].radius = A_RADIUS; - cm.a[AXIS_A].search_velocity = A_SEARCH_VELOCITY; - cm.a[AXIS_A].latch_velocity = A_LATCH_VELOCITY; - cm.a[AXIS_A].latch_backoff = A_LATCH_BACKOFF; - cm.a[AXIS_A].zero_backoff = A_ZERO_BACKOFF; - - cm.a[AXIS_B].axis_mode = B_AXIS_MODE; - cm.a[AXIS_B].velocity_max = B_VELOCITY_MAX; - cm.a[AXIS_B].feedrate_max = B_FEEDRATE_MAX; - cm.a[AXIS_B].travel_min = B_TRAVEL_MIN; - cm.a[AXIS_B].travel_max = B_TRAVEL_MAX; - cm.a[AXIS_B].jerk_max = B_JERK_MAX; - cm.a[AXIS_B].junction_dev = B_JUNCTION_DEVIATION; - cm.a[AXIS_B].radius = B_RADIUS; - - cm.a[AXIS_C].axis_mode = C_AXIS_MODE; - cm.a[AXIS_C].velocity_max = C_VELOCITY_MAX; - cm.a[AXIS_C].feedrate_max = C_FEEDRATE_MAX; - cm.a[AXIS_C].travel_min = C_TRAVEL_MIN; - cm.a[AXIS_C].travel_max = C_TRAVEL_MAX; - cm.a[AXIS_C].jerk_max = C_JERK_MAX; - cm.a[AXIS_C].junction_dev = C_JUNCTION_DEVIATION; - cm.a[AXIS_C].radius = C_RADIUS; - // Init 1/jerk for (uint8_t axis = 0; axis < AXES; axis++) cm.a[axis].recip_jerk = 1 / (cm.a[axis].jerk_max * JERK_MULTIPLIER); - // Coordinate system offset defaults (G54-G59) - cm.offset[G54][AXIS_X] = G54_X_OFFSET; - cm.offset[G54][AXIS_Y] = G54_Y_OFFSET; - cm.offset[G54][AXIS_Z] = G54_Z_OFFSET; - cm.offset[G54][AXIS_A] = G54_A_OFFSET; - cm.offset[G54][AXIS_B] = G54_B_OFFSET; - cm.offset[G54][AXIS_C] = G54_C_OFFSET; - - cm.offset[G55][AXIS_X] = G55_X_OFFSET; - cm.offset[G55][AXIS_Y] = G55_Y_OFFSET; - cm.offset[G55][AXIS_Z] = G55_Z_OFFSET; - cm.offset[G55][AXIS_A] = G55_A_OFFSET; - cm.offset[G55][AXIS_B] = G55_B_OFFSET; - cm.offset[G55][AXIS_C] = G55_C_OFFSET; - - cm.offset[G56][AXIS_X] = G56_X_OFFSET; - cm.offset[G56][AXIS_Y] = G56_Y_OFFSET; - cm.offset[G56][AXIS_Z] = G56_Z_OFFSET; - cm.offset[G56][AXIS_A] = G56_A_OFFSET; - cm.offset[G56][AXIS_B] = G56_B_OFFSET; - cm.offset[G56][AXIS_C] = G56_C_OFFSET; - - cm.offset[G57][AXIS_X] = G57_X_OFFSET; - cm.offset[G57][AXIS_Y] = G57_Y_OFFSET; - cm.offset[G57][AXIS_Z] = G57_Z_OFFSET; - cm.offset[G57][AXIS_A] = G57_A_OFFSET; - cm.offset[G57][AXIS_B] = G57_B_OFFSET; - cm.offset[G57][AXIS_C] = G57_C_OFFSET; - - cm.offset[G58][AXIS_X] = G58_X_OFFSET; - cm.offset[G58][AXIS_Y] = G58_Y_OFFSET; - cm.offset[G58][AXIS_Z] = G58_Z_OFFSET; - cm.offset[G58][AXIS_A] = G58_A_OFFSET; - cm.offset[G58][AXIS_B] = G58_B_OFFSET; - cm.offset[G58][AXIS_C] = G58_C_OFFSET; - - cm.offset[G59][AXIS_X] = G59_X_OFFSET; - cm.offset[G59][AXIS_Y] = G59_Y_OFFSET; - cm.offset[G59][AXIS_Z] = G59_Z_OFFSET; - cm.offset[G59][AXIS_A] = G59_A_OFFSET; - cm.offset[G59][AXIS_B] = G59_B_OFFSET; - cm.offset[G59][AXIS_C] = G59_C_OFFSET; - - // Machine defaults - cm.junction_acceleration = JUNCTION_ACCELERATION; - cm.chordal_tolerance = CHORDAL_TOLERANCE; - cm.soft_limit_enable = SOFT_LIMIT_ENABLE; - cm.arc_segment_len = ARC_SEGMENT_LENGTH; - - // GCode defaults - cm.select_plane = GCODE_DEFAULT_PLANE; - cm.units_mode = GCODE_DEFAULT_UNITS; - cm.coord_system = GCODE_DEFAULT_COORD_SYSTEM; - cm.path_control = GCODE_DEFAULT_PATH_CONTROL; - cm.distance_mode = GCODE_DEFAULT_DISTANCE_MODE; - // Set gcode defaults cm_set_units_mode(cm.units_mode); cm_set_coord_system(cm.coord_system); @@ -663,20 +649,6 @@ void canonical_machine_init() { cm_set_distance_mode(cm.distance_mode); cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // always the default - cm.gmx.block_delete_switch = true; - - // Never start a machine in a motion mode - cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; - - // Reset request flags - cm.feedhold_requested = false; - cm.queue_flush_requested = false; - cm.cycle_start_requested = false; - - // Signal that the machine is ready for action - cm.machine_state = MACHINE_READY; - cm.combined_state = COMBINED_READY; - // Sub-system inits cm_spindle_init(); cm_arc_init(); diff --git a/src/config.h b/src/config.h index 66c4196..27b4f4a 100644 --- a/src/config.h +++ b/src/config.h @@ -34,15 +34,23 @@ #define __STEP_CORRECTION //#define __JERK_EXEC // Use computed jerk (vs. forward difference) //#define __KAHAN // Use Kahan summation in aline exec functions +#define __CLOCK_EXTERNAL_16MHZ // uses PLL to provide 32 MHz system clock + + +#define AXES 6 // number of axes +#define MOTORS 4 // number of motors on the board +#define COORDS 6 // number of supported coordinate systems +#define SWITCHES 8 // number of supported limit switches +#define PWMS 2 // number of supported PWM channels -#define INPUT_BUFFER_LEN 255 // text buffer size (255 max) +// Axes +typedef enum { + AXIS_X, AXIS_Y, AXIS_Z, + AXIS_A, AXIS_B, AXIS_C, + AXIS_U, AXIS_V, AXIS_W // reserved +} axis_t; -#define AXES 6 // number of axes -#define MOTORS 4 // number of motors on the board -#define COORDS 6 // number of supported coordinate systems (1-6) -#define SWITCHES 8 // number of supported limit switches -#define PWMS 2 // number of supported PWM channels // Motor settings #define MOTOR_CURRENT 0.8 // 1.0 is full power @@ -96,16 +104,15 @@ #define C_SWITCH_MODE_MAX SW_MODE_DISABLED // Switch ISRs -#define X_SWITCH_ISR_vect PORTA_INT0_vect -#define Y_SWITCH_ISR_vect PORTD_INT0_vect -#define Z_SWITCH_ISR_vect PORTE_INT0_vect -#define A_SWITCH_ISR_vect PORTF_INT0_vect - -#define SWITCH_INTLVL PORT_INT0LVL_MED_gc +#define X_SWITCH_ISR_vect PORTA_INT0_vect +#define Y_SWITCH_ISR_vect PORTD_INT0_vect +#define Z_SWITCH_ISR_vect PORTE_INT0_vect +#define A_SWITCH_ISR_vect PORTF_INT0_vect +#define SWITCH_INTLVL PORT_INT0LVL_MED_gc // Timer for debouncing switches -#define SW_LOCKOUT_TICKS 25 // 25=250ms. RTC ticks are ~10ms each -#define SW_DEGLITCH_TICKS 3 // 3=30ms +#define SW_LOCKOUT_TICKS 250 // ms +#define SW_DEGLITCH_TICKS 30 // ms // Machine settings @@ -120,8 +127,7 @@ #define VELOCITY_MAX 15000 // mm/min #define FEEDRATE_MAX VELOCITY_MAX -// See canonical_machine.h cmAxisMode for valid values -#define X_AXIS_MODE AXIS_STANDARD +#define X_AXIS_MODE AXIS_STANDARD // See canonical_machine.h #define X_VELOCITY_MAX VELOCITY_MAX // G0 max velocity in mm/min #define X_FEEDRATE_MAX FEEDRATE_MAX // G1 max feed rate in mm/min #define X_TRAVEL_MIN 0 // minimum travel for soft limits @@ -225,28 +231,12 @@ #define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS #define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE -#define AXIS_X 0 -#define AXIS_Y 1 -#define AXIS_Z 2 -#define AXIS_A 3 -#define AXIS_B 4 -#define AXIS_C 5 -#define AXIS_U 6 // reserved -#define AXIS_V 7 // reserved -#define AXIS_W 8 // reserved - - -#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N) -#define SYS_ID_LEN 12 // length of system ID string from sys_get_id() - -// Clock Crystal Config. See clock.c -#define __CLOCK_EXTERNAL_16MHZ // uses PLL to provide 32 MHz system clock // Motors mapped to ports -#define PORT_MOTOR_1 PORTA -#define PORT_MOTOR_2 PORTF -#define PORT_MOTOR_3 PORTE -#define PORT_MOTOR_4 PORTD +#define PORT_MOTOR_1 PORTA +#define PORT_MOTOR_2 PORTF +#define PORT_MOTOR_3 PORTE +#define PORT_MOTOR_4 PORTD // Motor fault ISRs #define PORT_1_FAULT_ISR_vect PORTA_INT1_vect @@ -255,16 +245,16 @@ #define PORT_4_FAULT_ISR_vect PORTF_INT1_vect // Switch axes mapped to ports -#define PORT_SWITCH_X PORTA -#define PORT_SWITCH_Y PORTF -#define PORT_SWITCH_Z PORTE -#define PORT_SWITCH_A PORTD +#define PORT_SWITCH_X PORT_MOTOR_1 +#define PORT_SWITCH_Y PORT_MOTOR_2 +#define PORT_SWITCH_Z PORT_MOTOR_3 +#define PORT_SWITCH_A PORT_MOTOR_4 // Axes mapped to output ports -#define PORT_OUT_X PORTA -#define PORT_OUT_Y PORTF -#define PORT_OUT_Z PORTE -#define PORT_OUT_A PORTD +#define PORT_OUT_X PORT_MOTOR_1 +#define PORT_OUT_Y PORT_MOTOR_2 +#define PORT_OUT_Z PORT_MOTOR_3 +#define PORT_OUT_A PORT_MOTOR_4 #define MOTOR_PORT_DIR_gm 0x2f // pin dir settings @@ -340,32 +330,34 @@ * instead of shrink (or oscillate). */ /// magnitude of forwarding error (in steps) -#define STEP_CORRECTION_THRESHOLD (float)2.00 +#define STEP_CORRECTION_THRESHOLD 2.00 /// apply to step correction for a single segment -#define STEP_CORRECTION_FACTOR (float)0.25 +#define STEP_CORRECTION_FACTOR 0.25 /// max step correction allowed in a single segment -#define STEP_CORRECTION_MAX (float)0.60 +#define STEP_CORRECTION_MAX 0.60 /// minimum wait between error correction #define STEP_CORRECTION_HOLDOFF 5 #define STEP_INITIAL_DIRECTION DIRECTION_CW // TMC2660 driver settings -#define TMC2660_SPI_PORT PORTC -#define TMC2660_SPI_SS_PIN 4 -#define TMC2660_SPI_SCK_PIN 5 -#define TMC2660_SPI_MISO_PIN 6 -#define TMC2660_SPI_MOSI_PIN 7 - -#define TMC2660_TIMER TCC1 - +#define TMC2660_SPI_PORT PORTC +#define TMC2660_SPI_SS_PIN 4 +#define TMC2660_SPI_SCK_PIN 5 +#define TMC2660_SPI_MISO_PIN 6 +#define TMC2660_SPI_MOSI_PIN 7 +#define TMC2660_TIMER TCC1 #define TMC2660_POLL_RATE 0.01 // sec. Must be in (0, 1] #define TMC2660_STABILIZE_TIME 0.01 // sec. Must be at least 1ms // PWM settings -#define PWM1_CTRLB (3 | TC1_CCBEN_bm) // single slope PWM channel B -#define PWM1_ISR_vect TCD1_CCB_vect -#define PWM2_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc -#define PWM2_CTRLB 3 // single slope PWM no output -#define PWM2_ISR_vect TCE1_CCB_vect +#define PWM1_CTRLB (3 | TC1_CCBEN_bm) // single slope PWM channel B +#define PWM1_ISR_vect TCD1_CCB_vect +#define PWM2_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc +#define PWM2_CTRLB 3 // single slope PWM no output +#define PWM2_ISR_vect TCE1_CCB_vect + + +// Input +#define INPUT_BUFFER_LEN 255 // text buffer size (255 max) diff --git a/src/motor.c b/src/motor.c index 7cc429e..6813d5f 100644 --- a/src/motor.c +++ b/src/motor.c @@ -102,7 +102,46 @@ typedef struct { float corrected_steps; // accumulated for cycle (diagnostic) } motor_t; -static motor_t motors[MOTORS]; + +static motor_t motors[MOTORS] = { + { + .motor_map = M1_MOTOR_MAP, + .step_angle = M1_STEP_ANGLE, + .travel_rev = M1_TRAVEL_PER_REV, + .microsteps = M1_MICROSTEPS, + .polarity = M1_POLARITY, + .power_mode = M1_POWER_MODE, + .timer = (TC0_t *)&M1_TIMER, + .prev_direction = STEP_INITIAL_DIRECTION + }, { + .motor_map = M2_MOTOR_MAP, + .step_angle = M2_STEP_ANGLE, + .travel_rev = M2_TRAVEL_PER_REV, + .microsteps = M2_MICROSTEPS, + .polarity = M2_POLARITY, + .power_mode = M2_POWER_MODE, + .timer = &M2_TIMER, + .prev_direction = STEP_INITIAL_DIRECTION + }, { + .motor_map = M3_MOTOR_MAP, + .step_angle = M3_STEP_ANGLE, + .travel_rev = M3_TRAVEL_PER_REV, + .microsteps = M3_MICROSTEPS, + .polarity = M3_POLARITY, + .power_mode = M3_POWER_MODE, + .timer = &M3_TIMER, + .prev_direction = STEP_INITIAL_DIRECTION + }, { + .motor_map = M4_MOTOR_MAP, + .step_angle = M4_STEP_ANGLE, + .travel_rev = M4_TRAVEL_PER_REV, + .microsteps = M4_MICROSTEPS, + .polarity = M4_POLARITY, + .power_mode = M4_POWER_MODE, + .timer = &M4_TIMER, + .prev_direction = STEP_INITIAL_DIRECTION + } +}; /// Special interrupt for X-axis @@ -112,45 +151,6 @@ ISR(TCE1_CCA_vect) { void motor_init() { - // Reset steppers to known state - memset(&motors, 0, sizeof(motors)); - - for (int motor = 0; motor < MOTORS; motor++) - motors[motor].prev_direction = STEP_INITIAL_DIRECTION; - - // Defaults - motors[0].motor_map = M1_MOTOR_MAP; - motors[0].step_angle = M1_STEP_ANGLE; - motors[0].travel_rev = M1_TRAVEL_PER_REV; - motors[0].microsteps = M1_MICROSTEPS; - motors[0].polarity = M1_POLARITY; - motors[0].power_mode = M1_POWER_MODE; - motors[0].timer = (TC0_t *)&M1_TIMER; - - motors[1].motor_map = M2_MOTOR_MAP; - motors[1].step_angle = M2_STEP_ANGLE; - motors[1].travel_rev = M2_TRAVEL_PER_REV; - motors[1].microsteps = M2_MICROSTEPS; - motors[1].polarity = M2_POLARITY; - motors[1].power_mode = M2_POWER_MODE; - motors[1].timer = &M2_TIMER; - - motors[2].motor_map = M3_MOTOR_MAP; - motors[2].step_angle = M3_STEP_ANGLE; - motors[2].travel_rev = M3_TRAVEL_PER_REV; - motors[2].microsteps = M3_MICROSTEPS; - motors[2].polarity = M3_POLARITY; - motors[2].power_mode = M3_POWER_MODE; - motors[2].timer = &M3_TIMER; - - motors[3].motor_map = M4_MOTOR_MAP; - motors[3].step_angle = M4_STEP_ANGLE; - motors[3].travel_rev = M4_TRAVEL_PER_REV; - motors[3].microsteps = M4_MICROSTEPS; - motors[3].polarity = M4_POLARITY; - motors[3].power_mode = M4_POWER_MODE; - motors[3].timer = &M4_TIMER; - // Reset position mp_set_steps_to_runtime_position(); diff --git a/src/switch.c b/src/switch.c index d129dd0..e165baa 100644 --- a/src/switch.c +++ b/src/switch.c @@ -88,7 +88,19 @@ typedef struct { switch_t switches[SWITCHES]; } swSingleton_t; -swSingleton_t sw; + +swSingleton_t sw = { + .switches = { + {.type = SWITCH_TYPE, .mode = X_SWITCH_MODE_MIN, .debounce = SW_IDLE}, + {.type = SWITCH_TYPE, .mode = X_SWITCH_MODE_MAX, .debounce = SW_IDLE}, + {.type = SWITCH_TYPE, .mode = Y_SWITCH_MODE_MIN, .debounce = SW_IDLE}, + {.type = SWITCH_TYPE, .mode = Y_SWITCH_MODE_MAX, .debounce = SW_IDLE}, + {.type = SWITCH_TYPE, .mode = Z_SWITCH_MODE_MIN, .debounce = SW_IDLE}, + {.type = SWITCH_TYPE, .mode = Z_SWITCH_MODE_MAX, .debounce = SW_IDLE}, + {.type = SWITCH_TYPE, .mode = A_SWITCH_MODE_MIN, .debounce = SW_IDLE}, + {.type = SWITCH_TYPE, .mode = A_SWITCH_MODE_MAX, .debounce = SW_IDLE}, + } +}; static bool _read_switch(uint8_t sw_num) { @@ -153,24 +165,11 @@ void switch_init() { hw.sw_port[i]->INTCTRL |= SWITCH_INTLVL; } - // Defaults - sw.switches[0].mode = X_SWITCH_MODE_MIN; - sw.switches[1].mode = X_SWITCH_MODE_MAX; - sw.switches[2].mode = Y_SWITCH_MODE_MIN; - sw.switches[3].mode = Y_SWITCH_MODE_MAX; - sw.switches[4].mode = Z_SWITCH_MODE_MIN; - sw.switches[5].mode = Z_SWITCH_MODE_MAX; - sw.switches[6].mode = A_SWITCH_MODE_MIN; - sw.switches[7].mode = A_SWITCH_MODE_MAX; - + // Initialize state for (int i = 0; i < SWITCHES; i++) { switch_t *s = &sw.switches[i]; - s->type = SWITCH_TYPE; - s->debounce = SW_IDLE; s->state = (s->type == SW_TYPE_NORMALLY_OPEN) ^ _read_switch(i); } - - sw.limit_thrown = false; } -- 2.27.0