├── grbl ├── examples │ └── grblUpload │ │ └── grblUpload.ino ├── jog.h ├── gcode.h ├── spindle_control.h ├── print.h ├── protocol.h ├── byteordering.c ├── byteordering.h ├── stepper.h ├── eeprom.h ├── limits.h ├── serial.h ├── sd_raw.h ├── spindle_control.c ├── grbl.h ├── report.h ├── nuts_bolts.h ├── lcd.h ├── settings.h ├── planner.h ├── print.c ├── system.h ├── main.c ├── report.c ├── eeprom.c ├── nuts_bolts.c ├── cpu_map.h ├── fat.h ├── jog.c ├── serial.c ├── limits.c ├── settings.c ├── system.c ├── sd_raw.c ├── lcd.c ├── gcode.c ├── planner.c ├── fat.c ├── protocol.c └── stepper.c ├── uploads └── schematic.jpg └── README.md /grbl/examples/grblUpload/grblUpload.ino: -------------------------------------------------------------------------------- 1 | #include 2 | -------------------------------------------------------------------------------- /grbl/jog.h: -------------------------------------------------------------------------------- 1 | #ifndef jog_h 2 | #define jog_h 3 | 4 | void jog_execute(uint8_t i); 5 | 6 | #endif 7 | -------------------------------------------------------------------------------- /uploads/schematic.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mcu51c51/standaloneGRBL/HEAD/uploads/schematic.jpg -------------------------------------------------------------------------------- /grbl/gcode.h: -------------------------------------------------------------------------------- 1 | #ifndef gcode_h 2 | #define gcode_h 3 | 4 | uint8_t gc_execute_line(char *line); 5 | 6 | #endif 7 | -------------------------------------------------------------------------------- /grbl/spindle_control.h: -------------------------------------------------------------------------------- 1 | #ifndef spindle_control_h 2 | #define spindle_control_h 3 | 4 | void spindle_init(); 5 | void spindle_set_speed(uint8_t pwm_value); 6 | 7 | #endif 8 | -------------------------------------------------------------------------------- /grbl/print.h: -------------------------------------------------------------------------------- 1 | #ifndef print_h 2 | #define print_h 3 | 4 | void printPgmString(const char *s); 5 | void print_uint32_base10(uint32_t n); 6 | void print_uint8_base10(uint8_t n); 7 | void printFloat(float n, uint8_t decimal_places); 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /grbl/protocol.h: -------------------------------------------------------------------------------- 1 | #ifndef protocol_h 2 | #define protocol_h 3 | 4 | #define LINE_BUFFER_SIZE 30 5 | 6 | void protocol_main_loop(); 7 | void protocol_buffer_synchronize(); 8 | void protocol_execute_realtime(); 9 | void protocol_exec_rt_system(); 10 | void protocol_auto_cycle_start(); 11 | 12 | #endif 13 | -------------------------------------------------------------------------------- /grbl/byteordering.c: -------------------------------------------------------------------------------- 1 | #include "grbl.h" 2 | 3 | uint16_t read16(const uint8_t *p) { 4 | return (((uint16_t)p[1]) << 8) | (((uint16_t)p[0]) << 0); } 5 | 6 | uint32_t read32(const uint8_t *p) { 7 | return (((uint32_t)p[3]) << 24) | (((uint32_t)p[2]) << 16) | (((uint32_t)p[1]) << 8) | (((uint32_t)p[0]) << 0); } 8 | -------------------------------------------------------------------------------- /grbl/byteordering.h: -------------------------------------------------------------------------------- 1 | #ifndef BYTEORDERING_H 2 | #define BYTEORDERING_H 3 | 4 | #define HTOL16(val) (val) 5 | #define HTOL32(val) (val) 6 | #define LTOH16(val) (val) 7 | #define LTOH32(val) (val) 8 | 9 | #define htol16(h) (h) 10 | #define htol32(h) (h) 11 | #define ltoh16(l) (l) 12 | #define ltoh32(l) (l) 13 | 14 | uint16_t read16(const uint8_t *p); 15 | uint32_t read32(const uint8_t *p); 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /grbl/stepper.h: -------------------------------------------------------------------------------- 1 | #ifndef stepper_h 2 | #define stepper_h 3 | 4 | #define SEGMENT_BUFFER_SIZE 6 5 | 6 | #define ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING 7 | #define ACCELERATION_TICKS_PER_SECOND 100 8 | 9 | void stepper_init(); 10 | void st_wake_up(); 11 | void st_go_idle(); 12 | void st_generate_step_dir_invert_masks(); 13 | void st_reset(); 14 | void st_prep_buffer(); 15 | void st_update_plan_block_parameters(); 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /grbl/eeprom.h: -------------------------------------------------------------------------------- 1 | #ifndef eeprom_h 2 | #define eeprom_h 3 | 4 | #define EEPROM_IGNORE_SELFPROG 5 | 6 | unsigned char eeprom_get_char(unsigned int addr); 7 | void eeprom_put_char(unsigned int addr, unsigned char new_value); 8 | void memcpy_to_eeprom_with_checksum(unsigned int destination, char *source, unsigned int size); 9 | int memcpy_from_eeprom_with_checksum(char *destination, unsigned int source, unsigned int size); 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /grbl/limits.h: -------------------------------------------------------------------------------- 1 | #ifndef limits_h 2 | #define limits_h 3 | 4 | #define HOMING_AXIS_SEARCH_SCALAR 1.1 5 | #define HOMING_PULLOFF 10.0 6 | #define HOMING_FEED_RATE 600.0 7 | #define HOMING_DEBOUNCE_DELAY 200 8 | 9 | #define limit_get_state() ((bit_isfalse(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) ? ((LIMIT_PIN & (1< 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "cpu_map.h" 19 | #include "nuts_bolts.h" 20 | #include "lcd.h" 21 | #include "serial.h" 22 | #include "eeprom.h" 23 | #include "settings.h" 24 | #include "byteordering.h" 25 | #include "sd_raw.h" 26 | #include "fat.h" 27 | #include "system.h" 28 | #include "planner.h" 29 | #include "gcode.h" 30 | #include "limits.h" 31 | #include "print.h" 32 | #include "protocol.h" 33 | #include "report.h" 34 | #include "spindle_control.h" 35 | #include "stepper.h" 36 | #include "jog.h" 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /grbl/report.h: -------------------------------------------------------------------------------- 1 | #ifndef report_h 2 | #define report_h 3 | 4 | #define STATUS_OK 0 5 | #define STATUS_BAD_NUMBER_FORMAT 2 6 | #define STATUS_INVALID_STATEMENT 3 7 | #define STATUS_NEGATIVE_VALUE 4 8 | #define STATUS_SETTING_DISABLED 5 9 | #define STATUS_SETTING_READ_FAIL 7 10 | #define STATUS_IDLE_ERROR 8 11 | #define STATUS_SYSTEM_GC_LOCK 9 12 | #define STATUS_SOFT_LIMIT_ERROR 10 13 | #define STATUS_OVERFLOW 11 14 | #define STATUS_STOP 12 15 | #define STATUS_RESET 14 16 | #define STATUS_INVALID_JOG_COMMAND 16 17 | #define STATUS_GCODE_UNSUPPORTED_COMMAND 20 18 | #define STATUS_GCODE_UNDEFINED_FEED_RATE 22 19 | #define STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER 23 20 | #define STATUS_HOMING_FAIL_RESET 17 21 | #define STATUS_HOMING_FAIL_PULLOFF 18 22 | #define STATUS_HOMING_FAIL_APPROACH 19 23 | 24 | void report_status_message(uint8_t status_code); 25 | void report_grbl_settings(); 26 | void report_build_info(); 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /grbl/nuts_bolts.h: -------------------------------------------------------------------------------- 1 | #ifndef nuts_bolts_h 2 | #define nuts_bolts_h 3 | 4 | #define SOME_LARGE_VALUE 1.0E+38 5 | 6 | #define N_AXIS 2 7 | #define X_AXIS 0 8 | #define Y_AXIS 1 9 | 10 | #define A_MOTOR X_AXIS 11 | #define B_MOTOR Y_AXIS 12 | 13 | #define DWELL_TIME_STEP 10 14 | #define MM_PER_INCH (25.4) 15 | #define TICKS_PER_MICROSECOND (F_CPU/1000000) 16 | 17 | #define max(a,b) (((a) > (b)) ? (a) : (b)) 18 | #define min(a,b) (((a) < (b)) ? (a) : (b)) 19 | 20 | #define bit(n) (1 << n) 21 | #define bit_true(x,mask) (x) |= (mask) 22 | #define bit_false(x,mask) (x) &= ~(mask) 23 | #define bit_istrue(x,mask) (0 != (x & mask)) 24 | #define bit_isfalse(x,mask) (0 == (x & mask)) 25 | 26 | uint8_t read_float(char *line, uint8_t *char_counter, float *float_ptr); 27 | void delay_ms(uint16_t ms); 28 | float convert_delta_vector_to_unit_vector(float *vector); 29 | float limit_value_by_axis_maximum(float *max_value, float *unit_vec); 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /grbl/lcd.h: -------------------------------------------------------------------------------- 1 | #ifndef LCD_H 2 | #define LCD_H 3 | 4 | #define LCD_START_LINE1 0x00 5 | #define LCD_START_LINE2 0x40 6 | 7 | #define LCD_DELAY_BOOTUP 16000 8 | #define LCD_DELAY_INIT 5000 9 | #define LCD_DELAY_INIT_REP 64 10 | #define LCD_DELAY_INIT_4BIT 64 11 | #define LCD_DELAY_BUSY_FLAG 4 12 | #define LCD_DELAY_ENABLE_PULSE 1 13 | 14 | #define LCD_CLR 0 15 | #define LCD_ENTRY_MODE 2 16 | #define LCD_ENTRY_INC 1 17 | #define LCD_DDRAM 7 18 | #define LCD_BUSY 7 19 | 20 | #define LCD_DISP_OFF 0x08 21 | #define LCD_DISP_ON 0x0C 22 | 23 | #define LCD_FUNCTION_4BIT_2LINES 0x28 24 | 25 | #define LCD_MODE_DEFAULT ((1<= 100) { 12 | digit_a = '0' + n % 10; 13 | n /= 10; } 14 | if (n >= 10) { 15 | digit_b = '0' + n % 10; 16 | n /= 10; } 17 | serial_write('0' + n); 18 | if (digit_b) { serial_write(digit_b); } 19 | if (digit_a) { serial_write(digit_a); } } 20 | 21 | void print_uint32_base10(uint32_t n) { 22 | if (n == 0) { 23 | serial_write('0'); 24 | return; } 25 | 26 | unsigned char buf[10]; 27 | uint8_t i = 0; 28 | 29 | while (n > 0) { 30 | buf[i++] = n % 10; 31 | n /= 10; } 32 | 33 | for (; i > 0; i--) 34 | serial_write('0' + buf[i-1]); } 35 | 36 | void printFloat(float n, uint8_t decimal_places) { 37 | if (n < 0) { 38 | serial_write('-'); 39 | n = -n; } 40 | 41 | uint8_t decimals = decimal_places; 42 | while (decimals >= 2) { 43 | n *= 100; 44 | decimals -= 2; } 45 | if (decimals) { n *= 10; } 46 | n += 0.5; 47 | 48 | unsigned char buf[13]; 49 | uint8_t i = 0; 50 | uint32_t a = (long)n; 51 | while(a > 0) { 52 | buf[i++] = (a % 10) + '0'; 53 | a /= 10; } 54 | while (i < decimal_places) { 55 | buf[i++] = '0'; } 56 | if (i == decimal_places) { 57 | buf[i++] = '0'; } 58 | 59 | for (; i > 0; i--) { 60 | if (i == decimal_places) { serial_write('.'); } 61 | serial_write(buf[i-1]); } } 62 | -------------------------------------------------------------------------------- /grbl/system.h: -------------------------------------------------------------------------------- 1 | #ifndef system_h 2 | #define system_h 3 | 4 | #include "grbl.h" 5 | 6 | #define EXEC_STATUS_REPORT bit(0) 7 | #define EXEC_CYCLE_START bit(1) 8 | #define EXEC_CYCLE_STOP bit(2) 9 | #define EXEC_FEED_HOLD bit(3) 10 | #define EXEC_RESET bit(4) 11 | 12 | #define STATE_IDLE 0 13 | #define STATE_ALARM bit(0) 14 | #define STATE_NULL bit(1) 15 | #define STATE_HOMING bit(2) 16 | #define STATE_CYCLE bit(3) 17 | #define STATE_HOLD bit(4) 18 | #define STATE_JOG bit(5) 19 | 20 | #define STATE_UART 0 21 | #define STATE_FILE 1 22 | #define STATE_ORIGIN 2 23 | 24 | #define STEP_CONTROL_NORMAL_OP 0 25 | #define STEP_CONTROL_END_MOTION bit(0) 26 | #define STEP_CONTROL_EXECUTE_HOLD bit(1) 27 | #define STEP_CONTROL_UPDATE_SPINDLE_PWM bit(3) 28 | 29 | typedef struct { 30 | uint8_t state; 31 | uint8_t last_state; 32 | uint8_t state2; 33 | uint8_t abort; 34 | uint8_t suspend; 35 | uint8_t step_control; 36 | uint8_t f_override; 37 | uint8_t spindle_speed; 38 | uint8_t non_modal_dwell; 39 | } system_t; 40 | extern system_t sys; 41 | 42 | extern volatile uint8_t btn_state; 43 | extern volatile uint8_t btn; 44 | 45 | extern int32_t sys_position[N_AXIS]; 46 | extern float last_position[N_AXIS]; 47 | extern int32_t wco[N_AXIS]; 48 | 49 | extern volatile uint8_t sys_rt_exec_state; 50 | 51 | #ifdef COREXY 52 | int32_t system_convert_corexy_to_x_axis_steps(int32_t *steps); 53 | int32_t system_convert_corexy_to_y_axis_steps(int32_t *steps); 54 | #endif 55 | 56 | void system_init(); 57 | void buttons_check(); 58 | uint8_t adc_read(uint8_t pin); 59 | uint8_t system_execute_line(char *line); 60 | void system_set_exec_state_flag(uint8_t mask); 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /grbl/main.c: -------------------------------------------------------------------------------- 1 | #include "grbl.h" 2 | 3 | system_t sys; 4 | plan_line_data_t pl_data; 5 | int32_t sys_position[N_AXIS]; 6 | float last_position[N_AXIS]; 7 | int32_t wco[N_AXIS]; 8 | volatile uint8_t sys_rt_exec_state; 9 | volatile uint8_t btn_state; 10 | volatile uint8_t btn; 11 | 12 | int main(void) { 13 | lcd_init(); 14 | system_init(); 15 | serial_init(); 16 | settings_init(); 17 | stepper_init(); 18 | spindle_init(); 19 | 20 | if (bit_istrue(settings.flags,BITFLAG_XY_HOME_PIN_AS_ST_ENABLE)) { 21 | STEPPERS_DISABLE_DDR |= (1< 0; size--) { 51 | checksum = (checksum << 1) || (checksum >> 7); 52 | checksum += *source; 53 | eeprom_put_char(destination++, *(source++)); } 54 | 55 | eeprom_put_char(destination, checksum); } 56 | 57 | int memcpy_from_eeprom_with_checksum(char *destination, unsigned int source, unsigned int size) { 58 | unsigned char data, checksum = 0; 59 | 60 | for(; size > 0; size--) { 61 | data = eeprom_get_char(source++); 62 | checksum = (checksum << 1) || (checksum >> 7); 63 | checksum += data; 64 | *(destination++) = data; } 65 | 66 | return(checksum == eeprom_get_char(source)); } 67 | -------------------------------------------------------------------------------- /grbl/nuts_bolts.c: -------------------------------------------------------------------------------- 1 | #include "grbl.h" 2 | 3 | uint8_t read_float(char *line, uint8_t *char_counter, float *float_ptr) { 4 | char *ptr = line + *char_counter; 5 | unsigned char c; 6 | 7 | c = *ptr++; 8 | 9 | bool isnegative = 0; 10 | if (c == '-') { 11 | isnegative = 1; 12 | c = *ptr++; } 13 | 14 | uint32_t intval = 0; 15 | int8_t exp = 0; 16 | uint8_t ndigit = 0; 17 | bool isdecimal = 0; 18 | 19 | while(1) { 20 | c -= '0'; 21 | if (c <= 9) { 22 | ndigit++; 23 | if (isdecimal) { exp--; } 24 | intval = (((intval << 2) + intval) << 1) + c; 25 | } else if (c == (('.'-'0') & 0xff) && !(isdecimal)) { 26 | isdecimal = 1; 27 | } else { 28 | break; } 29 | c = *ptr++; } 30 | 31 | if (!ndigit) { return(0); } 32 | 33 | float fval; 34 | fval = (float)intval; 35 | 36 | if (fval != 0) { 37 | while (exp <= -2) { 38 | fval *= 0.01; 39 | exp += 2; } 40 | if (exp < 0) { 41 | fval *= 0.1; } } 42 | 43 | if (isnegative) { 44 | *float_ptr = -fval; 45 | } else { 46 | *float_ptr = fval; } 47 | 48 | *char_counter = ptr - line - 1; 49 | 50 | return(1); } 51 | 52 | void delay_ms(uint16_t ms) { 53 | while (ms--) { _delay_ms(1); } } 54 | 55 | float convert_delta_vector_to_unit_vector(float *vector) { 56 | uint8_t idx; 57 | float magnitude = 0.0; 58 | 59 | for (idx=0; idx (0.8/settings.steps_per_mm[idx] + i/10.0)) { 15 | pos -= i/10.0; } 16 | pl_data.xyz[idx] = lround(pos*settings.steps_per_mm[idx]) + wco[idx]; 17 | if (bit_istrue(settings.flags, BITFLAG_HOMING_ENABLE)) { 18 | if (pl_data.xyz[idx] > lround(settings.max_travel[idx] * settings.steps_per_mm[idx])) { 19 | pl_data.xyz[idx] = plan_get_position(idx); 20 | j--; } } 21 | } else if (bit_istrue(btn_state,bit(N_AXIS+idx))) { 22 | f_pos = (plan_get_position(idx) - wco[idx]) / settings.steps_per_mm[idx]; 23 | pos = round(f_pos * 10) / 10 - i/10.0; 24 | if (pos - f_pos < -(0.8/settings.steps_per_mm[idx] + i/10.0)) { 25 | pos += i/10.0; } 26 | pl_data.xyz[idx] = lround(pos*settings.steps_per_mm[idx]) + wco[idx]; 27 | if (bit_istrue(settings.flags, BITFLAG_HOMING_ENABLE)) { 28 | if (pl_data.xyz[idx] < 0) { 29 | pl_data.xyz[idx] = plan_get_position(idx); 30 | j--; } } 31 | } else { 32 | pl_data.xyz[idx] = plan_get_position(idx); 33 | j--; } } 34 | 35 | pl_data.feed_rate = 3000.0 * sqrt(j); 36 | 37 | if (settings.max_rate[X_AXIS] < 600.0 || settings.max_rate[Y_AXIS] < 600.0) { 38 | j = 1; 39 | } else if (settings.max_rate[X_AXIS] < 1200.0 || settings.max_rate[Y_AXIS] < 1200.0) { 40 | j = 5; 41 | } else { 42 | j = 10; } 43 | 44 | while (plan_get_block_buffer_count() > j) { 45 | buttons_check(); 46 | protocol_exec_rt_system(); 47 | if (sys.abort) { return; } } 48 | 49 | plan_buffer_line(); 50 | 51 | if (sys.state == STATE_IDLE) { 52 | sys_rt_exec_state |= EXEC_STATUS_REPORT; 53 | sys.step_control = STEP_CONTROL_NORMAL_OP; 54 | if (plan_get_current_block() != NULL) { 55 | sys.state = STATE_JOG; 56 | st_prep_buffer(); 57 | st_wake_up(); } } } 58 | -------------------------------------------------------------------------------- /grbl/serial.c: -------------------------------------------------------------------------------- 1 | #include "grbl.h" 2 | 3 | #define RX_RING_BUFFER (RX_BUFFER_SIZE+1) 4 | #define TX_RING_BUFFER (TX_BUFFER_SIZE+1) 5 | 6 | uint8_t serial_rx_buffer[RX_RING_BUFFER]; 7 | uint16_t serial_rx_buffer_head = 0; 8 | volatile uint16_t serial_rx_buffer_tail = 0; 9 | 10 | uint8_t serial_tx_buffer[TX_RING_BUFFER]; 11 | uint8_t serial_tx_buffer_head = 0; 12 | volatile uint8_t serial_tx_buffer_tail = 0; 13 | 14 | void serial_init() { 15 | #if BAUD_RATE < 57600 16 | uint16_t UBRR0_value = ((F_CPU / (8L * BAUD_RATE)) - 1)/2; 17 | UCSR0A &= ~(1 << U2X0); 18 | #else 19 | uint16_t UBRR0_value = ((F_CPU / (4L * BAUD_RATE)) - 1)/2; 20 | UCSR0A |= (1 << U2X0); 21 | #endif 22 | UBRR0H = UBRR0_value >> 8; 23 | UBRR0L = UBRR0_value; 24 | 25 | UCSR0B |= (1< 50) { 27 | sys.f_override -= 5; 28 | if (sys.f_override < 50) { 29 | sys.f_override = 50; } 30 | plan_update_velocity_profile_parameters(); 31 | plan_cycle_reinitialize(); } } } 32 | } else if (val < 191) { 33 | if (sys.state == STATE_IDLE || sys.state == STATE_JOG) { 34 | btn_state &= ~(1<<1); 35 | btn_state |= (1<<3); 36 | } else if (sys.state == STATE_CYCLE || sys.state == STATE_HOLD) { 37 | if (!(btn_state & (1<<4))) { 38 | btn_state |= (1<<4); 39 | if (sys.f_override < 150) { 40 | sys.f_override += 5; 41 | if (sys.f_override > 150) { 42 | sys.f_override = 150; } 43 | plan_update_velocity_profile_parameters(); 44 | plan_cycle_reinitialize(); } } } 45 | } else { 46 | btn_state &= ~((1<<1)|(1<<3)|(1<<4)); } 47 | if (btn_state) { return; } 48 | if (abs(adc_read(CONTROL_OTHER) - (val = adc_read(CONTROL_OTHER))) <= 2) { 49 | if (val < 21) { 50 | if (btn) { 51 | sys_rt_exec_state |= EXEC_RESET; 52 | report_status_message(STATUS_STOP); 53 | btn = 0; } 54 | } else if (val < 58) { 55 | if (btn) { 56 | if (sys.state == STATE_IDLE) { 57 | _delay_ms(30); 58 | if (abs(adc_read(CONTROL_OTHER) - val) > 2) { return; } 59 | btn_state |= (1<<6); 60 | } else if (sys.state == STATE_CYCLE) { 61 | sys_rt_exec_state |= EXEC_FEED_HOLD; 62 | } else if (sys.state == STATE_HOLD) { 63 | sys_rt_exec_state |= EXEC_CYCLE_START; } 64 | btn = 0; } 65 | } else if (val < 85) { 66 | if (btn) { 67 | if (sys.state == STATE_IDLE) { 68 | _delay_ms(30); 69 | if (abs(adc_read(CONTROL_OTHER) - val) > 2) { return; } 70 | if (sys.spindle_speed == SPINDLE_PWM_OFF_VALUE) { 71 | spindle_set_speed(SPINDLE_PWM_MIN_VALUE); 72 | } else { 73 | spindle_set_speed(SPINDLE_PWM_OFF_VALUE); } 74 | lcd_state2(); } 75 | btn = 0; } 76 | } else if (val < 105) { 77 | if (btn) { 78 | if (sys.state == STATE_IDLE) { 79 | _delay_ms(30); 80 | if (abs(adc_read(CONTROL_OTHER) - val) > 2) { return; } 81 | btn_state |= (1<<5); } 82 | btn = 0; } 83 | #ifndef COREXY 84 | } else if (val < 121) { 85 | if (btn) { 86 | if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { 87 | if (sys.state == STATE_IDLE || sys.state == STATE_ALARM) { 88 | _delay_ms(30); 89 | if (abs(adc_read(CONTROL_OTHER) - val) > 2) { return; } 90 | btn_state |= (1<<7); } } 91 | btn = 0; } 92 | #endif 93 | } else { 94 | btn = 1; } } } 95 | 96 | uint8_t adc_read(uint8_t pin) { 97 | ADMUX = ((1<> 24) & 0xff); 134 | sd_raw_send_byte((arg >> 16) & 0xff); 135 | sd_raw_send_byte((arg >> 8) & 0xff); 136 | sd_raw_send_byte((arg >> 0) & 0xff); 137 | switch(command) { 138 | case CMD_GO_IDLE_STATE: 139 | sd_raw_send_byte(0x95); 140 | break; 141 | case CMD_SEND_IF_COND: 142 | sd_raw_send_byte(0x87); 143 | break; 144 | default: 145 | sd_raw_send_byte(0xff); 146 | break; } 147 | 148 | for(uint8_t i = 0; i < 10; ++i) { 149 | response = sd_raw_rec_byte(); 150 | if(response != 0xff) { 151 | break; } } 152 | 153 | return response; } 154 | 155 | uint8_t sd_raw_read(uint64_t offset, uint8_t *buffer, uintptr_t length) { 156 | uint64_t block_address; 157 | uint16_t block_offset; 158 | uint16_t read_length; 159 | while(length > 0) { 160 | block_offset = offset & 0x01ff; 161 | block_address = offset - block_offset; 162 | read_length = 512 - block_offset; 163 | if(read_length > length) { 164 | read_length = length; } 165 | 166 | if(block_address != raw_block_address) { 167 | select_card(); 168 | if(sd_raw_send_command(CMD_READ_SINGLE_BLOCK, (sd_raw_card_type & (1 << SD_RAW_SPEC_SDHC) ? block_address / 512 : block_address))) { 169 | unselect_card(); 170 | return 0; } 171 | 172 | while(sd_raw_rec_byte() != 0xfe); 173 | 174 | uint8_t* cache = raw_block; 175 | for(uint16_t i = 0; i < 512; ++i) { 176 | *cache++ = sd_raw_rec_byte(); } 177 | raw_block_address = block_address; 178 | 179 | memcpy(buffer, raw_block + block_offset, read_length); 180 | buffer += read_length; 181 | 182 | sd_raw_rec_byte(); 183 | sd_raw_rec_byte(); 184 | 185 | unselect_card(); 186 | 187 | sd_raw_rec_byte(); 188 | } else { 189 | memcpy(buffer, raw_block + block_offset, read_length); 190 | buffer += read_length; } 191 | length -= read_length; 192 | offset += read_length; } 193 | 194 | return 1; } 195 | 196 | uint8_t sd_raw_read_interval(uint64_t offset, uint8_t *buffer, uintptr_t interval, uintptr_t length, sd_raw_read_interval_handler_t callback, void *p) { 197 | if(!buffer || interval == 0 || length < interval || !callback) { 198 | return 0; } 199 | 200 | while(length >= interval) { 201 | if(!sd_raw_read(offset, buffer, interval)) { 202 | return 0; } 203 | if(!callback(buffer, offset, p)) { 204 | break; } 205 | offset += interval; 206 | length -= interval; } 207 | 208 | return 1; } 209 | -------------------------------------------------------------------------------- /grbl/lcd.c: -------------------------------------------------------------------------------- 1 | #include "grbl.h" 2 | 3 | #define lcd_e_delay() _delay_us(LCD_DELAY_ENABLE_PULSE) 4 | #define lcd_e_high() LCD_ENABLE_PORT |= _BV(LCD_ENABLE_BIT); 5 | #define lcd_e_low() LCD_ENABLE_PORT &= ~_BV(LCD_ENABLE_BIT); 6 | #define lcd_e_toggle() toggle_e() 7 | #define lcd_rw_high() LCD_RW_PORT |= _BV(LCD_RW_BIT) 8 | #define lcd_rw_low() LCD_RW_PORT &= ~_BV(LCD_RW_BIT) 9 | #define lcd_rs_high() LCD_RS_PORT |= _BV(LCD_RS_BIT) 10 | #define lcd_rs_low() LCD_RS_PORT &= ~_BV(LCD_RS_BIT) 11 | 12 | #define LCD_FUNCTION_DEFAULT LCD_FUNCTION_4BIT_2LINES 13 | 14 | #define delay(us) _delay_us(us) 15 | 16 | uint8_t counter = 0; 17 | 18 | static void toggle_e(void) { 19 | lcd_e_high(); 20 | lcd_e_delay(); 21 | lcd_e_low(); } 22 | 23 | static void lcd_write(uint8_t data, uint8_t rs) { 24 | unsigned char dataBits ; 25 | 26 | if (rs) { 27 | lcd_rs_high(); 28 | } else { 29 | lcd_rs_low(); } 30 | lcd_rw_low(); 31 | 32 | LCD_DATA_DDR |= 0x0F; 33 | 34 | dataBits = LCD_DATA_PORT & 0xF0; 35 | LCD_DATA_PORT = dataBits |((data>>4)&0x0F); 36 | lcd_e_toggle(); 37 | 38 | LCD_DATA_PORT = dataBits | (data&0x0F); 39 | lcd_e_toggle(); 40 | 41 | LCD_DATA_PORT = dataBits | 0x0F; } 42 | 43 | static uint8_t lcd_read() { 44 | uint8_t data; 45 | 46 | lcd_rs_low(); 47 | lcd_rw_high(); 48 | 49 | LCD_DATA_DDR &= 0xF0; 50 | 51 | lcd_e_high(); 52 | lcd_e_delay(); 53 | data = LCD_DATA_PIN << 4; 54 | lcd_e_low(); 55 | 56 | lcd_e_delay(); 57 | 58 | lcd_e_high(); 59 | lcd_e_delay(); 60 | data |= LCD_DATA_PIN & 0x0F; 61 | lcd_e_low(); 62 | return data; } 63 | 64 | static void lcd_waitbusy() { 65 | while (lcd_read() & (1< 0) { 117 | buf[i++] = (a % 10) + '0'; 118 | a /= 10; } 119 | while (i < 3) { 120 | buf[i++] = '0'; } 121 | if (i == 3) { 122 | buf[i++] = '0'; } 123 | if (isnegative) { 124 | buf[i++] = '-'; } 125 | while (i < 8) { 126 | buf[i++] = ' '; } 127 | lcd_gotoxy(2,idx); 128 | for (; i > 0; i--) { 129 | if (i == 3) { lcd_gotoxy(8,idx); } 130 | lcd_putc(buf[i-1]); } } } } 131 | 132 | void lcd_state() { 133 | if (sys.state != sys.last_state) { 134 | sys.last_state = sys.state; 135 | lcd_gotoxy(12,0); 136 | switch (sys.state) { 137 | case STATE_CYCLE: lcd_puts_P(" RUN"); break; 138 | case STATE_IDLE: lcd_puts_P("IDLE"); break; 139 | case STATE_HOLD: 140 | if (sys.non_modal_dwell) { 141 | lcd_puts_P(" RUN"); 142 | } else { 143 | lcd_puts_P("PAUS"); } 144 | break; 145 | case STATE_JOG: lcd_puts_P("MANL"); break; 146 | case STATE_HOMING: lcd_puts_P("HOME"); break; 147 | case STATE_ALARM: lcd_puts_P("ALRM"); break; } 148 | lcd_state2(); } } 149 | 150 | void lcd_state2() { 151 | lcd_gotoxy(12,1); 152 | if (sys.state == STATE_CYCLE || sys.state == STATE_HOLD) { 153 | if (sys.state2 == STATE_UART) { 154 | lcd_puts_P("UART"); 155 | } else if (sys.state2 == STATE_FILE) { 156 | lcd_puts_P("FILE"); 157 | } else if (sys.state2 == STATE_ORIGIN) { 158 | lcd_puts_P("ORGN"); } 159 | } else { 160 | if (sys.spindle_speed == SPINDLE_PWM_OFF_VALUE) { 161 | lcd_puts_P("LOFF"); 162 | } else { 163 | lcd_puts_P("SAFE"); } } } 164 | 165 | void lcd_clear() { 166 | last_position[X_AXIS] = 0; last_position[Y_AXIS] = 0; 167 | lcd_gotoxy(0,0); lcd_puts_P("X 0.000 "); 168 | lcd_gotoxy(0,1); lcd_puts_P("Y 0.000 "); 169 | sys.last_state = STATE_NULL; lcd_state(); 170 | sys_rt_exec_state = EXEC_STATUS_REPORT; } 171 | 172 | void lcd_file(const char *s) { 173 | register char c; 174 | uint8_t i = 0; 175 | while ((c = *s++)) { 176 | if (c >= 'a' && c <= 'z') { 177 | lcd_putc(c-'a'+'A'); 178 | } else { 179 | lcd_putc(c); } 180 | i++; } 181 | while (i < 12) { 182 | lcd_putc(' '); i++; } } 183 | 184 | void lcd_error(uint8_t status_code) { 185 | lcd_clrscr(); lcd_gotoxy(0,0); 186 | switch(status_code) { 187 | case 100: 188 | lcd_puts_P("Insert SD card!"); 189 | break; 190 | case 101: 191 | lcd_puts_P("Unknown SD card!"); 192 | break; 193 | case 102: 194 | lcd_puts_P("No U-disk files!"); 195 | break; 196 | default: 197 | lcd_puts_P("error:"); 198 | uint8_t digit_a = 0, digit_b = 0; 199 | if (status_code >= 100) { 200 | digit_a = '0' + status_code % 10; 201 | status_code /= 10; } 202 | if (status_code >= 10) { 203 | digit_b = '0' + status_code % 10; 204 | status_code /= 10; } 205 | lcd_putc('0' + status_code); 206 | if (digit_b) { lcd_putc(digit_b); } 207 | if (digit_a) { lcd_putc(digit_a); } 208 | break; } } 209 | 210 | void lcd_init() { 211 | LCD_DATA_DDR |= 0x0F; 212 | LCD_RS_DDR |= _BV(LCD_RS_BIT); 213 | LCD_RW_DDR |= _BV(LCD_RW_BIT); 214 | LCD_ENABLE_DDR |= _BV(LCD_ENABLE_BIT); 215 | delay(LCD_DELAY_BOOTUP); 216 | 217 | LCD_DATA_PORT |= _BV(1); 218 | LCD_DATA_PORT |= _BV(0); 219 | lcd_e_toggle(); 220 | delay(LCD_DELAY_INIT); 221 | 222 | lcd_e_toggle(); 223 | delay(LCD_DELAY_INIT_REP); 224 | 225 | lcd_e_toggle(); 226 | delay(LCD_DELAY_INIT_REP); 227 | 228 | LCD_DATA_PORT &= ~_BV(0); 229 | lcd_e_toggle(); 230 | delay(LCD_DELAY_INIT_4BIT); 231 | 232 | lcd_command(LCD_FUNCTION_DEFAULT); 233 | lcd_clrscr(); 234 | lcd_command(LCD_MODE_DEFAULT); 235 | lcd_command(LCD_DISP_ON); 236 | 237 | lcd_clear(); } 238 | 239 | ISR(TIMER2_COMPA_vect) { 240 | if (counter++ > 100) { 241 | sys_rt_exec_state |= EXEC_STATUS_REPORT; 242 | counter = 0; } } 243 | -------------------------------------------------------------------------------- /grbl/gcode.c: -------------------------------------------------------------------------------- 1 | #include "grbl.h" 2 | 3 | uint8_t gc_execute_line(char *line) { 4 | uint8_t char_counter = 0; 5 | char letter; 6 | uint8_t dwell = 0; 7 | uint16_t P = 0; 8 | float f_val; 9 | uint8_t int_value; 10 | while (line[char_counter] != 0) { 11 | if ((letter = line[char_counter++]) == '(') { 12 | break; } 13 | if ('P' == letter || 'I' == letter) { 14 | if ((letter = line[char_counter]) == 'D' || letter == 'U') { 15 | for (idx=0; idx lround((settings.max_travel[X_AXIS] + 0.5) * settings.steps_per_mm[X_AXIS])) { 27 | return(STATUS_SOFT_LIMIT_ERROR); } } } 28 | if (line[char_counter] != ';') { return(STATUS_GCODE_UNSUPPORTED_COMMAND); } 29 | if (line[1] == 'D') { 30 | if (pl_data.condition & PL_COND_FLAG_RAPID_MOTION) { 31 | protocol_buffer_synchronize(); 32 | if (sys.abort) { return(0); } 33 | int_value = ceil(1.0 / DWELL_TIME_STEP * 400)); 34 | //spindle_set_speed(255); 35 | while (int_value > 0) { 36 | buttons_check(); 37 | protocol_exec_rt_system(); 38 | if (sys.abort) { return(0); } 39 | _delay_ms(DWELL_TIME_STEP); 40 | int_value--; } } 41 | pl_data.condition = 0; 42 | } else { 43 | if (0 == pl_data.condition) { 44 | protocol_buffer_synchronize(); 45 | if (sys.abort) { return(0); } 46 | int_value = ceil(1.0 / DWELL_TIME_STEP * 400)); 47 | //spindle_set_speed(0); 48 | while (int_value > 0) { 49 | buttons_check(); 50 | protocol_exec_rt_system(); 51 | if (sys.abort) { return(0); } 52 | _delay_ms(DWELL_TIME_STEP); 53 | int_value--; } } 54 | pl_data.condition = PL_COND_FLAG_RAPID_MOTION; } 55 | break; 56 | } else if (letter == 'N') { 57 | if (line[char_counter+1] != ';') { return(STATUS_GCODE_UNSUPPORTED_COMMAND); } 58 | pl_data.condition = PL_COND_FLAG_RAPID_MOTION; 59 | pl_data.xyz[X_AXIS] = 0; 60 | pl_data.xyz[Y_AXIS] = 0; 61 | break; 62 | } else { 63 | return(STATUS_GCODE_UNSUPPORTED_COMMAND); } } 64 | if (!read_float(line, &char_counter, &f_val)) { return(STATUS_BAD_NUMBER_FORMAT); } 65 | switch (letter) { 66 | case 'G': 67 | switch ((int_value = trunc(f_val))) { 68 | case 0: 69 | pl_data.condition = PL_COND_FLAG_RAPID_MOTION; break; 70 | case 1: 71 | pl_data.condition = 0; break; 72 | case 4: 73 | dwell = 1; break; 74 | case 20: 75 | pl_data.units = 1; break; 76 | case 21: 77 | pl_data.units = 0; break; 78 | case 90: 79 | pl_data.distance = 0; break; 80 | case 91: 81 | pl_data.distance = 1; break; 82 | case 2: case 3: case 80: case 10: case 28: case 30: case 53: case 92: case 93: case 18: case 19: case 43: case 54: case 55: case 56: case 57: case 58: case 59: 83 | return(STATUS_GCODE_UNSUPPORTED_COMMAND); } 84 | break; 85 | case 'X': 86 | if (pl_data.units) { 87 | f_val *= 25.4; } 88 | if (pl_data.distance) { 89 | pl_data.gc_pos[X_AXIS] += f_val; 90 | } else { 91 | pl_data.gc_pos[X_AXIS] = f_val; } 92 | pl_data.xyz[X_AXIS] = lround(pl_data.gc_pos[X_AXIS]*settings.steps_per_mm[X_AXIS]) + wco[X_AXIS]; 93 | if (bit_istrue(settings.flags, BITFLAG_HOMING_ENABLE)) { 94 | if (pl_data.xyz[X_AXIS] < -0.5 || pl_data.xyz[X_AXIS] > lround((settings.max_travel[X_AXIS] + 0.5) * settings.steps_per_mm[X_AXIS])) { 95 | return(STATUS_SOFT_LIMIT_ERROR); } } 96 | break; 97 | case 'Y': 98 | if (pl_data.units) { 99 | f_val *= 25.4; } 100 | if (pl_data.distance) { 101 | pl_data.gc_pos[Y_AXIS] += f_val; 102 | } else { 103 | pl_data.gc_pos[Y_AXIS] = f_val; } 104 | pl_data.xyz[Y_AXIS] = lround(-pl_data.gc_pos[Y_AXIS]*settings.steps_per_mm[Y_AXIS]) + wco[Y_AXIS]; 105 | if (bit_istrue(settings.flags, BITFLAG_HOMING_ENABLE)) { 106 | if (pl_data.xyz[Y_AXIS] < -0.5 || pl_data.xyz[Y_AXIS] > lround((settings.max_travel[Y_AXIS] + 0.5) * settings.steps_per_mm[Y_AXIS])) { 107 | return(STATUS_SOFT_LIMIT_ERROR); } } 108 | break; 109 | case 'S': 110 | if (f_val > SPINDLE_PWM_MAX_VALUE) { 111 | f_val = SPINDLE_PWM_MAX_VALUE; 112 | } else if (f_val > SPINDLE_PWM_OFF_VALUE && f_val < SPINDLE_PWM_MIN_VALUE+1) { 113 | f_val = SPINDLE_PWM_MIN_VALUE+1; } 114 | pl_data.spindle_speed = trunc(f_val); 115 | break; 116 | case 'F': 117 | pl_data.feed_rate = f_val; 118 | break; 119 | case 'P': 120 | P = ceil(f_val * 1000.0); 121 | break; 122 | case 'M': 123 | switch ((int_value = trunc(f_val))) { 124 | case 0: 125 | protocol_buffer_synchronize(); 126 | if (sys.abort) { return(0); } 127 | sys.state = STATE_HOLD; 128 | SPINDLE_OCR_REGISTER = SPINDLE_PWM_MAX_VALUE; 129 | SPINDLE_TCCRA_REGISTER |= (1< 0) { 146 | protocol_buffer_synchronize(); 147 | if (sys.abort) { return(0); } 148 | sys.state = STATE_HOLD; 149 | sys.non_modal_dwell = max(1, ceil(1.0 / DWELL_TIME_STEP * P)); 150 | spindle_set_speed(pl_data.spindle_speed); 151 | while (sys.non_modal_dwell > 0) { 152 | buttons_check(); 153 | protocol_exec_rt_system(); 154 | if (sys.abort) { return(0); } 155 | _delay_ms(DWELL_TIME_STEP); 156 | sys.non_modal_dwell--; } 157 | spindle_set_speed(SPINDLE_PWM_OFF_VALUE); 158 | return(STATUS_OK); } 159 | 160 | while (plan_check_full_buffer()) { 161 | protocol_execute_realtime(); 162 | if (sys.abort) { return(0); } } 163 | 164 | plan_buffer_line(); 165 | 166 | protocol_auto_cycle_start(); 167 | protocol_execute_realtime(); 168 | if (sys.abort) { return(0); } 169 | 170 | return(STATUS_OK); } 171 | -------------------------------------------------------------------------------- /grbl/planner.c: -------------------------------------------------------------------------------- 1 | #include "grbl.h" 2 | 3 | #define PLAN_OK 1 4 | #define PLAN_EMPTY_BLOCK 0 5 | 6 | static plan_block_t block_buffer[BLOCK_BUFFER_SIZE]; 7 | static uint8_t block_buffer_tail; 8 | static uint8_t block_buffer_head; 9 | static uint8_t next_buffer_head; 10 | static uint8_t block_buffer_planned; 11 | 12 | typedef struct { 13 | int32_t position[N_AXIS]; 14 | float previous_unit_vec[N_AXIS]; 15 | float previous_nominal_speed; 16 | } planner_t; 17 | static planner_t pl; 18 | 19 | uint8_t plan_next_block_index(uint8_t block_index) { 20 | block_index++; 21 | if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } 22 | return(block_index); } 23 | 24 | static uint8_t plan_prev_block_index(uint8_t block_index) { 25 | if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } 26 | block_index--; 27 | return(block_index); } 28 | 29 | static void planner_recalculate() { 30 | uint8_t block_index = plan_prev_block_index(block_buffer_head); 31 | 32 | if (block_index == block_buffer_planned) { return; } 33 | 34 | float entry_speed_sqr; 35 | plan_block_t *next; 36 | plan_block_t *current = &block_buffer[block_index]; 37 | 38 | current->entry_speed_sqr = min(current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); 39 | 40 | block_index = plan_prev_block_index(block_index); 41 | if (block_index == block_buffer_planned) { 42 | if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); } 43 | } else { 44 | while (block_index != block_buffer_planned) { 45 | next = current; 46 | current = &block_buffer[block_index]; 47 | block_index = plan_prev_block_index(block_index); 48 | 49 | if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); } 50 | 51 | if (current->entry_speed_sqr != current->max_entry_speed_sqr) { 52 | entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; 53 | if (entry_speed_sqr < current->max_entry_speed_sqr) { 54 | current->entry_speed_sqr = entry_speed_sqr; 55 | } else { 56 | current->entry_speed_sqr = current->max_entry_speed_sqr; } } } } 57 | 58 | next = &block_buffer[block_buffer_planned]; 59 | block_index = plan_next_block_index(block_buffer_planned); 60 | while (block_index != block_buffer_head) { 61 | current = next; 62 | next = &block_buffer[block_index]; 63 | 64 | if (current->entry_speed_sqr < next->entry_speed_sqr) { 65 | entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; 66 | if (entry_speed_sqr < next->entry_speed_sqr) { 67 | next->entry_speed_sqr = entry_speed_sqr; 68 | block_buffer_planned = block_index; } } 69 | 70 | if (next->entry_speed_sqr == next->max_entry_speed_sqr) { block_buffer_planned = block_index; } 71 | block_index = plan_next_block_index(block_index); } } 72 | 73 | void plan_reset() { 74 | memset(&pl, 0, sizeof(planner_t)); 75 | block_buffer_tail = 0; 76 | block_buffer_head = 0; 77 | next_buffer_head = 1; 78 | block_buffer_planned = 0; } 79 | 80 | void plan_discard_current_block() { 81 | if (block_buffer_head != block_buffer_tail) { 82 | uint8_t block_index = plan_next_block_index(block_buffer_tail); 83 | if (block_buffer_tail == block_buffer_planned) { block_buffer_planned = block_index; } 84 | block_buffer_tail = block_index; } } 85 | 86 | plan_block_t *plan_get_current_block() { 87 | if (block_buffer_head == block_buffer_tail) { return(NULL); } 88 | return(&block_buffer[block_buffer_tail]); } 89 | 90 | float plan_get_exec_block_exit_speed_sqr() { 91 | uint8_t block_index = plan_next_block_index(block_buffer_tail); 92 | if (block_index == block_buffer_head) { return(0); } 93 | return(block_buffer[block_index].entry_speed_sqr); } 94 | 95 | uint8_t plan_check_full_buffer() { 96 | if (block_buffer_tail == next_buffer_head) { return(1); } 97 | return(0); } 98 | 99 | float plan_compute_profile_nominal_speed(plan_block_t *block) { 100 | float nominal_speed = block->programmed_rate; 101 | if (!(block->condition & PL_COND_FLAG_RAPID_MOTION)) { 102 | nominal_speed *= (.01*sys.f_override); 103 | if (nominal_speed > block->rapid_rate) { nominal_speed = block->rapid_rate; } } 104 | if (nominal_speed > MINIMUM_FEED_RATE) { return(nominal_speed); } 105 | return(MINIMUM_FEED_RATE); } 106 | 107 | static void plan_compute_profile_parameters(plan_block_t *block, float nominal_speed, float prev_nominal_speed) { 108 | if (nominal_speed > prev_nominal_speed) { 109 | block->max_entry_speed_sqr = prev_nominal_speed*prev_nominal_speed; 110 | } else { 111 | block->max_entry_speed_sqr = nominal_speed*nominal_speed; } 112 | if (block->max_entry_speed_sqr > block->max_junction_speed_sqr) { 113 | block->max_entry_speed_sqr = block->max_junction_speed_sqr; } } 114 | 115 | void plan_update_velocity_profile_parameters() { 116 | uint8_t block_index = block_buffer_tail; 117 | plan_block_t *block; 118 | float nominal_speed; 119 | float prev_nominal_speed = SOME_LARGE_VALUE; 120 | while (block_index != block_buffer_head) { 121 | block = &block_buffer[block_index]; 122 | nominal_speed = plan_compute_profile_nominal_speed(block); 123 | plan_compute_profile_parameters(block, nominal_speed, prev_nominal_speed); 124 | prev_nominal_speed = nominal_speed; 125 | block_index = plan_next_block_index(block_index); } 126 | pl.previous_nominal_speed = prev_nominal_speed; } 127 | 128 | uint8_t plan_buffer_line() { 129 | plan_block_t *block = &block_buffer[block_buffer_head]; 130 | memset(block, 0, sizeof(plan_block_t)); 131 | block->condition = pl_data.condition; 132 | 133 | int32_t position_steps[N_AXIS]; 134 | float unit_vec[N_AXIS], delta_mm; 135 | uint8_t idx; 136 | 137 | memcpy(position_steps, pl.position, sizeof(pl.position)); 138 | 139 | #ifdef COREXY 140 | block->steps[A_MOTOR] = labs((pl_data.xyz[X_AXIS]-position_steps[X_AXIS]) + (pl_data.xyz[Y_AXIS]-position_steps[Y_AXIS])); 141 | block->steps[B_MOTOR] = labs((pl_data.xyz[X_AXIS]-position_steps[X_AXIS]) - (pl_data.xyz[Y_AXIS]-position_steps[Y_AXIS])); 142 | #endif 143 | 144 | for (idx=0; idxstep_event_count = max(block->step_event_count, block->steps[idx]); 147 | if (idx == A_MOTOR) { 148 | delta_mm = (pl_data.xyz[X_AXIS]-position_steps[X_AXIS] + pl_data.xyz[Y_AXIS]-position_steps[Y_AXIS])/settings.steps_per_mm[idx]; 149 | } else { 150 | delta_mm = (pl_data.xyz[X_AXIS]-position_steps[X_AXIS] - pl_data.xyz[Y_AXIS]+position_steps[Y_AXIS])/settings.steps_per_mm[idx]; } 151 | #else 152 | block->steps[idx] = labs(pl_data.xyz[idx]-position_steps[idx]); 153 | block->step_event_count = max(block->step_event_count, block->steps[idx]); 154 | delta_mm = (pl_data.xyz[idx] - position_steps[idx])/settings.steps_per_mm[idx]; 155 | #endif 156 | unit_vec[idx] = delta_mm; 157 | 158 | if (delta_mm < 0.0 ) { block->direction_bits |= get_direction_pin_mask(idx); } } 159 | 160 | if (block->step_event_count == 0) { return(PLAN_EMPTY_BLOCK); } 161 | 162 | block->millimeters = convert_delta_vector_to_unit_vector(unit_vec); 163 | block->acceleration = limit_value_by_axis_maximum(settings.acceleration, unit_vec); 164 | block->rapid_rate = limit_value_by_axis_maximum(settings.max_rate, unit_vec); 165 | 166 | if (block->condition & PL_COND_FLAG_RAPID_MOTION) { 167 | block->programmed_rate = block->rapid_rate; 168 | } else { 169 | block->programmed_rate = pl_data.feed_rate; 170 | block->spindle_speed = pl_data.spindle_speed; 171 | } 172 | 173 | if (block_buffer_head == block_buffer_tail) { 174 | 175 | block->entry_speed_sqr = 0; 176 | block->max_junction_speed_sqr = 0; 177 | 178 | } else { 179 | 180 | float junction_unit_vec[N_AXIS]; 181 | float junction_cos_theta = 0; 182 | for (idx=0; idx .999999) { 187 | block->max_junction_speed_sqr = MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED; 188 | } else { 189 | if (junction_cos_theta < -.999999) { 190 | block->max_junction_speed_sqr = SOME_LARGE_VALUE; 191 | } else { 192 | convert_delta_vector_to_unit_vector(junction_unit_vec); 193 | float junction_acceleration = limit_value_by_axis_maximum(settings.acceleration, junction_unit_vec); 194 | float sin_theta_d2 = sqrt(.5*(1-junction_cos_theta)); 195 | block->max_junction_speed_sqr = max(MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, (junction_acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2)); } } } 196 | 197 | float nominal_speed = plan_compute_profile_nominal_speed(block); 198 | plan_compute_profile_parameters(block, nominal_speed, pl.previous_nominal_speed); 199 | pl.previous_nominal_speed = nominal_speed; 200 | 201 | memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); 202 | memcpy(pl.position, pl_data.xyz, sizeof(pl_data.xyz)); 203 | 204 | block_buffer_head = next_buffer_head; 205 | next_buffer_head = plan_next_block_index(block_buffer_head); 206 | 207 | planner_recalculate(); 208 | 209 | return(PLAN_OK); } 210 | 211 | void plan_sync_position() { 212 | #ifdef COREXY 213 | pl.position[X_AXIS] = system_convert_corexy_to_x_axis_steps(sys_position); 214 | pl.position[Y_AXIS] = system_convert_corexy_to_y_axis_steps(sys_position); 215 | #else 216 | memcpy(pl.position, sys_position, sizeof(pl.position)); 217 | #endif 218 | } 219 | 220 | int32_t plan_get_position(uint8_t idx) { 221 | return(pl.position[idx]); } 222 | 223 | uint8_t plan_get_block_buffer_count() { 224 | if (block_buffer_head >= block_buffer_tail) { return(block_buffer_head-block_buffer_tail); } 225 | return(BLOCK_BUFFER_SIZE - (block_buffer_tail-block_buffer_head)); } 226 | 227 | void plan_cycle_reinitialize() { 228 | st_update_plan_block_parameters(); 229 | block_buffer_planned = block_buffer_tail; 230 | planner_recalculate(); } 231 | -------------------------------------------------------------------------------- /grbl/fat.c: -------------------------------------------------------------------------------- 1 | #include "grbl.h" 2 | 3 | #define FAT16_CLUSTER_FREE 0x0000 4 | #define FAT16_CLUSTER_RESERVED_MIN 0xfff0 5 | #define FAT16_CLUSTER_RESERVED_MAX 0xfff6 6 | #define FAT16_CLUSTER_BAD 0xfff7 7 | #define FAT16_CLUSTER_LAST_MIN 0xfff8 8 | #define FAT16_CLUSTER_LAST_MAX 0xffff 9 | 10 | #define FAT32_CLUSTER_FREE 0x00000000 11 | #define FAT32_CLUSTER_RESERVED_MIN 0x0ffffff0 12 | #define FAT32_CLUSTER_RESERVED_MAX 0x0ffffff6 13 | #define FAT32_CLUSTER_BAD 0x0ffffff7 14 | #define FAT32_CLUSTER_LAST_MIN 0x0ffffff8 15 | #define FAT32_CLUSTER_LAST_MAX 0x0fffffff 16 | 17 | #define FAT_DIRENTRY_DELETED 0xe5 18 | 19 | struct fat_header_struct { 20 | uint64_t size; 21 | 22 | uint64_t fat_offset; 23 | uint32_t fat_size; 24 | 25 | uint16_t sector_size; 26 | uint16_t cluster_size; 27 | 28 | uint64_t cluster_zero_offset; 29 | 30 | uint64_t root_dir_offset; 31 | uint32_t root_dir_cluster; }; 32 | 33 | struct fat_fs_struct { 34 | struct partition_struct* partition; 35 | struct fat_header_struct header; 36 | uint32_t cluster_free; }; 37 | 38 | struct fat_file_struct { 39 | struct fat_fs_struct* fs; 40 | struct fat_dir_entry_struct dir_entry; 41 | uint64_t pos; 42 | uint32_t pos_cluster; }; 43 | 44 | struct fat_dir_struct { 45 | struct fat_fs_struct* fs; 46 | struct fat_dir_entry_struct dir_entry; 47 | uint32_t entry_cluster; 48 | uint16_t entry_offset; }; 49 | 50 | struct fat_read_dir_callback_arg { 51 | struct fat_dir_entry_struct* dir_entry; 52 | uintptr_t bytes_read; 53 | uint8_t finished; }; 54 | 55 | static struct partition_struct partition_handles[1]; 56 | static struct fat_fs_struct fat_fs_handles[1]; 57 | static struct fat_file_struct fat_file_handles[1]; 58 | static struct fat_dir_struct fat_dir_handles[1]; 59 | 60 | static uint8_t fat_read_header(struct fat_fs_struct *fs); 61 | static uint32_t fat_get_next_cluster(const struct fat_fs_struct *fs, uint32_t cluster_num); 62 | static uint64_t fat_cluster_offset(const struct fat_fs_struct *fs, uint32_t cluster_num); 63 | static uint8_t fat_dir_entry_read_callback(uint8_t *buffer, uint64_t offset, void *p); 64 | 65 | struct partition_struct *partition_open(device_read_t device_read, device_read_interval_t device_read_interval, int8_t index) { 66 | struct partition_struct *new_partition = 0; 67 | uint8_t buffer[0x10]; 68 | 69 | if(!device_read || !device_read_interval || index >= 4) { 70 | return 0; } 71 | 72 | if(index >= 0) { 73 | if(!device_read(0x01be + index * 0x10, buffer, sizeof(buffer))) { 74 | return 0; } 75 | 76 | if(buffer[4] == 0x00) { 77 | return 0; } } 78 | 79 | new_partition = partition_handles; 80 | uint8_t i; 81 | for(i = 0; i < 1; ++i) { 82 | if(new_partition->type == PARTITION_TYPE_FREE) { 83 | break; } 84 | 85 | ++new_partition; } 86 | 87 | if(i >= 1) { 88 | return 0; } 89 | 90 | memset(new_partition, 0, sizeof(*new_partition)); 91 | 92 | new_partition->device_read = device_read; 93 | new_partition->device_read_interval = device_read_interval; 94 | 95 | if(index >= 0) { 96 | new_partition->type = buffer[4]; 97 | new_partition->offset = read32(&buffer[8]); 98 | new_partition->length = read32(&buffer[12]); 99 | } else { 100 | new_partition->type = 0xff; } 101 | 102 | return new_partition; } 103 | 104 | void partition_close(struct partition_struct *partition) { 105 | if(partition) { 106 | partition->type = PARTITION_TYPE_FREE; } } 107 | 108 | struct fat_fs_struct *fat_open(struct partition_struct* partition) { 109 | if(!partition) { 110 | return 0; } 111 | 112 | struct fat_fs_struct *fs = fat_fs_handles; 113 | uint8_t i; 114 | for(i = 0; i < 1; ++i) { 115 | if(!fs->partition) { 116 | break; } 117 | 118 | ++fs; } 119 | if(i >= 1) { 120 | return 0; } 121 | 122 | memset(fs, 0, sizeof(*fs)); 123 | 124 | fs->partition = partition; 125 | if(!fat_read_header(fs)) { 126 | fs->partition = 0; 127 | return 0; } 128 | 129 | return fs; } 130 | 131 | void fat_close(struct fat_fs_struct *fs) { 132 | if(!fs) { 133 | return; } 134 | 135 | fs->partition = 0; } 136 | 137 | uint8_t fat_read_header(struct fat_fs_struct *fs) { 138 | if(!fs) { 139 | return 0; } 140 | 141 | struct partition_struct *partition = fs->partition; 142 | if(!partition) { 143 | return 0; } 144 | 145 | uint8_t buffer[37]; 146 | 147 | uint64_t partition_offset = (uint64_t) partition->offset * 512; 148 | if(!partition->device_read(partition_offset + 0x0b, buffer, sizeof(buffer))) { 149 | return 0; } 150 | 151 | uint16_t bytes_per_sector = read16(&buffer[0x00]); 152 | uint16_t reserved_sectors = read16(&buffer[0x03]); 153 | uint8_t sectors_per_cluster = buffer[0x02]; 154 | uint8_t fat_copies = buffer[0x05]; 155 | uint16_t max_root_entries = read16(&buffer[0x06]); 156 | uint16_t sector_count_16 = read16(&buffer[0x08]); 157 | uint16_t sectors_per_fat = read16(&buffer[0x0b]); 158 | uint32_t sector_count = read32(&buffer[0x15]); 159 | uint32_t sectors_per_fat32 = read32(&buffer[0x19]); 160 | uint32_t cluster_root_dir = read32(&buffer[0x21]); 161 | 162 | if(sector_count == 0) { 163 | if(sector_count_16 == 0) { 164 | return 0; 165 | } else { 166 | sector_count = sector_count_16; } } 167 | if(sectors_per_fat != 0) { 168 | sectors_per_fat32 = sectors_per_fat; 169 | } else if(sectors_per_fat32 == 0) { 170 | return 0; } 171 | 172 | uint32_t data_sector_count = sector_count - reserved_sectors - sectors_per_fat32 * fat_copies - ((max_root_entries * 32 + bytes_per_sector - 1) / bytes_per_sector); 173 | uint32_t data_cluster_count = data_sector_count / sectors_per_cluster; 174 | if(data_cluster_count < 4085) { 175 | return 0; 176 | } else if(data_cluster_count < 65525) { 177 | partition->type = PARTITION_TYPE_FAT16; 178 | } else { 179 | partition->type = PARTITION_TYPE_FAT32; } 180 | 181 | struct fat_header_struct *header = &fs->header; 182 | memset(header, 0, sizeof(*header)); 183 | 184 | header->size = (uint64_t) sector_count * bytes_per_sector; 185 | 186 | header->fat_offset = partition_offset + (uint64_t) reserved_sectors * bytes_per_sector; 187 | header->fat_size = (data_cluster_count + 2) * (partition->type == PARTITION_TYPE_FAT16 ? 2 : 4); 188 | 189 | header->sector_size = bytes_per_sector; 190 | header->cluster_size = (uint16_t) bytes_per_sector * sectors_per_cluster; 191 | 192 | if(partition->type == PARTITION_TYPE_FAT16) { 193 | header->root_dir_offset = header->fat_offset + (uint64_t) fat_copies * sectors_per_fat * bytes_per_sector; 194 | 195 | header->cluster_zero_offset = header->root_dir_offset + (uint64_t) max_root_entries * 32; 196 | } else { 197 | header->cluster_zero_offset = header->fat_offset + (uint64_t) fat_copies * sectors_per_fat32 * bytes_per_sector; 198 | 199 | header->root_dir_cluster = cluster_root_dir; } 200 | 201 | return 1; } 202 | 203 | uint32_t fat_get_next_cluster(const struct fat_fs_struct *fs, uint32_t cluster_num) { 204 | if(!fs || cluster_num < 2) { 205 | return 0; } 206 | 207 | if(fs->partition->type == PARTITION_TYPE_FAT32) { 208 | uint32_t fat_entry; 209 | if(!fs->partition->device_read(fs->header.fat_offset + (uint64_t) cluster_num * sizeof(fat_entry), (uint8_t*) &fat_entry, sizeof(fat_entry))) { 210 | return 0; } 211 | 212 | cluster_num = ltoh32(fat_entry); 213 | 214 | if(cluster_num == FAT32_CLUSTER_FREE || cluster_num == FAT32_CLUSTER_BAD || (cluster_num >= FAT32_CLUSTER_RESERVED_MIN && cluster_num <= FAT32_CLUSTER_RESERVED_MAX) || (cluster_num >= FAT32_CLUSTER_LAST_MIN && cluster_num <= FAT32_CLUSTER_LAST_MAX)) { 215 | return 0; } 216 | } else { 217 | uint16_t fat_entry; 218 | if(!fs->partition->device_read(fs->header.fat_offset + (uint64_t) cluster_num * sizeof(fat_entry), (uint8_t*) &fat_entry, sizeof(fat_entry))) { 219 | return 0; } 220 | 221 | cluster_num = ltoh16(fat_entry); 222 | 223 | if(cluster_num == FAT16_CLUSTER_FREE || cluster_num == FAT16_CLUSTER_BAD || (cluster_num >= FAT16_CLUSTER_RESERVED_MIN && cluster_num <= FAT16_CLUSTER_RESERVED_MAX) || (cluster_num >= FAT16_CLUSTER_LAST_MIN && cluster_num <= FAT16_CLUSTER_LAST_MAX)) { 224 | return 0; } } 225 | 226 | return cluster_num; } 227 | 228 | uint64_t fat_cluster_offset(const struct fat_fs_struct *fs, uint32_t cluster_num) { 229 | if(!fs || cluster_num < 2) { 230 | return 0; } 231 | 232 | return fs->header.cluster_zero_offset + (uint64_t) (cluster_num - 2) * fs->header.cluster_size; } 233 | 234 | uint8_t fat_get_dir_entry_of_path(struct fat_fs_struct *fs, const char* path, struct fat_dir_entry_struct *dir_entry) { 235 | if(!fs || !path || path[0] == '\0' || !dir_entry) { 236 | return 0; } 237 | 238 | if(path[0] == '/') { 239 | ++path; } 240 | 241 | memset(dir_entry, 0, sizeof(*dir_entry)); 242 | dir_entry->attributes = FAT_ATTRIB_DIR; 243 | 244 | while(1) { 245 | if(path[0] == '\0') { 246 | return 1; } 247 | 248 | struct fat_dir_struct *dd = fat_open_dir(fs, dir_entry); 249 | if(!dd) { 250 | break; } 251 | 252 | const char *sub_path = strchr(path, '/'); 253 | uint8_t length_to_sep; 254 | if(sub_path) { 255 | length_to_sep = sub_path - path; 256 | ++sub_path; 257 | } else { 258 | length_to_sep = strlen(path); 259 | sub_path = path + length_to_sep; } 260 | 261 | while(fat_read_dir(dd, dir_entry)) { 262 | if((strlen(dir_entry->long_name) != length_to_sep || strncmp(path, dir_entry->long_name, length_to_sep) != 0)) { 263 | continue; } 264 | 265 | fat_close_dir(dd); 266 | dd = 0; 267 | 268 | if(path[length_to_sep] == '\0') { 269 | return 1; } 270 | 271 | if(dir_entry->attributes & FAT_ATTRIB_DIR) { 272 | path = sub_path; 273 | break; } 274 | 275 | return 0; } 276 | 277 | fat_close_dir(dd); } 278 | 279 | return 0; } 280 | 281 | struct fat_file_struct *fat_open_file(struct fat_fs_struct *fs, const struct fat_dir_entry_struct *dir_entry) { 282 | if(!fs || !dir_entry || (dir_entry->attributes & FAT_ATTRIB_DIR)) { 283 | return 0; } 284 | 285 | struct fat_file_struct *fd = fat_file_handles; 286 | uint8_t i; 287 | for(i = 0; i < 1; ++i) { 288 | if(!fd->fs) { 289 | break; } 290 | 291 | ++fd; } 292 | if(i >= 1) { 293 | return 0; } 294 | 295 | memcpy(&fd->dir_entry, dir_entry, sizeof(*dir_entry)); 296 | fd->fs = fs; 297 | fd->pos = 0; 298 | fd->pos_cluster = dir_entry->cluster; 299 | 300 | return fd; } 301 | 302 | void fat_close_file(struct fat_file_struct *fd) { 303 | if(fd) { 304 | fd->fs = 0; } } 305 | 306 | uint8_t fat_read_byte(struct fat_file_struct *fd) { 307 | if(!fd || fd->pos + 1 > fd->dir_entry.file_size) { 308 | return 0xff; } 309 | 310 | uint16_t cluster_size = fd->fs->header.cluster_size; 311 | uint32_t cluster_num = fd->pos_cluster; 312 | uint16_t first_cluster_offset = (uint16_t)(fd->pos & (cluster_size - 1)); 313 | 314 | if(!cluster_num) { 315 | cluster_num = fd->dir_entry.cluster; 316 | 317 | if(!cluster_num) { 318 | return 0xff; } 319 | 320 | if(fd->pos) { 321 | uint32_t pos = fd->pos; 322 | while(pos >= cluster_size) { 323 | pos -= cluster_size; 324 | cluster_num = fat_get_next_cluster(fd->fs, cluster_num); 325 | if(!cluster_num) { 326 | return 0xff; } } } } 327 | 328 | uint64_t cluster_offset = fat_cluster_offset(fd->fs, cluster_num) + first_cluster_offset; 329 | 330 | uint8_t buffer[1]; 331 | if(!fd->fs->partition->device_read(cluster_offset, buffer, 1)) { 332 | return 0xff; } 333 | 334 | fd->pos++; 335 | 336 | if(first_cluster_offset + 1 >= cluster_size) { 337 | if((cluster_num = fat_get_next_cluster(fd->fs, cluster_num))) { 338 | first_cluster_offset = 0; 339 | } else { 340 | fd->pos_cluster = 0; 341 | return buffer[0]; } } 342 | 343 | fd->pos_cluster = cluster_num; 344 | 345 | return buffer[0]; } 346 | 347 | struct fat_dir_struct* fat_open_dir(struct fat_fs_struct *fs, const struct fat_dir_entry_struct *dir_entry) { 348 | if(!fs || !dir_entry || !(dir_entry->attributes & FAT_ATTRIB_DIR)) { 349 | return 0; } 350 | 351 | struct fat_dir_struct* dd = fat_dir_handles; 352 | uint8_t i; 353 | for(i = 0; i < 1; ++i) { 354 | if(!dd->fs) { 355 | break; } 356 | 357 | ++dd; } 358 | if(i >= 1) { 359 | return 0; } 360 | 361 | memcpy(&dd->dir_entry, dir_entry, sizeof(*dir_entry)); 362 | dd->fs = fs; 363 | dd->entry_cluster = dir_entry->cluster; 364 | dd->entry_offset = 0; 365 | 366 | return dd; } 367 | 368 | void fat_close_dir(struct fat_dir_struct *dd) { 369 | if(dd) { 370 | dd->fs = 0; } } 371 | 372 | uint8_t fat_read_dir(struct fat_dir_struct *dd, struct fat_dir_entry_struct *dir_entry) { 373 | if(!dd || !dir_entry) { 374 | return 0; } 375 | 376 | struct fat_fs_struct* fs = dd->fs; 377 | const struct fat_header_struct* header = &fs->header; 378 | uint16_t cluster_size = header->cluster_size; 379 | uint32_t cluster_num = dd->entry_cluster; 380 | uint16_t cluster_offset = dd->entry_offset; 381 | struct fat_read_dir_callback_arg arg; 382 | 383 | if(cluster_offset >= cluster_size) { 384 | fat_reset_dir(dd); 385 | return 0; } 386 | 387 | memset(&arg, 0, sizeof(arg)); 388 | memset(dir_entry, 0, sizeof(*dir_entry)); 389 | arg.dir_entry = dir_entry; 390 | 391 | if(cluster_num == 0) { 392 | if(fs->partition->type == PARTITION_TYPE_FAT32) { 393 | cluster_num = header->root_dir_cluster; 394 | } else { 395 | cluster_size = header->cluster_zero_offset - header->root_dir_offset; } } 396 | 397 | uint8_t buffer[32]; 398 | while(!arg.finished) { 399 | uint16_t cluster_left = cluster_size - cluster_offset; 400 | uint64_t pos = cluster_offset; 401 | if(cluster_num == 0) { 402 | pos += header->root_dir_offset; 403 | } else { 404 | pos += fat_cluster_offset(fs, cluster_num); } 405 | 406 | arg.bytes_read = 0; 407 | if(!fs->partition->device_read_interval(pos, buffer, sizeof(buffer), cluster_left, fat_dir_entry_read_callback, &arg)) { 408 | return 0; } 409 | 410 | cluster_offset += arg.bytes_read; 411 | 412 | if(cluster_offset >= cluster_size) { 413 | if((cluster_num = fat_get_next_cluster(fs, cluster_num)) != 0) { 414 | cluster_offset = 0; 415 | continue; } 416 | if(!arg.finished) { 417 | fat_reset_dir(dd); 418 | return 0; } 419 | break; } } 420 | 421 | dd->entry_cluster = cluster_num; 422 | dd->entry_offset = cluster_offset; 423 | 424 | return arg.finished; } 425 | 426 | uint8_t fat_reset_dir(struct fat_dir_struct *dd) { 427 | if(!dd) { 428 | return 0; } 429 | 430 | dd->entry_cluster = dd->dir_entry.cluster; 431 | dd->entry_offset = 0; 432 | return 1; } 433 | 434 | uint8_t fat_dir_entry_read_callback(uint8_t *buffer, uint64_t offset, void *p) { 435 | struct fat_read_dir_callback_arg *arg = p; 436 | struct fat_dir_entry_struct *dir_entry = arg->dir_entry; 437 | 438 | arg->bytes_read += 32; 439 | 440 | if(buffer[0] == FAT_DIRENTRY_DELETED || !buffer[0]) { 441 | return 1; } 442 | 443 | if(buffer[11] == 0x0f) { 444 | return 1; } 445 | 446 | char *long_name = dir_entry->long_name; 447 | 448 | memset(dir_entry, 0, sizeof(*dir_entry)); 449 | dir_entry->entry_offset = offset; 450 | 451 | uint8_t i; 452 | for(i = 0; i < 8; ++i) { 453 | if(buffer[i] == ' ') { 454 | break; } 455 | long_name[i] = buffer[i]; 456 | 457 | if((buffer[12] & 0x08) && buffer[i] >= 'A' && buffer[i] <= 'Z') { 458 | long_name[i] += 'a' - 'A'; } } 459 | 460 | if(long_name[0] == 0x05) { 461 | long_name[0] = (char) FAT_DIRENTRY_DELETED; } 462 | 463 | if(buffer[8] != ' ') { 464 | long_name[i++] = '.'; 465 | 466 | uint8_t j = 8; 467 | for(; j < 11; ++j) { 468 | if(buffer[j] == ' ') { 469 | break; } 470 | long_name[i] = buffer[j]; 471 | 472 | if((buffer[12] & 0x10) && buffer[j] >= 'A' && buffer[j] <= 'Z') { 473 | long_name[i] += 'a' - 'A'; } 474 | 475 | ++i; } } 476 | 477 | long_name[i] = '\0'; 478 | 479 | dir_entry->attributes = buffer[11]; 480 | dir_entry->cluster = read16(&buffer[26]); 481 | dir_entry->cluster |= ((uint32_t) read16(&buffer[20])) << 16; 482 | dir_entry->file_size = read32(&buffer[28]); 483 | 484 | arg->finished = 1; 485 | return 0; } 486 | -------------------------------------------------------------------------------- /grbl/protocol.c: -------------------------------------------------------------------------------- 1 | #include "grbl.h" 2 | 3 | #define LINE_FLAG_OVERFLOW bit(0) 4 | 5 | static char line[LINE_BUFFER_SIZE]; 6 | 7 | void protocol_main_loop() { 8 | uint8_t line_flags = 0; 9 | uint8_t char_counter = 0; 10 | uint8_t c, idx, val, f_cnt; 11 | struct partition_struct *partition; 12 | struct fat_fs_struct *fs; 13 | struct fat_dir_struct *dd; 14 | struct fat_file_struct *fd; 15 | struct fat_dir_entry_struct dir_entry; 16 | for (;;) { 17 | if (btn_state && !(btn_state & (1<<4))) { 18 | if (btn_state & (1<<7)) { 19 | btn_state = 0; 20 | #ifndef COREXY 21 | if ((val = limits_go_home()) != STATUS_OK) { 22 | lcd_error(val); 23 | while(1) { 24 | if (abs(adc_read(CONTROL_OTHER) - (val = adc_read(CONTROL_OTHER))) <= 2) { 25 | if (val < 58) { 26 | if (btn) { 27 | btn = 0; 28 | break; } 29 | } else { 30 | btn = 1; } } 31 | _delay_ms(10); } 32 | lcd_clear(); } 33 | #endif 34 | } else if (btn_state & (1<<6)) { 35 | if (sd_raw_init()) { 36 | btn_state = 0; 37 | sys.state2 = STATE_FILE; 38 | if (!(partition = partition_open(sd_raw_read,sd_raw_read_interval,0))) { 39 | if (!(partition = partition_open(sd_raw_read,sd_raw_read_interval,-1))) { 40 | lcd_error(101); } } 41 | if (partition) { 42 | if ((fs = fat_open(partition))) { 43 | fat_get_dir_entry_of_path(fs, "/", &dir_entry); 44 | if ((dd = fat_open_dir(fs, &dir_entry))) { 45 | idx = 0; f_cnt = 0; 46 | while (fat_read_dir(dd, &dir_entry)) { 47 | if (!(dir_entry.attributes & FAT_ATTRIB_DIR)) { 48 | f_cnt++; } } 49 | if (f_cnt > 0) { 50 | lcd_clrscr(); val = 0; 51 | fat_reset_dir(dd); 52 | while (fat_read_dir(dd, &dir_entry)) { 53 | if (!(dir_entry.attributes & FAT_ATTRIB_DIR)) { 54 | lcd_gotoxy(0,val); 55 | if (val == 0) { 56 | lcd_puts_P("~"); lcd_file(dir_entry.long_name); 57 | } else { 58 | lcd_puts_P(" "); lcd_file(dir_entry.long_name); break; } 59 | val++; } } 60 | while(1) { 61 | if (f_cnt > 1) { 62 | if ((val = adc_read(CONTROL_JOG_Y)) < 63) { 63 | if (!(btn_state & (1<<4))) { 64 | btn_state |= (1<<4); 65 | val = 0; idx++; 66 | fat_reset_dir(dd); 67 | if (idx < f_cnt) { 68 | while (fat_read_dir(dd, &dir_entry)) { 69 | if (!(dir_entry.attributes & FAT_ATTRIB_DIR)) { 70 | if (val == idx-1) { 71 | lcd_gotoxy(0,0); 72 | lcd_puts_P(" "); lcd_file(dir_entry.long_name); 73 | } else if (val == idx) { 74 | lcd_gotoxy(0,1); 75 | lcd_puts_P("~"); lcd_file(dir_entry.long_name); break; } 76 | val++; } } 77 | } else { 78 | idx = 0; 79 | while (fat_read_dir(dd, &dir_entry)) { 80 | if (!(dir_entry.attributes & FAT_ATTRIB_DIR)) { 81 | if (val == idx) { 82 | lcd_gotoxy(0,0); 83 | lcd_puts_P("~"); lcd_file(dir_entry.long_name); 84 | } else if (val == idx+1) { 85 | lcd_gotoxy(0,1); 86 | lcd_puts_P(" "); lcd_file(dir_entry.long_name); break; } 87 | val++; } } } } 88 | } else if (val < 191) { 89 | if (!(btn_state & (1<<4))) { 90 | btn_state |= (1<<4); 91 | val = 0; 92 | fat_reset_dir(dd); 93 | if (idx > 0) { 94 | idx--; 95 | while (fat_read_dir(dd, &dir_entry)) { 96 | if (!(dir_entry.attributes & FAT_ATTRIB_DIR)) { 97 | if (val == idx) { 98 | lcd_gotoxy(0,0); 99 | lcd_puts_P("~"); lcd_file(dir_entry.long_name); 100 | } else if (val == idx+1) { 101 | lcd_gotoxy(0,1); 102 | lcd_puts_P(" "); lcd_file(dir_entry.long_name); break; } 103 | val++; } } 104 | } else { 105 | idx = f_cnt-1; 106 | while (fat_read_dir(dd, &dir_entry)) { 107 | if (!(dir_entry.attributes & FAT_ATTRIB_DIR)) { 108 | if (val == idx-1) { 109 | lcd_gotoxy(0,0); 110 | lcd_puts_P(" "); lcd_file(dir_entry.long_name); 111 | } else if (val == idx) { 112 | lcd_gotoxy(0,1); 113 | lcd_puts_P("~"); lcd_file(dir_entry.long_name); break; } 114 | val++; } } } } 115 | } else { 116 | btn_state &= ~(1<<4); } } 117 | if (btn_state & (1<<4)) { continue; } 118 | if (abs(adc_read(CONTROL_OTHER) - (val = adc_read(CONTROL_OTHER))) <= 2) { 119 | if (val < 58) { 120 | if (btn) { 121 | btn = 0; 122 | if (val < 21) { 123 | break; } 124 | fat_reset_dir(dd); val = 0; 125 | while (fat_read_dir(dd, &dir_entry)) { 126 | if (!(dir_entry.attributes & FAT_ATTRIB_DIR)) { 127 | if (val == idx) { 128 | fd = fat_open_file(fs, &dir_entry); 129 | break; } 130 | val++; } } 131 | break; } 132 | } else { 133 | btn = 1; } } 134 | _delay_ms(10); } 135 | if (fd) { 136 | lcd_clear(); 137 | 138 | sys.f_override = 100; 139 | pl_data.feed_rate = 1200; 140 | pl_data.spindle_speed = 255; 141 | pl_data.units = 0; 142 | pl_data.distance = 0; 143 | pl_data.condition = PL_COND_FLAG_RAPID_MOTION; 144 | pl_data.gc_pos[X_AXIS] = ((pl_data.xyz[X_AXIS] = plan_get_position(X_AXIS)) - wco[X_AXIS]) / settings.steps_per_mm[X_AXIS]; 145 | pl_data.gc_pos[Y_AXIS] = ((pl_data.xyz[Y_AXIS] = plan_get_position(Y_AXIS)) - wco[Y_AXIS]) / settings.steps_per_mm[Y_AXIS]; 146 | spindle_set_speed(SPINDLE_PWM_OFF_VALUE); lcd_state2(); 147 | char_counter = 0; val = STATUS_OK; 148 | while ((c = fat_read_byte(fd)) != 0xff) { 149 | if (c == '\n' || c == '\r') { 150 | if (char_counter > 0) { 151 | if (sys.abort) { break; } 152 | 153 | if (sys.state != STATE_HOLD) { 154 | protocol_execute_realtime(); 155 | if (sys.abort) { break; } } 156 | 157 | line[char_counter] = 0; 158 | 159 | if ((val = gc_execute_line(line)) != STATUS_OK) { 160 | break; } 161 | 162 | char_counter = 0; } 163 | 164 | } else if (c != ' ') { 165 | 166 | if (char_counter >= (LINE_BUFFER_SIZE-1)) { 167 | val = STATUS_OVERFLOW; break; 168 | } else { 169 | line[char_counter++] = c; } } } 170 | 171 | protocol_buffer_synchronize(); 172 | if (sys.abort) { val = STATUS_OK; } 173 | if (val != STATUS_OK) { 174 | lcd_error(val); 175 | while(1) { 176 | if (abs(adc_read(CONTROL_OTHER) - (val = adc_read(CONTROL_OTHER))) <= 2) { 177 | if (val < 58) { 178 | if (btn) { 179 | btn = 0; 180 | break; } 181 | } else { 182 | btn = 1; } } 183 | _delay_ms(10); } 184 | lcd_clear(); } 185 | 186 | fat_close_file(fd); 187 | fat_close_dir(dd); 188 | fat_close(fs); 189 | partition_close(partition); 190 | return; } 191 | } else { 192 | lcd_error(102); } 193 | } else { 194 | lcd_error(101); } 195 | } else { 196 | lcd_error(101); } } 197 | } else { 198 | lcd_error(100); } 199 | fat_close_dir(dd); 200 | fat_close(fs); 201 | partition_close(partition); 202 | if (f_cnt == 0) { 203 | while(1) { 204 | if (abs(adc_read(CONTROL_OTHER) - (val = adc_read(CONTROL_OTHER))) <= 2) { 205 | if (val < 58) { 206 | if (btn) { 207 | if (val < 21) { 208 | btn_state = 0; } 209 | btn = 0; 210 | break; } 211 | } else { 212 | btn = 1; } } 213 | _delay_ms(10); } } 214 | if (btn_state) { 215 | continue; 216 | } else { 217 | lcd_clear(); } 218 | } else if (btn_state & (1<<5)) { 219 | btn_state = 0; 220 | val = 35; 221 | while (!btn && val > 0) { 222 | _delay_ms(10); 223 | buttons_check(); 224 | val--; } 225 | if (!btn) { 226 | if (bit_isfalse(settings.flags,BITFLAG_HOMING_ENABLE)) { 227 | memset(sys_position, 0, sizeof(sys_position)); 228 | plan_sync_position(); 229 | } else { 230 | memcpy(wco, sys_position, sizeof(wco)); 231 | memcpy_to_eeprom_with_checksum(EEPROM_ADDR_PARAMETERS, (char*)&wco, sizeof(wco)); } 232 | } else { 233 | sys.state2 = STATE_ORIGIN; 234 | gc_execute_line("IN;"); } 235 | } else { 236 | val = 35; 237 | jog_execute(1); 238 | while (btn_state && val > 0) { 239 | _delay_ms(10); 240 | buttons_check(); 241 | protocol_exec_rt_system(); 242 | if (sys.abort) { return; } 243 | val--; } 244 | while (btn_state) { 245 | protocol_execute_realtime(); 246 | if (sys.abort) { return; } 247 | jog_execute(2); } 248 | if (sys.abort) { return; } } 249 | serial_reset_read_buffer(); } 250 | 251 | while ((c = serial_read()) != SERIAL_NO_DATA) { 252 | if (c == '\n' || c == '\r') { 253 | if (char_counter > 0) { 254 | if (sys.abort) { break; } 255 | 256 | if (sys.state != STATE_HOLD) { 257 | protocol_execute_realtime(); 258 | if (sys.abort) { break; } } 259 | 260 | line[char_counter] = 0; 261 | 262 | if (line_flags & LINE_FLAG_OVERFLOW) { 263 | report_status_message(STATUS_OVERFLOW); 264 | } else if (line[0] == '$') { 265 | report_status_message(system_execute_line(line)); 266 | } else if (sys.state & (STATE_ALARM | STATE_JOG)) { 267 | report_status_message(STATUS_SYSTEM_GC_LOCK); 268 | } else { 269 | report_status_message(gc_execute_line(line)); } 270 | 271 | line_flags = 0; 272 | char_counter = 0; } 273 | 274 | } else if (c == '#') { 275 | 276 | line_flags = 0; 277 | char_counter = 0; 278 | 279 | } else { 280 | 281 | if (!line_flags) { 282 | if (char_counter >= (LINE_BUFFER_SIZE-1)) { 283 | line_flags |= LINE_FLAG_OVERFLOW; 284 | } else { 285 | line[char_counter++] = c; } } } } 286 | if (sys.abort) { return; } 287 | 288 | if (STATE_IDLE == sys.state) { 289 | if (bit_istrue(settings.flags,BITFLAG_XY_HOME_PIN_AS_ST_ENABLE)) { 290 | if ((bit_isfalse(settings.flags,BITFLAG_INVERT_LIMIT_PINS) ? bit_isfalse(STEPPERS_DISABLE_PORT,(1<> 3); 107 | TCCR0B = (1<prescaler<cycles_per_tick; 121 | st.step_count = st.exec_segment->n_step; 122 | if (st.exec_block_index != st.exec_segment->st_block_index) { 123 | st.exec_block_index = st.exec_segment->st_block_index; 124 | st.exec_block = &st_block_buffer[st.exec_block_index]; 125 | 126 | st.counter_x = st.counter_y = (st.exec_block->step_event_count >> 1); } 127 | st.dir_outbits = st.exec_block->direction_bits ^ dir_port_invert_mask; 128 | 129 | #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING 130 | st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.exec_segment->amass_level; 131 | st.steps[Y_AXIS] = st.exec_block->steps[Y_AXIS] >> st.exec_segment->amass_level; 132 | #endif 133 | 134 | if (sys.state == STATE_CYCLE && sys.spindle_speed != SPINDLE_PWM_MIN_VALUE) { 135 | spindle_set_speed(st.exec_segment->spindle_pwm); } 136 | } else { 137 | TIMSK1 &= ~(1< SPINDLE_PWM_MIN_VALUE) { spindle_set_speed(SPINDLE_PWM_OFF_VALUE); } 141 | system_set_exec_state_flag(EXEC_CYCLE_STOP); 142 | busy = 0; 143 | return; } } 144 | 145 | st.step_outbits = 0; 146 | 147 | #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING 148 | st.counter_x += st.steps[X_AXIS]; 149 | #else 150 | st.counter_x += st.exec_block->steps[X_AXIS]; 151 | #endif 152 | if (st.counter_x > st.exec_block->step_event_count) { 153 | st.step_outbits |= (1<step_event_count; 155 | if (st.exec_block->direction_bits & (1<steps[Y_AXIS]; 162 | #endif 163 | if (st.counter_y > st.exec_block->step_event_count) { 164 | st.step_outbits |= (1<step_event_count; 166 | if (st.exec_block->direction_bits & (1<entry_speed_sqr = prep.current_speed*prep.current_speed; 226 | pl_block = NULL; } } 227 | 228 | static uint8_t st_next_block_index(uint8_t block_index) { 229 | block_index++; 230 | if (block_index == (SEGMENT_BUFFER_SIZE-1)) { return(0); } 231 | return(block_index); } 232 | 233 | void st_prep_buffer() { 234 | if (bit_istrue(sys.step_control,STEP_CONTROL_END_MOTION)) { return; } 235 | 236 | while (segment_buffer_tail != segment_next_head) { 237 | 238 | if (pl_block == NULL) { 239 | 240 | pl_block = plan_get_current_block(); 241 | if (pl_block == NULL) { return; } 242 | 243 | if (prep.recalculate_flag & PREP_FLAG_RECALCULATE) { 244 | 245 | prep.recalculate_flag = 0; 246 | 247 | } else { 248 | 249 | prep.st_block_index = st_next_block_index(prep.st_block_index); 250 | 251 | st_prep_block = &st_block_buffer[prep.st_block_index]; 252 | st_prep_block->direction_bits = pl_block->direction_bits; 253 | uint8_t idx; 254 | #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING 255 | for (idx=0; idxsteps[idx] = (pl_block->steps[idx] << 1); } 256 | st_prep_block->step_event_count = (pl_block->step_event_count << 1); 257 | #else 258 | for (idx=0; idxsteps[idx] = pl_block->steps[idx] << MAX_AMASS_LEVEL; } 259 | st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL; 260 | #endif 261 | 262 | prep.steps_remaining = (float)pl_block->step_event_count; 263 | prep.step_per_mm = prep.steps_remaining/pl_block->millimeters; 264 | prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR/prep.step_per_mm; 265 | prep.dt_remainder = 0.0; 266 | 267 | if ((sys.step_control & STEP_CONTROL_EXECUTE_HOLD) || (prep.recalculate_flag & PREP_FLAG_DECEL_OVERRIDE)) { 268 | prep.current_speed = prep.exit_speed; 269 | pl_block->entry_speed_sqr = prep.exit_speed*prep.exit_speed; 270 | prep.recalculate_flag &= ~(PREP_FLAG_DECEL_OVERRIDE); 271 | } else { 272 | prep.current_speed = sqrt(pl_block->entry_speed_sqr); } } 273 | 274 | prep.mm_complete = 0; 275 | float inv_2_accel = .5/pl_block->acceleration; 276 | 277 | if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) { 278 | 279 | prep.ramp_type = RAMP_DECEL; 280 | 281 | float decel_dist = pl_block->millimeters - inv_2_accel*pl_block->entry_speed_sqr; 282 | if (decel_dist < 0) { 283 | prep.exit_speed = sqrt(pl_block->entry_speed_sqr-2*pl_block->acceleration*pl_block->millimeters); 284 | } else { 285 | prep.mm_complete = decel_dist; 286 | prep.exit_speed = 0; } 287 | 288 | } else { 289 | 290 | prep.ramp_type = RAMP_ACCEL; 291 | prep.accelerate_until = pl_block->millimeters; 292 | 293 | float exit_speed_sqr; 294 | float nominal_speed; 295 | exit_speed_sqr = plan_get_exec_block_exit_speed_sqr(); 296 | prep.exit_speed = sqrt(exit_speed_sqr); 297 | 298 | nominal_speed = plan_compute_profile_nominal_speed(pl_block); 299 | float nominal_speed_sqr = nominal_speed*nominal_speed; 300 | float intersect_distance = .5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr)); 301 | if (pl_block->entry_speed_sqr > nominal_speed_sqr) { 302 | 303 | prep.accelerate_until = pl_block->millimeters - inv_2_accel*(pl_block->entry_speed_sqr-nominal_speed_sqr); 304 | if (prep.accelerate_until <= 0) { 305 | 306 | prep.ramp_type = RAMP_DECEL; 307 | //prep.decelerate_after = pl_block->millimeters; 308 | //prep.maximum_speed = prep.current_speed; 309 | 310 | prep.exit_speed = sqrt(pl_block->entry_speed_sqr - 2*pl_block->acceleration*pl_block->millimeters); 311 | prep.recalculate_flag |= PREP_FLAG_DECEL_OVERRIDE; 312 | 313 | } else { 314 | 315 | prep.decelerate_after = inv_2_accel*(nominal_speed_sqr-exit_speed_sqr); 316 | prep.maximum_speed = nominal_speed; 317 | prep.ramp_type = RAMP_DECEL_OVERRIDE; } 318 | 319 | } else if (intersect_distance > 0) { 320 | 321 | if (intersect_distance < pl_block->millimeters) { 322 | prep.decelerate_after = inv_2_accel*(nominal_speed_sqr-exit_speed_sqr); 323 | if (prep.decelerate_after < intersect_distance) { 324 | prep.maximum_speed = nominal_speed; 325 | if (pl_block->entry_speed_sqr == nominal_speed_sqr) { 326 | prep.ramp_type = RAMP_CRUISE; 327 | } else { 328 | prep.accelerate_until -= inv_2_accel*(nominal_speed_sqr-pl_block->entry_speed_sqr); } 329 | } else { 330 | prep.accelerate_until = intersect_distance; 331 | prep.decelerate_after = intersect_distance; 332 | prep.maximum_speed = sqrt(2*pl_block->acceleration*intersect_distance+exit_speed_sqr); } 333 | } else { 334 | prep.ramp_type = RAMP_DECEL; 335 | //prep.decelerate_after = pl_block->millimeters; 336 | //prep.maximum_speed = prep.current_speed; 337 | } 338 | 339 | } else { 340 | 341 | prep.accelerate_until = 0; 342 | //prep.decelerate_after = 0; 343 | prep.maximum_speed = prep.exit_speed; } } 344 | 345 | bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); 346 | } 347 | 348 | segment_t *prep_segment = &segment_buffer[segment_buffer_head]; 349 | 350 | prep_segment->st_block_index = prep.st_block_index; 351 | 352 | float dt_max = DT_SEGMENT; 353 | float dt = 0; 354 | float time_var = dt_max; 355 | float mm_var; 356 | float speed_var; 357 | float mm_remaining = pl_block->millimeters; 358 | float minimum_mm = mm_remaining-prep.req_mm_increment; 359 | if (minimum_mm < 0) { minimum_mm = 0; } 360 | 361 | do { 362 | switch (prep.ramp_type) { 363 | case RAMP_DECEL_OVERRIDE: 364 | speed_var = pl_block->acceleration*time_var; 365 | if (prep.current_speed-prep.maximum_speed <= speed_var) { 366 | mm_remaining = prep.accelerate_until; 367 | time_var = 2*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed); 368 | prep.ramp_type = RAMP_CRUISE; 369 | prep.current_speed = prep.maximum_speed; 370 | } else { 371 | mm_remaining -= time_var*(prep.current_speed - .5*speed_var); 372 | prep.current_speed -= speed_var; } 373 | break; 374 | case RAMP_ACCEL: 375 | speed_var = pl_block->acceleration*time_var; 376 | mm_remaining -= time_var*(prep.current_speed + .5*speed_var); 377 | if (mm_remaining < prep.accelerate_until) { 378 | mm_remaining = prep.accelerate_until; 379 | time_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed); 380 | if (mm_remaining == prep.decelerate_after) { prep.ramp_type = RAMP_DECEL; } 381 | else { prep.ramp_type = RAMP_CRUISE; } 382 | prep.current_speed = prep.maximum_speed; 383 | } else { 384 | prep.current_speed += speed_var; } 385 | break; 386 | case RAMP_CRUISE: 387 | mm_var = mm_remaining - prep.maximum_speed*time_var; 388 | if (mm_var < prep.decelerate_after) { 389 | time_var = (mm_remaining - prep.decelerate_after)/prep.maximum_speed; 390 | mm_remaining = prep.decelerate_after; 391 | prep.ramp_type = RAMP_DECEL; 392 | } else { 393 | mm_remaining = mm_var; } 394 | break; 395 | default: 396 | speed_var = pl_block->acceleration*time_var; 397 | if (prep.current_speed > speed_var) { 398 | mm_var = mm_remaining - time_var*(prep.current_speed - .5*speed_var); 399 | if (mm_var > prep.mm_complete) { 400 | mm_remaining = mm_var; 401 | prep.current_speed -= speed_var; 402 | break; } } 403 | time_var = 2*(mm_remaining-prep.mm_complete)/(prep.current_speed+prep.exit_speed); 404 | mm_remaining = prep.mm_complete; 405 | prep.current_speed = prep.exit_speed; } 406 | dt += time_var; 407 | if (dt < dt_max) { time_var = dt_max - dt; } 408 | else { 409 | if (mm_remaining > minimum_mm) { 410 | dt_max += DT_SEGMENT; 411 | time_var = dt_max - dt; 412 | } else { 413 | break; } } 414 | } while (mm_remaining > prep.mm_complete); 415 | 416 | if (sys.step_control & STEP_CONTROL_UPDATE_SPINDLE_PWM) { 417 | prep.current_spindle_pwm = pl_block->spindle_speed; 418 | bit_false(sys.step_control,STEP_CONTROL_UPDATE_SPINDLE_PWM); } 419 | prep_segment->spindle_pwm = prep.current_spindle_pwm; 420 | 421 | float step_dist_remaining = prep.step_per_mm*mm_remaining; 422 | float n_steps_remaining = ceil(step_dist_remaining); 423 | float last_n_steps_remaining = ceil(prep.steps_remaining); 424 | prep_segment->n_step = last_n_steps_remaining-n_steps_remaining; 425 | 426 | if (prep_segment->n_step == 0) { 427 | if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) { 428 | bit_true(sys.step_control,STEP_CONTROL_END_MOTION); 429 | return; } } 430 | 431 | dt += prep.dt_remainder; 432 | float inv_rate = dt/(last_n_steps_remaining - step_dist_remaining); 433 | uint32_t cycles = ceil((TICKS_PER_MICROSECOND*1000000*60)*inv_rate); 434 | 435 | #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING 436 | if (cycles < AMASS_LEVEL1) { prep_segment->amass_level = 0; } 437 | else { 438 | if (cycles < AMASS_LEVEL2) { prep_segment->amass_level = 1; } 439 | else if (cycles < AMASS_LEVEL3) { prep_segment->amass_level = 2; } 440 | else { prep_segment->amass_level = 3; } 441 | cycles >>= prep_segment->amass_level; 442 | prep_segment->n_step <<= prep_segment->amass_level; } 443 | if (cycles < (1UL << 16)) { prep_segment->cycles_per_tick = cycles; } 444 | else { prep_segment->cycles_per_tick = 0xffff; } 445 | #else 446 | if (cycles < (1UL << 16)) { 447 | prep_segment->prescaler = 1; 448 | prep_segment->cycles_per_tick = cycles; 449 | } else if (cycles < (1UL << 19)) { 450 | prep_segment->prescaler = 2; 451 | prep_segment->cycles_per_tick = cycles >> 3; 452 | } else { 453 | prep_segment->prescaler = 3; 454 | if (cycles < (1UL << 22)) { 455 | prep_segment->cycles_per_tick = cycles >> 6; 456 | } else { 457 | prep_segment->cycles_per_tick = 0xffff; } } 458 | #endif 459 | 460 | segment_buffer_head = segment_next_head; 461 | if (++segment_next_head == SEGMENT_BUFFER_SIZE) { segment_next_head = 0; } 462 | 463 | pl_block->millimeters = mm_remaining; 464 | prep.steps_remaining = n_steps_remaining; 465 | prep.dt_remainder = (n_steps_remaining - step_dist_remaining)*inv_rate; 466 | 467 | if (mm_remaining == prep.mm_complete) { 468 | if (mm_remaining > 0) { 469 | bit_true(sys.step_control,STEP_CONTROL_END_MOTION); 470 | return; 471 | } else { 472 | pl_block = NULL; 473 | plan_discard_current_block(); } } } } 474 | --------------------------------------------------------------------------------