void spindle_run(int8_t direction) //, uint16_t rpm)
{
if (direction != current_direction) {
plan_synchronize();
if (direction) {
if(direction > 0) {
#ifdef RASPBERRYPI
#else
SPINDLE_DIRECTION_PORT &= ~(1<<SPINDLE_DIRECTION_BIT);
#endif
} else {
#ifdef RASPBERRYPI
#else
SPINDLE_DIRECTION_PORT |= 1<<SPINDLE_DIRECTION_BIT;
#endif
}
#ifdef RASPBERRYPI
#else
SPINDLE_ENABLE_PORT |= 1<<SPINDLE_ENABLE_BIT;
#endif
} else {
spindle_stop();
}
current_direction = direction;
}
}
// Perform homing cycle to locate and set machine zero. Only '$H' executes this command.
// NOTE: There should be no motions in the buffer and Grbl must be in an idle state before
// executing the homing cycle. This prevents incorrect buffered plans after homing.
void mc_go_home()
{
sys.state = STATE_HOMING; // Set system state variable
LIMIT_PCMSK &= ~LIMIT_MASK; // Disable hard limits pin change register for cycle duration
limits_go_home(); // Perform homing routine.
protocol_execute_runtime(); // Check for reset and set system abort.
if (sys.abort) { return; } // Did not complete. Alarm state set by mc_alarm.
// The machine should now be homed and machine zero has been located. Upon completion,
// reset system position and sync internal position vectors.
clear_vector_float(sys.position); // Set machine zero
sys_sync_current_position();
sys.state = STATE_IDLE; // Set system state to IDLE to complete motion and indicate homed.
// Pull-off axes (that have been homed) from limit switches before continuing motion.
// This provides some initial clearance off the switches and should also help prevent them
// from falsely tripping when hard limits are enabled.
/// 8c1
int8_t x_dir, y_dir, z_dir, t_dir;
x_dir = y_dir = z_dir = t_dir = 0;
if (HOMING_LOCATE_CYCLE & (1<<X_AXIS)) {
if (settings.homing_dir_mask & (1<<X_DIRECTION_BIT))
x_dir = 1;
else
x_dir = -1;
}
if (HOMING_LOCATE_CYCLE & (1<<Y_AXIS)) {
if (settings.homing_dir_mask & (1<<Y_DIRECTION_BIT)) { y_dir = 1; }
else { y_dir = -1; }
}
if (HOMING_LOCATE_CYCLE & (1<<Z_AXIS)) {
if (settings.homing_dir_mask & (1<<Z_DIRECTION_BIT)) { z_dir = 1; }
else { z_dir = -1; }
}
/// 8c1
if (HOMING_LOCATE_CYCLE & (1<<T_AXIS)) {
if (settings.homing_dir_mask & (1<<T_DIRECTION_BIT)) { t_dir = 1; }
else { t_dir = -1; }
}
/// 8c1 : line
mc_line(x_dir*settings.homing_pulloff, y_dir*settings.homing_pulloff,
z_dir*settings.homing_pulloff, t_dir*settings.homing_pulloff, settings.homing_seek_rate, false, C_LINE);
st_cycle_start(); // Move it. Nothing should be in the buffer except this motion.
plan_synchronize(); // Make sure the motion completes.
// The gcode parser position circumvented by the pull-off maneuver, so sync position vectors.
sys_sync_current_position();
// If hard limits feature enabled, re-enable hard limits pin change register after homing cycle.
if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE))
LIMIT_PCMSK |= LIMIT_MASK;
// Finished!
}
// Execute dwell in seconds.
void mc_dwell(float seconds)
{
uint16_t i = floor(1000/DWELL_TIME_STEP*seconds);
plan_synchronize();
delay_ms(floor(1000*seconds-i*DWELL_TIME_STEP)); // Delay millisecond remainder
while (i-- > 0) {
// NOTE: Check and execute runtime commands during dwell every <= DWELL_TIME_STEP milliseconds.
protocol_execute_runtime();
if (sys.abort) { return; }
_delay_ms(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment
}
}
void coolant_stop(void) {
/* If coolant was running, we need to wait until all previous moves are
* executed before turning it off. */
if(current_coolant_mode) plan_synchronize();
#ifdef CSPRAY_ENABLE
host_gpio_write(CSPRAY_ENABLE, false, HOST_GPIO_MODE_BIT);
#endif
#ifdef CFLOOD_ENABLE
host_gpio_write(CFLOOD_ENABLE, false, HOST_GPIO_MODE_BIT);
#endif
}
// Perform homing cycle to locate and set machine zero. Only '$H' executes this command.
// NOTE: There should be no motions in the buffer and Grbl must be in an idle state before
// executing the homing cycle. This prevents incorrect buffered plans after homing.
void mc_go_home()
{
sys.state = STATE_HOMING; // Set system state variable
LIMIT_PCMSK &= ~LIMIT_MASK; // Disable hard limits pin change register for cycle duration
limits_go_home(); // Perform homing routine.
protocol_execute_runtime(); // Check for reset and set system abort.
if (sys.abort) { return; } // Did not complete. Alarm state set by mc_alarm.
// The machine should now be homed and machine limits have been located. By default,
// grbl defines machine space as all negative, as do most CNCs. Since limit switches
// can be on either side of an axes, check and set machine zero appropriately.
// At the same time, set up pull-off maneuver from axes limit switches that have been homed.
// This provides some initial clearance off the switches and should also help prevent them
// from falsely tripping when hard limits are enabled.
// TODO: Need to improve dir_mask[] to be more axes independent.
float pulloff_target[N_AXIS];
clear_vector_float(pulloff_target); // Zero pulloff target.
clear_vector_long(sys.position); // Zero current position for now.
uint8_t dir_mask[N_AXIS];
dir_mask[X_AXIS] = (1<<X_DIRECTION_BIT);
dir_mask[Y_AXIS] = (1<<Y_DIRECTION_BIT);
dir_mask[Z_AXIS] = (1<<Z_DIRECTION_BIT);
uint8_t i;
for (i=0; i<N_AXIS; i++) {
// Set up pull off targets and machine positions for limit switches homed in the negative
// direction, rather than the traditional positive. Leave non-homed positions as zero and
// do not move them.
if (HOMING_LOCATE_CYCLE & bit(i)) {
if (settings.homing_dir_mask & dir_mask[i]) {
pulloff_target[i] = settings.homing_pulloff-settings.max_travel[i];
sys.position[i] = -lround(settings.max_travel[i]*settings.steps_per_mm[i]);
} else {
pulloff_target[i] = -settings.homing_pulloff;
}
}
}
sys_sync_current_position();
sys.state = STATE_IDLE; // Set system state to IDLE to complete motion and indicate homed.
mc_line(pulloff_target, settings.homing_seek_rate, false);
st_cycle_start(); // Move it. Nothing should be in the buffer except this motion.
plan_synchronize(); // Make sure the motion completes.
// The gcode parser position circumvented by the pull-off maneuver, so sync position vectors.
sys_sync_current_position();
// If hard limits feature enabled, re-enable hard limits pin change register after homing cycle.
if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)) { LIMIT_PCMSK |= LIMIT_MASK; }
// Finished!
}
void spindle_run(int8_t direction) {
/* If we need to change state, we must wait for all moves to complete before
* doing so. */
if(direction != current_direction) {
plan_synchronize();
if(direction != SPINDLE_STOP) {
#ifdef SPINDLE_DIRECTION
if(direction == SPINDLE_CW)
host_gpio_write(SPINDLE_DIRECTION, false, HOST_GPIO_MODE_BIT);
else host_gpio_write(SPINDLE_DIRECTION, true, HOST_GPIO_MODE_BIT);
#endif
host_gpio_write(SPINDLE_ENABLE, true, HOST_GPIO_MODE_BIT);
} else spindle_stop();
current_direction = direction;
}
}
void coolant_run(uint8_t mode)
{
if (mode != current_coolant_mode)
{
plan_synchronize(); // Ensure coolant turns on when specified in program.
if (mode == COOLANT_FLOOD_ENABLE) {
COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT);
#ifdef ENABLE_M7
} else if (mode == COOLANT_MIST_ENABLE) {
COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT);
#endif
} else {
coolant_stop();
}
current_coolant_mode = mode;
}
}
void spindle_run(int8_t direction) //, uint16_t rpm)
{
if (direction != current_direction) {
plan_synchronize();
if (direction) {
if(direction > 0) {
SPINDLE_DIRECTION_PORT &= ~(1<<SPINDLE_DIRECTION_BIT);
} else {
SPINDLE_DIRECTION_PORT |= 1<<SPINDLE_DIRECTION_BIT;
}
SPINDLE_ENABLE_PORT |= 1<<SPINDLE_ENABLE_BIT;
} else {
spindle_stop();
}
current_direction = direction;
}
}
void coolant_run(uint8_t mode) {
/* If we need to change state, we must wait for all moves to complete before
* doing so. */
if(mode != current_coolant_mode) {
plan_synchronize();
if(mode != COOLANT_OFF) {
#ifdef CSPRAY_ENABLE
if(mode & COOLANT_MIST)
host_gpio_write(CSPRAY_ENABLE, true, HOST_GPIO_MODE_BIT);
#endif
#ifdef CFLOOD_ENABLE
if(mode & COOLANT_FLOOD)
host_gpio_write(CFLOOD_ENABLE, true, HOST_GPIO_MODE_BIT);
#endif
} else coolant_stop();
current_coolant_mode = mode;
}
}
void jogging()
// Tests jog port pins and moves steppers
{
uint8_t jog_bits, jog_bits_old, out_bits0, jog_exit, last_sys_state;
uint8_t i, limit_state, spindle_bits;
uint32_t dest_step_rate, step_rate, step_delay; // Step delay after pulse
switch (sys.state) {
case STATE_CYCLE: case STATE_HOMING: case STATE_INIT:
LED_PORT |= (1<<LED_ERROR_BIT);
return;
case STATE_ALARM: case STATE_QUEUED:
LED_PORT &= ~(1<<LED_ERROR_BIT); break;
default:
LED_PORT |= (1<<LED_ERROR_BIT);
}
last_sys_state = sys.state;
spindle_bits = (~PINOUT_PIN) & (1<<PIN_SPIN_TOGGLE); // active low
if (spindle_bits) {
if (spindle_status) {
// gc.spindle_direction = 0;
spindle_run(0);
}
else {
// gc.spindle_direction = 1; // also update gcode spindle status
spindle_run(1);
}
spindle_btn_release();
delay_ms(20);
}
jog_bits = (~JOGSW_PIN) & JOGSW_MASK; // active low
if (!jog_bits) { return; } // nothing pressed
// At least one jog/joystick switch is active
if (jog_bits & (1<<JOG_ZERO)) { // Zero-Button gedrückt
jog_btn_release();
sys.state = last_sys_state;
if (bit_isfalse(PINOUT_PIN,bit(PIN_RESET))) { // RESET und zusätzlich ZERO gedrückt: Homing
if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) {
// Only perform homing if Grbl is idle or lost.
if ( sys.state==STATE_IDLE || sys.state==STATE_ALARM ) {
mc_go_home();
if (!sys.abort) { protocol_execute_startup(); } // Execute startup scripts after successful homing.
}
}
} else { // Zero current work position
sys_sync_current_position();
// gc.coord_system[i] Current work coordinate system (G54+). Stores offset from absolute machine
// position in mm. Loaded from EEPROM when called.
// gc.coord_offset[i] Retains the G92 coordinate offset (work coordinates) relative to
// machine zero in mm. Non-persistent. Cleared upon reset and boot.
for (i=0; i<=2; i++) { // Axes indices are consistent, so loop may be used.
gc.coord_offset[i] = gc.position[i] - gc.coord_system[i];
}
// Z-Achse um bestimmten Betrag zurückziehen
mc_line(gc.position[X_AXIS], gc.position[Y_AXIS], gc.position[Z_AXIS]
+ (settings.z_zero_pulloff * settings.z_scale), settings.default_seek_rate, false);
plan_synchronize(); // Make sure the motion completes
gc.position[Z_AXIS] = gc.position[Z_AXIS] - (settings.z_zero_gauge * settings.z_scale);
gc.coord_offset[Z_AXIS] = gc.position[Z_AXIS] - gc.coord_system[Z_AXIS];
// The gcode parser position circumvented by the pull-off maneuver, so sync position vectors.
// Sets the planner position vector to current steps. Called by the system abort routine.
// Sets g-code parser position in mm. Input in steps. Called by the system abort and hard
sys_sync_current_position(); // Syncs all internal position vectors to the current system position.
}
return;
}
ADCSRA = ADCSRA_init | (1<<ADIF); //0x93, clear ADIF
uint8_t reverse_flag = 0;
uint8_t out_bits = 0;
uint8_t jog_select = 0;
out_bits0 = (0) ^ (settings.invert_mask);
ADCSRA = ADCSRA_init | (1<<ADIF); //0x93, clear ADIF
ADCSRA = ADCSRA_init | (1<<ADSC); //0xC3; start conversion
sys.state = STATE_JOG;
// check for reverse switches
if (jog_bits & (1<<JOGREV_X_BIT)) { // X reverse switch on
out_bits0 ^= (1<<X_DIRECTION_BIT);
out_bits = out_bits0 ^ (1<<X_STEP_BIT);
reverse_flag = 1;
}
if (jog_bits & (1<<JOGREV_Y_BIT)) { // Y reverse switch on
out_bits0 ^= (1<<Y_DIRECTION_BIT);
out_bits = out_bits0 ^ (1<<Y_STEP_BIT);
reverse_flag = 1;
jog_select = 1;
}
if (jog_bits & (1<<JOGREV_Z_BIT)) { // Z reverse switch on
out_bits0 ^= (1<<Z_DIRECTION_BIT);
out_bits = out_bits0 ^ (1<<Z_STEP_BIT);
reverse_flag = 1;
jog_select = 2;
}
// check for forward switches
if (jog_bits & (1<<JOGFWD_X_BIT)) { // X forward switch on
out_bits = out_bits0 ^ (1<<X_STEP_BIT);
}
if (jog_bits & (1<<JOGFWD_Y_BIT)) { // Y forward switch on
out_bits = out_bits0 ^ (1<<Y_STEP_BIT);
jog_select = 1;
}
if (jog_bits & (1<<JOGFWD_Z_BIT)) { // Z forward switch on
out_bits = out_bits0 ^ (1<<Z_STEP_BIT);
jog_select = 2;
}
dest_step_rate = ADCH; // set initial dest_step_rate according to analog input
dest_step_rate = (dest_step_rate * JOG_SPEED_FAC) + JOG_MIN_SPEED;
step_rate = JOG_MIN_SPEED; // set initial step rate
jog_exit = 0;
while (!(ADCSRA && (1<<ADIF))) {} // warte bis ADIF-Bit gesetzt
ADCSRA = ADCSRA_init; // exit conversion
st_wake_up();
// prepare direction with small delay, direction settle time
STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | (out_bits0 & STEPPING_MASK);
delay_us(10);
jog_bits_old = jog_bits;
i = 0; // now index for sending position data
for(;;) { // repeat until button/joystick released
// report_realtime_status(); // benötigt viel Zeit!
ADCSRA = ADCSRA_init | (1<<ADIF); //0x93, clear ADIF
// Get limit pin state
#ifdef LIMIT_SWITCHES_ACTIVE_HIGH
// When in an active-high switch configuration
limit_state = LIMIT_PIN;
#else
limit_state = LIMIT_PIN ^ LIMIT_MASK;
#endif
if ((limit_state & LIMIT_MASK) && reverse_flag) { jog_exit = 1; } // immediate stop
jog_bits = (~JOGSW_PIN) & JOGSW_MASK; // active low
if (jog_bits == jog_bits_old) { // nothing changed
if (step_rate < (dest_step_rate - 5)) { // Hysterese für A/D-Wandlung
step_rate += JOG_RAMP; // accellerate
}
if (step_rate > (dest_step_rate + 5)) { // Hysterese für A/D-Wandlung
step_rate -= JOG_RAMP; // brake
}
}
else {
if (step_rate > (JOG_MIN_SPEED*2)) { // switch change happened, fast brake to complete stop
step_rate = ((step_rate * 99) / 100) - 10;
}
else { jog_exit = 1; } // finished to stop and exit
}
// stop and exit if done
if (jog_exit || (sys.execute & EXEC_RESET)) {
st_go_idle();
sys.state = last_sys_state;
sys_sync_current_position();
return;
}
// update position registers
if (reverse_flag) {
sys.position[jog_select]--;
}
else {
sys.position[jog_select]++;
}
ADCSRA = ADCSRA_init | (1<<ADSC); //0xC3; start ADC conversion
// Both direction and step pins appropriately inverted and set. Perform one step
STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | (out_bits & STEPPING_MASK);
delay_us(settings.pulse_microseconds);
STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | (out_bits0 & STEPPING_MASK);
step_delay = (1000000/step_rate) - settings.pulse_microseconds - 100; // 100 = fester Wert für Schleifenzeit
if (sys.execute & EXEC_STATUS_REPORT) { // status report requested, print short msg only
printPgmString(PSTR("Jog\r\n"));
sys.execute = 0;
}
delay_us(step_delay);
#ifdef JOG_SPI_PRESENT
send_spi_position(i); // bei jedem Durchlauf nur eine Achse übertragen
i++;
if (i>2) {i=0;}
#endif
while (!(ADCSRA && (1<<ADIF))) {} // warte ggf. bis ADIF-Bit gesetzt
ADCSRA = ADCSRA_init; // exit conversion
dest_step_rate = ADCH; // set next dest_step_rate according to analog input
dest_step_rate = (dest_step_rate * JOG_SPEED_FAC) + JOG_MIN_SPEED;
}
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace). Comments and block delete
// characters have been removed. All units and positions are converted and exported to grbl's
// internal functions in terms of (mm, mm/min) and absolute machine coordinates, respectively.
uint8_t gc_execute_line(char *line)
{
// If in alarm state, don't process. Immediately return with error.
// NOTE: Might not be right place for this, but also prevents $N storing during alarm.
if (sys.state == STATE_ALARM) { return(STATUS_ALARM_LOCK); }
uint8_t char_counter = 0;
char letter;
float value;
int int_value;
uint16_t modal_group_words = 0; // Bitflag variable to track and check modal group words in block
uint8_t axis_words = 0; // Bitflag to track which XYZ(ABC) parameters exist in block
float inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
uint8_t absolute_override = false; // true(1) = absolute motion for this block only {G53}
uint8_t non_modal_action = NON_MODAL_NONE; // Tracks the actions of modal group 0 (non-modal)
float target[4], offset[4];
clear_vector(target); // XYZ(ABC) axes parameters.
clear_vector(offset); // IJK Arc offsets are incremental. Value of zero indicates no change.
gc.status_code = STATUS_OK;
/* Pass 1: Commands and set all modes. Check for modal group violations.
NOTE: Modal group numbers are defined in Table 4 of NIST RS274-NGC v3, pg.20 */
uint8_t group_number = MODAL_GROUP_NONE;
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
switch(letter) {
case 'G':
// Set modal group values
switch(int_value) {
case 4: case 10: case 28: case 30: case 53: case 92: group_number = MODAL_GROUP_0; break;
case 0: case 1: case 2: case 3: case 80: group_number = MODAL_GROUP_1; break;
case 17: case 18: case 19: group_number = MODAL_GROUP_2; break;
case 90: case 91: group_number = MODAL_GROUP_3; break;
case 93: case 94: group_number = MODAL_GROUP_5; break;
case 20: case 21: group_number = MODAL_GROUP_6; break;
case 54: case 55: case 56: case 57: case 58: case 59: group_number = MODAL_GROUP_12; break;
}
// Set 'G' commands
switch(int_value) {
case 0: gc.motion_mode = MOTION_MODE_SEEK; break;
case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
case 4: non_modal_action = NON_MODAL_DWELL; break;
case 10: non_modal_action = NON_MODAL_SET_COORDINATE_DATA; break;
case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 28: case 30:
int_value = trunc(10*value); // Multiply by 10 to pick up Gxx.1
switch(int_value) {
case 280: non_modal_action = NON_MODAL_GO_HOME_0; break;
case 281: non_modal_action = NON_MODAL_SET_HOME_0; break;
case 300: non_modal_action = NON_MODAL_GO_HOME_1; break;
case 301: non_modal_action = NON_MODAL_SET_HOME_1; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 53: absolute_override = true; break;
case 54: case 55: case 56: case 57: case 58: case 59:
gc.coord_select = int_value-54;
break;
case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
case 90: gc.absolute_mode = true; break;
case 91: gc.absolute_mode = false; break;
case 92:
int_value = trunc(10*value); // Multiply by 10 to pick up G92.1
switch(int_value) {
case 920: non_modal_action = NON_MODAL_SET_COORDINATE_OFFSET; break;
case 921: non_modal_action = NON_MODAL_RESET_COORDINATE_OFFSET; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 93: gc.inverse_feed_rate_mode = true; break;
case 94: gc.inverse_feed_rate_mode = false; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'M':
// Set modal group values
switch(int_value) {
case 0: case 1: case 2: case 30: group_number = MODAL_GROUP_4; break;
case 3: case 4: case 5: group_number = MODAL_GROUP_7; break;
}
// Set 'M' commands
switch(int_value) {
case 0: gc.program_flow = PROGRAM_FLOW_PAUSED; break; // Program pause
case 1: break; // Optional stop not supported. Ignore.
case 2: case 30: gc.program_flow = PROGRAM_FLOW_COMPLETED; break; // Program end and reset
case 3: gc.spindle_direction = 1; break;
case 4: gc.spindle_direction = -1; break;
case 5: gc.spindle_direction = 0; break;
#ifdef ENABLE_M7
case 7: gc.coolant_mode = COOLANT_MIST_ENABLE; break;
#endif
case 8: gc.coolant_mode = COOLANT_FLOOD_ENABLE; break;
case 9: gc.coolant_mode = COOLANT_DISABLE; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
}
// Check for modal group multiple command violations in the current block
if (group_number) {
if ( bit_istrue(modal_group_words,bit(group_number)) ) {
FAIL(STATUS_MODAL_GROUP_VIOLATION);
} else {
bit_true(modal_group_words,bit(group_number));
}
group_number = MODAL_GROUP_NONE; // Reset for next command.
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
/* Pass 2: Parameters. All units converted according to current block commands. Position
parameters are converted and flagged to indicate a change. These can have multiple connotations
for different commands. Each will be converted to their proper value upon execution. */
float p = 0, r = 0;
uint8_t l = 0;
char_counter = 0;
while(next_statement(&letter, &value, line, &char_counter)) {
switch(letter) {
case 'G': case 'M': case 'N': break; // Ignore command statements and line numbers
case 'F':
if (value <= 0) { FAIL(STATUS_INVALID_STATEMENT); } // Must be greater than zero
if (gc.inverse_feed_rate_mode) {
inverse_feed_rate = to_millimeters(value); // seconds per motion for this motion only
} else {
gc.feed_rate = to_millimeters(value); // millimeters per minute
}
break;
case 'I': case 'J': case 'K': offset[letter-'I'] = to_millimeters(value); break;
case 'L': l = trunc(value); break;
case 'P': p = value; break;
case 'R': r = to_millimeters(value); break;
case 'S':
if (value < 0) { FAIL(STATUS_INVALID_STATEMENT); } // Cannot be negative
// TBD: Spindle speed not supported due to PWM issues, but may come back once resolved.
// gc.spindle_speed = value;
break;
case 'T':
if (value < 0) { FAIL(STATUS_INVALID_STATEMENT); } // Cannot be negative
gc.tool = trunc(value);
break;
case 'X': target[X_AXIS] = to_millimeters(value); bit_true(axis_words,bit(X_AXIS)); break;
case 'Y': target[Y_AXIS] = to_millimeters(value); bit_true(axis_words,bit(Y_AXIS)); break;
case 'Z': target[Z_AXIS] = to_millimeters(value); bit_true(axis_words,bit(Z_AXIS)); break;
case 'C': target[C_AXIS] = to_millimeters(value); bit_true(axis_wors,bit(C_AXIS)); break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
/* Execute Commands: Perform by order of execution defined in NIST RS274-NGC.v3, Table 8, pg.41.
NOTE: Independent non-motion/settings parameters are set out of this order for code efficiency
and simplicity purposes, but this should not affect proper g-code execution. */
// ([F]: Set feed and seek rates.)
// TODO: Seek rates can change depending on the direction and maximum speeds of each axes. When
// max axis speed is installed, the calculation can be performed here, or maybe in the planner.
if (sys.state != STATE_CHECK_MODE) {
// ([M6]: Tool change should be executed here.)
// [M3,M4,M5]: Update spindle state
spindle_run(gc.spindle_direction);
// [*M7,M8,M9]: Update coolant state
coolant_run(gc.coolant_mode);
}
// [G54,G55,...,G59]: Coordinate system selection
if ( bit_istrue(modal_group_words,bit(MODAL_GROUP_12)) ) { // Check if called in block
float coord_data[N_AXIS];
if (!(settings_read_coord_data(gc.coord_select,coord_data))) { return(STATUS_SETTING_READ_FAIL); }
memcpy(gc.coord_system,coord_data,sizeof(coord_data));
}
// [G4,G10,G28,G30,G92,G92.1]: Perform dwell, set coordinate system data, homing, or set axis offsets.
// NOTE: These commands are in the same modal group, hence are mutually exclusive. G53 is in this
// modal group and do not effect these actions.
switch (non_modal_action) {
case NON_MODAL_DWELL:
if (p < 0) { // Time cannot be negative.
FAIL(STATUS_INVALID_STATEMENT);
} else {
// Ignore dwell in check gcode modes
if (sys.state != STATE_CHECK_MODE) { mc_dwell(p); }
}
break;
case NON_MODAL_SET_COORDINATE_DATA:
int_value = trunc(p); // Convert p value to int.
if ((l != 2 && l != 20) || (int_value < 0 || int_value > N_COORDINATE_SYSTEM)) { // L2 and L20. P1=G54, P2=G55, ...
FAIL(STATUS_UNSUPPORTED_STATEMENT);
} else if (!axis_words && l==2) { // No axis words.
FAIL(STATUS_INVALID_STATEMENT);
} else {
if (int_value > 0) { int_value--; } // Adjust P1-P6 index to EEPROM coordinate data indexing.
else { int_value = gc.coord_select; } // Index P0 as the active coordinate system
float coord_data[N_AXIS];
if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
uint8_t i;
// Update axes defined only in block. Always in machine coordinates. Can change non-active system.
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(i)) ) {
if (l == 20) {
coord_data[i] = gc.position[i]-target[i]; // L20: Update axis current position to target
} else {
coord_data[i] = target[i]; // L2: Update coordinate system axis
}
}
}
settings_write_coord_data(int_value,coord_data);
// Update system coordinate system if currently active.
if (gc.coord_select == int_value) { memcpy(gc.coord_system,coord_data,sizeof(coord_data)); }
}
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_GO_HOME_0: case NON_MODAL_GO_HOME_1:
// Move to intermediate position before going home. Obeys current coordinate system and offsets
// and absolute and incremental modes.
if (axis_words) {
// Apply absolute mode coordinate offsets or incremental mode offsets.
uint8_t i;
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if ( bit_istrue(axis_words,bit(i)) ) {
if (gc.absolute_mode) {
target[i] += gc.coord_system[i] + gc.coord_offset[i];
} else {
target[i] += gc.position[i];
}
} else {
target[i] = gc.position[i];
}
}
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[C_AXIS], settings.default_seek_rate, false);
}
// Retreive G28/30 go-home position data (in machine coordinates) from EEPROM
float coord_data[N_AXIS];
if (non_modal_action == NON_MODAL_GO_HOME_1) {
if (!settings_read_coord_data(SETTING_INDEX_G30 ,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
} else {
if (!settings_read_coord_data(SETTING_INDEX_G28 ,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
}
mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], coord_data[C_AXIS], settings.default_seek_rate, false);
memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[];
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_SET_HOME_0: case NON_MODAL_SET_HOME_1:
if (non_modal_action == NON_MODAL_SET_HOME_1) {
settings_write_coord_data(SETTING_INDEX_G30,gc.position);
} else {
settings_write_coord_data(SETTING_INDEX_G28,gc.position);
}
break;
case NON_MODAL_SET_COORDINATE_OFFSET:
if (!axis_words) { // No axis words
FAIL(STATUS_INVALID_STATEMENT);
} else {
// Update axes defined only in block. Offsets current system to defined value. Does not update when
// active coordinate system is selected, but is still active unless G92.1 disables it.
uint8_t i;
for (i=0; i<=2; i++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(i)) ) {
gc.coord_offset[i] = gc.position[i]-gc.coord_system[i]-target[i];
}
}
}
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_RESET_COORDINATE_OFFSET:
clear_vector(gc.coord_offset); // Disable G92 offsets by zeroing offset vector.
break;
}
// [G0,G1,G2,G3,G80]: Perform motion modes.
// NOTE: Commands G10,G28,G30,G92 lock out and prevent axis words from use in motion modes.
// Enter motion modes only if there are axis words or a motion mode command word in the block.
if ( bit_istrue(modal_group_words,bit(MODAL_GROUP_1)) || axis_words ) {
// G1,G2,G3 require F word in inverse time mode.
if ( gc.inverse_feed_rate_mode ) {
if (inverse_feed_rate < 0 && gc.motion_mode != MOTION_MODE_CANCEL) {
FAIL(STATUS_INVALID_STATEMENT);
}
}
// Absolute override G53 only valid with G0 and G1 active.
if ( absolute_override && !(gc.motion_mode == MOTION_MODE_SEEK || gc.motion_mode == MOTION_MODE_LINEAR)) {
FAIL(STATUS_INVALID_STATEMENT);
}
// Report any errors.
if (gc.status_code) { return(gc.status_code); }
// Convert all target position data to machine coordinates for executing motion. Apply
// absolute mode coordinate offsets or incremental mode offsets.
// NOTE: Tool offsets may be appended to these conversions when/if this feature is added.
uint8_t i;
for (i=0; i<=3; i++) { // Axes indices are consistent, so loop may be used to save flash space.
if ( bit_istrue(axis_words,bit(i)) ) {
if (!absolute_override) { // Do not update target in absolute override mode
if (gc.absolute_mode) {
target[i] += gc.coord_system[i] + gc.coord_offset[i]; // Absolute mode
} else {
target[i] += gc.position[i]; // Incremental mode
}
}
} else {
target[i] = gc.position[i]; // No axis word in block. Keep same axis position.
}
}
switch (gc.motion_mode) {
case MOTION_MODE_CANCEL:
if (axis_words) { FAIL(STATUS_INVALID_STATEMENT); } // No axis words allowed while active.
break;
case MOTION_MODE_SEEK:
if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);}
else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[C_AXIS], settings.default_seek_rate, false); }
break;
case MOTION_MODE_LINEAR:
// TODO: Inverse time requires F-word with each statement. Need to do a check. Also need
// to check for initial F-word upon startup. Maybe just set to zero upon initialization
// and after an inverse time move and then check for non-zero feed rate each time. This
// should be efficient and effective.
if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);}
else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[C_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); }
break;
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
// Check if at least one of the axes of the selected plane has been specified. If in center
// format arc mode, also check for at least one of the IJK axes of the selected plane was sent.
if ( !( bit_false(axis_words,bit(gc.plane_axis_2)) ) ||
( !r && !offset[gc.plane_axis_0] && !offset[gc.plane_axis_1] ) ) {
FAIL(STATUS_INVALID_STATEMENT);
} else {
if (r != 0) { // Arc Radius Mode
/*
We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == magnitude of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
d^2 == x^2 + y^2
h^2 == r^2 - (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
O <- [i,j]
- |
r - |
- |
- | h
- |
[0,0] -> C -----------------+--------------- T <- [x,y]
| <------ d/2 ---->|
C - Current position
T - Target position
O - center of circle that pass through both C and T
d - distance from C to T
r - designated radius
h - distance from center of CT to O
Expanding the equations:
d -> sqrt(x^2 + y^2)
h -> sqrt(4 * r^2 - x^2 - y^2)/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Which can be written:
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Which we for size and speed reasons optimize to:
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2
*/
// Calculate the change in position along each selected axis
float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
clear_vector(offset);
// First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller
// than d. If so, the sqrt of a negative number is complex and error out.
float h_x2_div_d = 4 * r*r - x*x - y*y;
if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return(gc.status_code); }
// Finish computing h_x2_div_d.
h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d)
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
the left hand circle will be generated - when it is negative the right hand circle is generated.
T <-- Target position
^
Clockwise circles with this center | Clockwise circles with this center will have
will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
\ | /
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
|
|
C <-- Current position */
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
if (r < 0) {
h_x2_div_d = -h_x2_div_d;
r = -r; // Finished with r. Set to positive for mc_arc
}
// Complete the operation by calculating the actual center of the arc
offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
} else { // Arc Center Format Offset Mode
r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
}
// Set clockwise/counter-clockwise sign for mc_arc computations
uint8_t isclockwise = false;
if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
// Trace the arc
mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
r, isclockwise);
}
break;
}
// Report any errors.
if (gc.status_code) { return(gc.status_code); }
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
memcpy(gc.position, target, sizeof(target)); // gc.position[] = target[];
}
// M0,M1,M2,M30: Perform non-running program flow actions. During a program pause, the buffer may
// refill and can only be resumed by the cycle start run-time command.
if (gc.program_flow) {
plan_synchronize(); // Finish all remaining buffered motions. Program paused when complete.
sys.auto_start = false; // Disable auto cycle start. Forces pause until cycle start issued.
// If complete, reset to reload defaults (G92.2,G54,G17,G90,G94,M48,G40,M5,M9). Otherwise,
// re-enable program flow after pause complete, where cycle start will resume the program.
if (gc.program_flow == PROGRAM_FLOW_COMPLETED) { mc_reset(); }
else { gc.program_flow = PROGRAM_FLOW_RUNNING; }
}
return(gc.status_code);
}
void limits_go_home() {
plan_synchronize();
// Enable steppers by resetting the stepper disable port
STEPPERS_ENABLE_PORT |= (1<<STEPPERS_ENABLE_BIT);
STEPPERS_ENABLE_PORT |= (1<<STEPPERS_ACTIVITY_BIT);
// STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
sys.position[X_AXIS]=0;
sys.position[Y_AXIS]=0;
sys.position[Z_AXIS]=0;
#ifdef Z_LIMIT_PRESENT
approach_limit_switch(false, false, true); // First home the z axis
delay_ms(50);
// Now carefully leave the limit switches
leave_limit_switch(false, false, true);
delay_ms(50);
#endif
#ifdef Y_HOME_FIRST
#ifdef Y_LIMIT_PRESENT
approach_limit_switch(false, true, false); // Home Y axis
delay_ms(50);
leave_limit_switch(false, true, false);
delay_ms(50);
#endif
#ifdef X_LIMIT_PRESENT
approach_limit_switch(true, false, false); // Home X axis
delay_ms(50);
leave_limit_switch(true, false, false);
delay_ms(50);
#endif
#else
#ifdef X_LIMIT_PRESENT
approach_limit_switch(true, false, false); // Home X axis
delay_ms(50);
leave_limit_switch(true, false, false);
delay_ms(50);
#endif
#ifdef Y_LIMIT_PRESENT
approach_limit_switch(false, true, false); // Home Y axis
delay_ms(50);
leave_limit_switch(false, true, false);
delay_ms(50);
#endif
#endif
// Disable steppers by setting stepper disable
delay_ms(50);
STEPPERS_ENABLE_PORT &= ~(1<<STEPPERS_ENABLE_BIT);
STEPPERS_ENABLE_PORT &= ~(1<<STEPPERS_ACTIVITY_BIT);
// STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
// Hardware-Zähler zurücksetzen
st_counter_null();
}
void spindle_stop(void) {
/* If we were spinning, we need to wait until all previous moves are executed
* before stopping. */
if(current_direction) plan_synchronize();
host_gpio_write(SPINDLE_ENABLE, false, HOST_GPIO_MODE_BIT);
}
