kopia lustrzana https://gitlab.com/markol/Teathimble_Firmware
353 wiersze
11 KiB
C
353 wiersze
11 KiB
C
#include "queue.h"
|
|
#include "motor.h"
|
|
#include "pinio.h"
|
|
#include "homing.h"
|
|
#include "serial.h"
|
|
#include "gcode_parser.h"
|
|
#include "sensors_control.h"
|
|
|
|
GCODE_PARAM BSS gcode_params[8];
|
|
static volatile uint8_t current_parameter = 0;
|
|
uint8_t option_all_relative = 0;
|
|
int16_t parameter_0 = -1;
|
|
int32_t parameter_1 = -1, parameter_2 = -1;
|
|
|
|
TARGET BSS next_target;
|
|
|
|
// Parser is implemented as a finite state automata (DFA)
|
|
// This is pointer holds function with actions expected for current progress, each of these functions
|
|
// represent one possible state
|
|
uint8_t (*parser_current_state)(uint8_t c);
|
|
|
|
/// convert a floating point input value into an integer with appropriate scaling.
|
|
/// \param mantissa the actual digits of our floating point number
|
|
/// \param exponent scale mantissa by \f$10^{-exponent}\f$
|
|
/// \param sign positive or negative?
|
|
/// \param multiplicand multiply by this amount during conversion to integer
|
|
///
|
|
/// Tested for up to 42'000 mm (accurate), 420'000 mm (precision 10 um) and
|
|
/// 4'200'000 mm (precision 100 um).
|
|
static int32_t decfloat_to_int(uint32_t mantissa, uint8_t exponent, uint8_t sign, uint16_t multiplicand) {
|
|
|
|
// exponent=1 means we've seen a decimal point but no digits after it, and e=2 means we've seen a decimal point with one digit so it's too high by one if not zero
|
|
if (exponent)
|
|
exponent--;
|
|
|
|
// This raises range for mm by factor 1000 and for inches by factor 100.
|
|
// It's a bit expensive, but we should have the time while parsing.
|
|
while (exponent && multiplicand % 10 == 0) {
|
|
multiplicand /= 10;
|
|
exponent--;
|
|
}
|
|
|
|
mantissa *= multiplicand;
|
|
if (exponent)
|
|
mantissa = (mantissa + powers[exponent] / 2) / powers[exponent];
|
|
|
|
return sign ? -(int32_t)mantissa : (int32_t)mantissa;
|
|
}
|
|
|
|
void parser_reset()
|
|
{
|
|
uint8_t i;
|
|
parser_current_state = start_parsing_parameter;
|
|
current_parameter = 0;
|
|
for(i = 0; i < 8; ++i)
|
|
{
|
|
gcode_params[i].name = 0;
|
|
gcode_params[i].value = 0;
|
|
gcode_params[i].exponent = 0;
|
|
gcode_params[i].is_negative = 0;
|
|
}
|
|
}
|
|
|
|
void parser_init()
|
|
{
|
|
#ifdef STEPS_PER_M_Z
|
|
next_target.F = SEARCH_FEEDRATE_Z;
|
|
#else
|
|
next_target.F = SEARCH_FEEDRATE_Y;
|
|
#endif
|
|
#ifdef STEPS_PER_M_E
|
|
next_target.e_multiplier = 256;
|
|
#endif
|
|
next_target.f_multiplier = 256;
|
|
parser_reset();
|
|
}
|
|
|
|
// This function executes all possible commands after those are parsed
|
|
// Tinker with this to add new commands
|
|
uint8_t process_command()
|
|
{
|
|
DDA *dda;
|
|
uint8_t result = 0;
|
|
if (option_all_relative)
|
|
next_target.axis[X] = next_target.axis[Y] = 0;
|
|
for(int i = 1; i <= current_parameter; ++i)
|
|
{
|
|
switch(gcode_params[i].name)
|
|
{
|
|
case 'X':
|
|
next_target.axis[X] = decfloat_to_int(gcode_params[i].value, gcode_params[i].exponent, gcode_params[i].is_negative, 1000);
|
|
break;
|
|
case 'Y':
|
|
next_target.axis[Y] = decfloat_to_int(gcode_params[i].value, gcode_params[i].exponent, gcode_params[i].is_negative, 1000);
|
|
break;
|
|
case 'F':
|
|
next_target.F = decfloat_to_int(gcode_params[i].value, gcode_params[i].exponent, gcode_params[i].is_negative, 1);
|
|
break;
|
|
case 'S':
|
|
parameter_0 = decfloat_to_int(gcode_params[i].value, gcode_params[i].exponent, gcode_params[i].is_negative, 1);
|
|
break;
|
|
case 'P':
|
|
parameter_1 = decfloat_to_int(gcode_params[i].value, gcode_params[i].exponent, gcode_params[i].is_negative, 1);
|
|
break;
|
|
case 'I':
|
|
parameter_2 = decfloat_to_int(gcode_params[i].value, gcode_params[i].exponent, gcode_params[i].is_negative, 1);
|
|
break;
|
|
}
|
|
}
|
|
// convert relative to absolute
|
|
if (option_all_relative) {
|
|
next_target.axis[X] += startpoint.axis[X];
|
|
next_target.axis[Y] += startpoint.axis[Y];
|
|
}
|
|
// gcode_params[0] is always a operation with code
|
|
switch(gcode_params[0].name)
|
|
{
|
|
case 'G':
|
|
switch(gcode_params[0].value)
|
|
{
|
|
case 0:
|
|
//? Example: G0
|
|
//?
|
|
//? Linear move
|
|
enqueue(&next_target); break;
|
|
case 1:
|
|
//? Example: G1
|
|
//?
|
|
//? Linear move with tool down
|
|
enqueue_home(&next_target, 0, 0xf0); break;
|
|
case 28:
|
|
//? Example: G28
|
|
//?
|
|
//? home all axis
|
|
queue_wait(); // wait for queue to empty
|
|
home();
|
|
break;
|
|
case 90:
|
|
//? Example: G90
|
|
//?
|
|
//? Absolute positioning
|
|
option_all_relative = 0;
|
|
break;
|
|
case 91:
|
|
//? Example: G91
|
|
//?
|
|
//? Relative positioning
|
|
option_all_relative = 1;
|
|
break;
|
|
default:
|
|
result = STATE_ERROR;
|
|
}
|
|
break;
|
|
case 'M':
|
|
switch(gcode_params[0].value)
|
|
{
|
|
case 0:
|
|
//? Example: M0
|
|
//?
|
|
//? Stop or unconditional stop
|
|
desired_speed = 0;
|
|
ATOMIC_START
|
|
dda = queue_current_movement();
|
|
if (dda != NULL)
|
|
{
|
|
update_current_position();
|
|
memcpy(&startpoint, ¤t_position, sizeof(TARGET));
|
|
dda->live = 0; dda->done = 1;
|
|
#ifdef LOOKAHEAD
|
|
dda->id--;
|
|
#endif
|
|
queue_flush();
|
|
queue_step() ;
|
|
dda_new_startpoint();
|
|
}
|
|
ATOMIC_END
|
|
break;
|
|
|
|
case 112:
|
|
//? Example: M112
|
|
//?
|
|
//? Any moves in progress are immediately terminated, then the controller
|
|
//? shuts down. All motors are turned off. Only way to
|
|
//? restart is to press the reset button on the master microcontroller.
|
|
//? See also M0.
|
|
//?
|
|
desired_speed = 0;
|
|
timer_stop();
|
|
queue_flush();
|
|
cli();
|
|
|
|
break;
|
|
|
|
case 114:
|
|
//? Example: M114
|
|
//?
|
|
//? Get current pos
|
|
update_current_position();
|
|
sersendf_P(PSTR("X:%lq,Y:%lq,F:%lu\n"),
|
|
current_position.axis[X], current_position.axis[Y],
|
|
current_position.F);
|
|
break;
|
|
|
|
case 119:
|
|
//? Example: M119
|
|
//?
|
|
//? Endstops status
|
|
//power_on();
|
|
endstops_on();
|
|
delay_us(1000); // allow the signal to stabilize
|
|
#if defined(X_MIN_PIN)
|
|
sersendf_P(PSTR("X:%d "), x_min());
|
|
#endif
|
|
#if defined(Y_MIN_PIN)
|
|
sersendf_P(PSTR("Y:%d"), y_min());
|
|
#endif
|
|
serial_writestr_P(PSTR("\n"));
|
|
endstops_off();
|
|
break;
|
|
|
|
case 202:
|
|
//set acceleration not supported
|
|
break;
|
|
case 222:
|
|
//? Example: M222 S400
|
|
//?
|
|
//? Set dc motor max speed
|
|
set_dc_motor_speed_margin(parameter_0);
|
|
parameter_0 = -1;
|
|
break;
|
|
case 301:
|
|
//? Example: M301 S0 P40000 I2000
|
|
//?
|
|
//? Set dc motor kP and kI controller parameters, S is a speed
|
|
if(parameter_0 >= 0)
|
|
desired_speed = parameter_0;
|
|
if(parameter_1 >= 0)
|
|
kp = parameter_1;
|
|
if(parameter_2 >= 0)
|
|
ki = parameter_2;
|
|
sersendf_P(PSTR("S:%d,P:%ld,I:%ld\n"),
|
|
desired_speed, kp, ki);
|
|
break;
|
|
default:
|
|
result = STATE_ERROR;
|
|
}
|
|
break;
|
|
default:
|
|
result = STATE_ERROR;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
|
|
uint8_t gcode_syntax_error(uint8_t c)
|
|
{
|
|
return STATE_ERROR;
|
|
}
|
|
|
|
uint8_t start_parsing_parameter(uint8_t c)
|
|
{
|
|
//ignore
|
|
if IS_WHITECHAR(c)
|
|
return 0;
|
|
|
|
// uppercase
|
|
if (c >= 'a' && c <= 'z')
|
|
c &= ~32;
|
|
|
|
if IS_LETTER(c)
|
|
{
|
|
gcode_params[current_parameter].name = c;
|
|
parser_current_state = start_parsing_number;
|
|
return 0;
|
|
}
|
|
parser_current_state = gcode_syntax_error;
|
|
return STATE_ERROR;
|
|
}
|
|
|
|
uint8_t parse_digit(uint8_t c)
|
|
{
|
|
//process digit
|
|
if IS_DIGIT(c)
|
|
{
|
|
// this is simply mantissa = (mantissa * 10) + atoi(c) in different clothes
|
|
gcode_params[current_parameter].value = (gcode_params[current_parameter].value << 3) +
|
|
(gcode_params[current_parameter].value << 1) + ATOI(c);
|
|
|
|
if(gcode_params[current_parameter].exponent)
|
|
++gcode_params[current_parameter].exponent;
|
|
return 0;
|
|
}
|
|
|
|
//this digit is a end of parameter
|
|
if IS_WHITECHAR(c)
|
|
{
|
|
parser_current_state = start_parsing_parameter;
|
|
++current_parameter;
|
|
return 0;
|
|
}
|
|
// all done, this digit is a end of instruction
|
|
if(c == '\n' || c == '\r') {
|
|
//process instruction
|
|
c = process_command();
|
|
parser_reset();
|
|
return c;
|
|
}
|
|
if(c == '.')
|
|
{
|
|
gcode_params[current_parameter].exponent = 1;
|
|
return 0;
|
|
}
|
|
|
|
parser_current_state = gcode_syntax_error;
|
|
return STATE_ERROR;
|
|
}
|
|
|
|
uint8_t start_parsing_number(uint8_t c)
|
|
{
|
|
parser_current_state = parse_digit;
|
|
//negative number
|
|
if(c == '-')
|
|
{
|
|
gcode_params[current_parameter].is_negative = 1;
|
|
return 0;
|
|
}
|
|
if IS_DIGIT(c)
|
|
{
|
|
parse_digit(c);
|
|
return 0;
|
|
}
|
|
parser_current_state = gcode_syntax_error;
|
|
return STATE_ERROR;
|
|
}
|
|
|
|
// parsing of new instruction starts from HERE
|
|
uint8_t gcode_parse_char(uint8_t c) {
|
|
|
|
uint8_t result, checksum_char = c;
|
|
result = parser_current_state(c);
|
|
|
|
if IS_ENDING(c)
|
|
{
|
|
if ( result == STATE_ERROR) //error
|
|
{
|
|
parser_reset();
|
|
return 2;
|
|
}
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|