Code:
#include<TeensyThreads.h>#include<errno.h>
//----------------------------------------------------------------------------
// message_32 defines
//----------------------------------------------------------------------------
#define MESSAGE_ERROR_DESC_LENGTH 32
#define MAX_XML_TAG 16 // 16 chars of XML tag (keep them short!!)
#define COMMAND_LINE_LENGTH 256 // maximum value of a line including terminator chars
#define UART_BUFFER_SIZE 1024 // max circular buffer
#define CHAR_COUNT_LOWER_THRESHOLD 256 //
#define CHAR_COUNT_UPPER_THRESHOLD 768 //
// gc_parser defines
#define MAX_ARGS 32
#define COMMAND_LENGTH 256
#define COMMENT_LENGTH 128
#define ERROR_DESC_LENGTH 64
// event_queue defines
#define MAX_EVENT_QUEUE 64 // up to 64 events in a queue
#define UPPER_EVENT_BUFFER_THRESHOLD 3* MAX_EVENT_QUEUE /4 // set 'full' above this threshold
#define LOWER_EVENT_BUFFER_THRESHOLD MAX_EVENT_QUEUE /4 // reset 'full' below this threshold
#define MAX_EVENT_PARAMS 3 // up to 3 integer parameters per event
#define MAX_EVENT_ID 16 // 16 chars in an event id
#define MAX_COEFFS 3
#define MAX_WORLDS 16
#define NAME_WIDTH 16
#define GC_PER_BLOCK 4
#define MC_PER_BLOCK 4
#define MESSAGE_ERROR_DESC_LENGTH 32
#define TRUE -1
#define FALSE 0
#define MAX_XML_TAG 16 // 16 chars of XML tag (keep them short!!)
//---------------------------------------------------------------------------------------
// cmd formats
#define GET_MOVE 10
#define GET_MODE_BITS 11
#define GET_PID 14
#define GET_ANALOG 15
#define GET_PORT 17
#define GET_ENCODER_POS 19
#define GET_ENCODER_CNT 20
//
#define GET_FLOW_CONTROL_STATE 0
#define GET_PING_REPLY 22
#define GET_CAL_FACTORS 25
#define GET_HASH 29
//#define GET_SPI_1 30
//#define GET_SPI_2 31
#define GET_I_VALUE 32
#define PING_SECRET 1234
//
#define GET_PROCESSOR_NAME 40 // The 'GET' prefix indicates a command that expects a reply (the host will wait up to 250mS)
#define GET_PROGRAM_VERSION 41
#define GET_MACHINE_NAME 42
#define GET_MACHINE_POSITION_XYZ 43
#define GET_MACHINE_POSITION_UVW 44
#define GET_MACHINE_POSITION_ABC 45
#define GET_MACHINE_STATE 46
#define GET_MACHINE_ERRORS 47
#define GET_GCODE_ERRORS 48
#define GET_MESSAGE_ERRORS 49
#define GET_UART_BUFFER_COUNT 50
#define GET_EVENT_QUEUE_COUNT 51
#define GET_UART_LINE_COUNT 52
#define GET_EVENT_QUEUE_ITEM 53
#define GET_EVENT_QUEUE_STATUS 54
#define GET_XML_ERRORS 55
#define GET_CONSOLE_ERRORS 56
#define GET_EVENT_QUEUE_NEXT_ITEM 57
#define GET_EXEC_CLEAR_EVENTS 58
#define GET_EVENT_QUEUE_NEXT 59
#define GET_SET_TIMER3_RATE 70
#define PUT_RESET_XYZ 60
#define PUT_RESET_ABC 61
#define PUT_RESET_UVW 62
//
#define GET_EXEC_GCODE 6000
//
#define PUT_MODE_CLEAR 5000 // The 'PUT' prefix indicates a command that doesn't expect a reply (the host won't wait becuse it doesn't expect a reply)
#define PUT_MODE_VERBOSE 5001
#define PUT_MODE_PAUSED 5002
#define PUT_MODE_DEBUG 5003
#define PUT_MODE_SINGLE_STEP 5004
#define PUT_MODE_LOG 5005
#define PUT_MODE_RUN 5006
#define PUT_MODE_STOP 5007
#define PUT_MODE_SYSTEM_HALT 5008
#define PUT_MODE_GLOBAL_OVERRIDE 5009
#define PUT_MODE_TEST 5010
#define PUT_CLEAR_GCODE_ERRORS 5011
#define PUT_CLEAR_MACHINE_ERRORS 5012
#define PUT_CLEAR_MESSAGE_ERRORS 5013
#define PUT_TEST_FUNCTION 5014
#define PUT_SET_TARE 5015
#define PUT_RESET_TARE 5016
#define PUT_MACHINE_FUNCTION 5017
#define PUT_SYSTEM_RESET 5018
#define PUT_EVENT_QUEUE_ITEM 5019
#define PUT_EVENT_QUEUE_GCODE 5020
//------------------------------------------------------------------------------
// axis constants
//------------------------------------------------------------------------------
#define N_AXES 9
#define N_ORIGINS 32
#define N_TOOLS 24
#define NAME_WIDTH 16
//------------------------------------------------------------------------------
// step ratios for microstepping
//------------------------------------------------------------------------------
#define STEP_BASE 200 // native stepper resolution = 200 steps per rev (1.8 degrees per step)
#define STEP_400 400 // 400 steps per rev
#define STEP_800 800 // ...
#define STEP_1000 1000 // this probably plenty. (0.36 degrees per step)
#define STEP_1600 1600 // ...
#define STEP_2000 2000
#define STEP_3200 3200
#define STEP_4000 4000
#define STEP_5000 5000
#define STEP_6400 6400
#define STEP_8000 8000
#define STEP_10000 10000
#define STEP_12800 12800
#define STEP_20000 20000
#define STEP_25000 25000
//------------------------------------------------------------------------------
// lathe axis constants
//------------------------------------------------------------------------------
#define DEFAULT_AXIS_CLASS ac_null
#define DEFAULT_MACHINE_UNITS mu_millimeters
#define DEFAULT_STEPS_REV STEP_400
#define DEFAULT_LEAD_SCREW_PITCH 3.0
#define DEFAULT_GEAR_RATIO 7.2
//----------------------------------------------------------------------------
// structs and typedefs
//----------------------------------------------------------------------------
typedef union {
int bits;
struct
{
unsigned float_error : 1;
unsigned integer_error : 1;
unsigned parameter_error : 1;
unsigned unknown_xml_tag : 1;
unsigned unknown_value : 1;
unsigned parse_error : 1;
unsigned invalid_char : 1;
unsigned value_error : 1;
unsigned missing_xml_tag : 1;
unsigned missing_field : 1;
unsigned unexpected_field : 1;
unsigned unknown_xml_cmd : 1;
unsigned missing_xml_msg : 1;
unsigned missing_xml_cmd : 1;
unsigned missing_xml_data : 1;
unsigned missing_xml_hash : 1;
unsigned hash_mismatch : 1;
};
} T_xml_errors; // xml parser/format errors
typedef union {
int bits;
struct
{
unsigned overrun : 1;
unsigned parity : 1;
unsigned idle : 1;
unsigned noise : 1;
unsigned line_too_long : 1;
};
} T_console_errors; // console (UART) errors
typedef union {
int bits;
struct
{
unsigned missing_block : 1;
unsigned block_too_long : 1;
unsigned unknown_gc_token : 1;
unsigned bad_float_format : 1;
unsigned bad_integer_format : 1;
unsigned too_many_arguments : 1;
unsigned missing_comment_brace : 1;
};
} T_gc_parse_errors;
//------------------------------------------------------------------------------
// parser context for gcode.
//------------------------------------------------------------------------------
typedef struct
{
T_gc_parse_errors gc_errors; // error flags
char last_error_desc[64]; // last error
char comments[COMMENT_LENGTH]; // last comment
int argc; // number of gcode tokens in gcode block.
int g; // count of G fields
int m; // count of M fields
char *args[MAX_ARGS]; // list of pointers to gcodes and values in gc_codeparams[].
char gc_codeblock[COMMAND_LENGTH]; // incomimg gcode command line.
char gc_codestrings[COMMAND_LENGTH]; // cleaned up and parsed gcode fields
char gc_codeparams[COMMAND_LENGTH]; // full list of all commands and parameters in a block
} T_gcparser;
// enumerated list of event class(es).
typedef enum { ec_null,
ec_move_axis,
ec_select_tool,
ec_adjust_parameter } T_event_class;
// enumerated list of machine event(s)
typedef enum { me_null,
me_move,
me_traverse,
me_select_tool,
me_test,
me_set_pid,
me_get_data,
me_exec_gcode } T_machine_events;
// enumerated list of possible machine function mode(s)
typedef enum { fm_null,
fm_paused,
fm_single_step,
fm_jog,
fm_manual,
fm_wait_for_tool } T_function_mode;
//
typedef union {
int bits;
struct
{
unsigned a_changed : 1;
unsigned b_changed : 1;
unsigned c_changed : 1;
unsigned d_changed : 1;
unsigned e_changed : 1;
unsigned f_changed : 1;
unsigned g_changed : 1;
unsigned h_changed : 1;
unsigned i_changed : 1;
unsigned j_changed : 1;
unsigned k_changed : 1;
unsigned l_changed : 1;
unsigned m_changed : 1;
unsigned n_changed : 1;
unsigned o_changed : 1;
unsigned p_changed : 1;
unsigned q_changed : 1;
unsigned r_changed : 1;
unsigned s_changed : 1;
unsigned t_changed : 1;
unsigned u_changed : 1;
unsigned v_changed : 1;
unsigned w_changed : 1;
unsigned x_changed : 1;
unsigned y_changed : 1;
unsigned z_changed : 1;
};
} T_field_flags;
//------------------------------------------------------------------------------
// An 'event' is something synchronous that may be initiated by the system.
// Typically, 'something' we want the controller to do in an ordered sequential way.
// example: move an axis, attach a tool, adjust a parameter etc.
//------------------------------------------------------------------------------
typedef struct
{
int line_number; //
int gcode_param[GC_PER_BLOCK]; // active gcodes per block
int mcode_param[MC_PER_BLOCK]; // active mcodes per block
int tool_select; // new tool
float feed_rate; //
float speed; //
int dwell_time; //
float radius; //
int coord_index; // relative coordinates index
int op_mode; // new mode
float xyz[MAX_EVENT_PARAMS]; // 'x,y,z' stored here.
float abc[MAX_EVENT_PARAMS]; // 'a,b,c' stored here.
float uvw[MAX_EVENT_PARAMS]; // 'u,v,w' stored here.
T_field_flags field_flags;
} T_event;
//------------------------------------------------------------------------------
// booleans for setting the state of an 'event_queue'
//------------------------------------------------------------------------------
typedef union {
int bits;
struct
{
unsigned busy : 1; // 'busy' means an event is underway and we have to wait until the event is ended.
unsigned paused : 1; // 'paused' means we are temporarily halting the whole process.
unsigned fault : 1; // 'fault' means the system has struck a significnt event that needs operator intervention.
unsigned waiting : 1; // 'waiting' for user input.(active prompt)
unsigned ready : 1; // 'ready' indicates all dependant fuctions have finished and ready for the next event (eg x,y,z axes - this is an AND function)
unsigned full : 1; // 'full' indicates that the queue_buffer is full - don't post any new event sequences.
};
} T_queue_state;
//------------------------------------------------------------------------------
// scale and offsets used for each move.
//------------------------------------------------------------------------------
typedef struct
{
float scale; // y = mx + c - integer step calculation, plus offset
float offset;
} T_scalar_coeffs; // these are used to calculate the number of steps required to move a calibrated distance (mm or inches)
//------------------------------------------------------------------------------
// 'T_event_queue' maintains a list of the events in an array buffer, say, 32 events long. (must be a power of 2)
// 'head' points to the last event pushed (buffered & ready for processing, - it's the last event to be extracted from the queue).
// 'tail' points to the first event pushed (and will be the first event in order, extracted from the queue).
// 'count' calculates the number of events left in the queue.
// 'busy' determines if an event is being run currently.
// 'full' stops new events being posted to the queue.
// How it works:
// This structure uses a virtual method table concept plus an array[] of events, set into global memory.
// This format enables functions (methods) with a dot '.' operator syntax.
// Examples:
// 1) event_queue.add(an_event); // copies an event structure into an array of event structures.
// 2) an_event = event_queue.next(); // retrieves a pointer to an event from the next one in the buffered array.
// 3) event_count = event_queue.count(); // retrieves the current number of events in the queue.
//------------------------------------------------------------------------------
typedef struct
{
T_event events[MAX_EVENT_QUEUE]; // circular buffer of events in an array. (must be a power of 2)
T_function_mode function_mode; // e.g. fm_paused, fm_single_step, fm_jog etc
T_queue_state state; // 'busy', 'paused', 'halted', 'full' etc.
T_scalar_coeffs scalar_coeffs[MAX_COEFFS]; // array of coordinates used.
int total; //
int head; // 'head is incremented and the next event is posted to the head of the buffer.
int tail; // 'tail' is where the next event is taken from in the circular buffer.
T_event *(*next)(void); // 'next' gets the next event from the list if one is available. returns 'null' otherwise.
void (*add)(T_event *e); // 'add' appends a new event to the buffer unless buffer is (near)ly full.
int (*count)(void); // 'count' calculates and returns the number of events in the (circular) buffer.
} T_event_queue;
//------------------------------------------------------------------------------
// enumerated type of the kinds of axes we can have and how they are measured.
// ac_null no effect
// ac dist calibrated in distance values; mm or inch
// ac_speed calibrated in speed - RPM
// ac_angle calibrated in angles. (degrees or radians)
//------------------------------------------------------------------------------
typedef enum { ac_null,
ac_dist,
ac_speed,
ac_angle } T_axis_class;
//------------------------------------------------------------------------------
typedef enum { mu_null,
mu_millimeters,
mu_inches } T_machine_units;
//------------------------------------------------------------------------------
typedef enum { mc_null,
mc_program_stop,
mc_optional_stop,
mc_spindle_on,
mc_spindle_off,
mc_tool_change,
mc_end_program } T_machine_m_codes;
//------------------------------------------------------------------------------
typedef struct
{
char name[NAME_WIDTH];
float pt[3]; // three floats that can be aliased as xyz, abc, uvw.
} T_origin_pt;
//------------------------------------------------------------------------------
typedef union {
int bits;
struct
{
unsigned use_ramp : 1; //
unsigned modal : 1; //
unsigned abs_mode : 1; //
unsigned use_tool_comp : 1; //
unsigned units_type : 2; //
unsigned changed : 1; // whether the machine has changed any parameter.
unsigned enabled : 1; // global enabled for the machine to run.
};
} T_machine_state;
//------------------------------------------------------------------------------
typedef struct
{
float ramp_steps[3];
float ramp_gradient[3];
float ramp_distance[3];
float upper_threshold[3];
float lower_threshold[3];
} T_ramp_envelope;
//------------------------------------------------------------------------------
typedef struct
{
float leadscrew_pitch; // typically 3.0mm ballscrew
float gear_ratio; // typically 7.2:1 on the hercus lathe (timing belt ratio)
float steps_rev; // typically 1000 (base of 200 steps per rev)
float distance_perstep; // mm per step / inches per step.
float offset_comp; //
float scale_comp; //
} T_axis_cal; //
//------------------------------------------------------------------------------
// A 'T_axis' is a collection of structured data sent to each axis over SPI_1.
typedef struct
{
char name[NAME_WIDTH]; //
T_axis_class axis_class; // type of axis ac_null, ac_dist, ac_speed, ac_angle, used to interpret T_axis data
T_axis_cal axis_cal; // how to calibrate this axis (using distance, speed, angle)
float current_position; // position measurement units
int current_step; // position in 'steps'
float move_speed; //
float traverse_speed; //
float acceleration_factor; //
} T_axis; //
//------------------------------------------------------------------------------
typedef struct
{
float radius;
float height;
float xyz_offset[3];
float xyz_comp[3];
} T_tool_params;
//------------------------------------------------------------------------------
typedef struct
{
char name[NAME_WIDTH];
T_tool_params params;
} T_tool;
//------------------------------------------------------------------------------
typedef struct
{
char name[NAME_WIDTH]; //
T_machine_state state; // current machine state with boolean and bit fields
T_ramp_envelope ramp_envelope; // how to describe a ramp speed envelope
T_axis axes[N_AXES]; // axis parameters
T_tool tools[N_TOOLS]; // array of tools (with tool parameters)
T_origin_pt origins[N_ORIGINS]; // array of relative origins - (offsets)
int gcode; // current G code
int mcode; // current M code
int tool_index; //
int origin_index; //
int dwell_time; //
float spindle_speed; //
float surface_speed; //
float move_rate; //
float traverse_rate; //
float feed_rate; //
float xyz_position[3]; // 3d 'xyz' position
float xyz_target[3]; //
float abc_position[3]; // 3d 'abc' position
float abc_target[3]; //
float uvw_position[3]; // 3d 'uvw' position
float uvw_target[3]; //
} T_machine; // the defintion of the 'machine' with axes and a host of variables
//------------------------------------------------------------------------------
// instances of typedef-ed types
//------------------------------------------------------------------------------
T_console_errors console_errors;
T_xml_errors xml_errors;
T_gcparser gc_parser;
T_event_queue event_queue;
T_machine machine;
//--------------------------------------------------------------------------------
// module variables for message handling
//--------------------------------------------------------------------------------
int rx_ctr; // keep track of the length of a message.
unsigned char rx_buffer[UART_BUFFER_SIZE]; // circular buffer, usually 1024 or 2048 chars (power of 2)
unsigned char rx_char; // latest character read from the uart
int buffer_head = 0; // pointer to last charcter of the message (usually the null terminator).
int buffer_tail = 0; // pointer to last charcter of the message (usually the null terminator).
int line_count = 0; // count of 'received lines' - the number of null terminated messages in circular buffer
int buffer_count = 0; // chars in circular buffer.
//------------------------------------------------------------------------------
// local module variables
//------------------------------------------------------------------------------
int LED = 13; // pin 13 is the orange LED
char block[256];
char cmdln[256];
//------------------------------------------------------------------------------
// kick-off code
//------------------------------------------------------------------------------
void setup() {
pinMode(LED, OUTPUT);
Serial.begin(115200);
Serial1.begin(115200, SERIAL_8N1);
Serial1.setRX(0);
Serial1.setTX(1);
Serial1.attachRts(2);
event_queue_init(); // setup an event queue
machine_init(); // initliase the machine with 3 axes.
}
//------------------------------------------------------------------------------
// main loop()
//------------------------------------------------------------------------------
void loop() {
serial1Event(); // check for the arrival of any chars
if (line_count) { // if we have a new line of chars terminated by CRLF
buff_read(cmdln); // read them out of the cirular buffer into a null terminated linear char array
parse_command(cmdln); // parse this into commands data and hash
blip_led(100); // this emulates a time delay/wait on the machine function (100us)
}
}
//--------------------------------------------------------------------------------
// take the block of data in the xml markup and see what to do with it
//--------------------------------------------------------------------------------
char cmd[32];
char data[256];
char hash[32];
void parse_command(char *commandline) {
int command;
strcpy_xml_string("cmd", commandline, cmd);
strcpy_xml_string("data", commandline, data);
strcpy_xml_string("hash", commandline, hash);
command = atol(cmd);
switch (command) {
case GET_EXEC_GCODE:
{
unpack_gcode_block(data);
}
break;
case GET_PING_REPLY:
{
host_reply(GET_PING_REPLY, "1234");
}
break;
case GET_MACHINE_POSITION_XYZ:
{
host_reply(GET_MACHINE_POSITION_XYZ, "<xyz>1.01,2.03,4.04</xyz>");
}
break;
}
}
//------------------------------------------------------------------------------
// gen_hash() generates the 'unique' hash of an argument string.
// Note:
// This is a very simple integrity check and, over time, may have collisions (duplicates) or repeats, if the string is long enough.
// However, each hash is 'unique' to the current buffer and, very likely, any others in close time proximity.
// It indicates the integrity of a particular message using a 16 bit binary number (65536 different values).
//------------------------------------------------------------------------------
unsigned int gen_hash(const char *cp) {
unsigned long int uhash = 5381; // seed the hash - although we use a 32 bit integer, we are only interested in the right most 16 bits
while (*cp) {
uhash = 33 * uhash ^ (unsigned char)*cp++; // accumulate hash value of the string (this algorithm was found online and is plagiarised here).
uhash &= 0x0000ffff; // limit result to 16 bits
}
return (uhash); // return the 'hash value' of the argument string '*cp' as the low 16 bits of a 32 bit integer.
}
//------------------------------------------------------------------------------
// host_reply() appropriately formats a reply to the hosting PC.
// Two argument fields are supplied.
// a) <cmd>3067</cmd> an integer value. 'cmd' mirrors the sent command constant, to which we are replying.
// b) <data>345452</data>, a string value. 'data' is the replied data value as a string.
// The xml tag 'data' may contain a parseable string with separators. Example: <data>f1,f2,f3,f4</data>
// This procedure concatenates a 'hash' value as an itegrity check. Example: <hash>30778</hash>
//------------------------------------------------------------------------------
char msgbuff[384]; // these are global variables. If there are too many large local variables, we risk
char hashbuff[64]; // running out of stack cpace. i.e. no 'malloc()', just statically allocated globals.
void host_reply(int cmd, const char *data) {
sprintf(msgbuff, "<msg><cmd>%d</cmd><data>%s</data></msg>", cmd, data); // format the reply message
sprintf(hashbuff, "<hash>%d</hash>", gen_hash(msgbuff)); // take its 'hash'
strcat(msgbuff, hashbuff); // append the hash value to the message
Serial1.println(msgbuff); // output the message with a 'LF + CR' pair
}
//------------------------------------------------------------------------------
// checking and reading the serial event data attached to serial1
//------------------------------------------------------------------------------
void serial1Event() {
char rx_char;
while (Serial1.available()) {
rx_char = (char)Serial1.read();
if (isgraph(rx_char) || (rx_char == 32) || (rx_char == 10)) // throw away 'funny' characters i.e. only allow printable chars or a new-line.
{ //
rx_buffer[buffer_head++] = rx_char; // store rx_char into the circular buffer and increment the buffer head
buffer_head &= (UART_BUFFER_SIZE - 1); // perform 'rollover' arithmetic on the buffer head index
if (++rx_ctr > COMMAND_LINE_LENGTH - 4) console_errors.line_too_long = 1; // note if the command length is longer than 254 chars
if (rx_char == '\n') // is the rx_char a new line? end of frame....
{ //
buffer_head--; // prepare to overwrite the LF char with a null.
buffer_head &= (UART_BUFFER_SIZE - 1); // perform circular 'rollover' arithemetic on the buffer_head index
rx_buffer[buffer_head] = '\0'; // overwrite the LF char with a null
rx_ctr = 0; // zero the rx_ctr ( the received string length)
line_count++; // signal that there is another command in the buffer
} //
buffer_count = input_buffer_charcount(); // input to manage buffer handshake
// if (buffer_count > CHAR_COUNT_UPPER_THRESHOLD) RTS_OFF; // disable communication if buffer is "near full".
}
}
}
//------------------------------------------------------------------------------
// input_buffer_charcount()
// Returns the number of characters in the circular receive buffer.
//------------------------------------------------------------------------------
unsigned int input_buffer_charcount(void) {
unsigned int char_count;
if (buffer_head >= buffer_tail) char_count = buffer_head - buffer_tail;
else char_count = (UART_BUFFER_SIZE - buffer_tail) + buffer_head;
return (char_count);
}
//------------------------------------------------------------------------------
// buff_read()....
// :Copies the source strs from the input circular buffer to the target linear buffer (the *s argument).
// :Expects 'null' terminated strings in the rx_buffer circular buffer.
// :Then cycles around the buffer to copy out a string until a null is found.
// :Also returns a pointer to the start of the source string.
//------------------------------------------------------------------------------
char *buff_read(char *s) {
register char *c = s; // make a local copy of the target pointer.
while (rx_buffer[buffer_tail]) // examine the the string until 'null' is found
{ // register pointer 'c' points to the far string.
*c++ = rx_buffer[buffer_tail++]; //
buffer_tail &= (UART_BUFFER_SIZE - 1); // RULE: for this to work, UART_BUFFER_SIZE must be an integral power of 2
} // eg, 256, 512, 1024 etc.otherwise you need to do some pointer arithmetic
*c = '\0'; // Null terminate the copied string just in case.
if (line_count) line_count--; // if we have a non zero line_count, decrement the count.
buffer_count = input_buffer_charcount();
// if (buffer_count < CHAR_COUNT_LOWER_THRESHOLD) RTS_ON;
return (s); // return the pointer to the string, as per standard str() functions
}
//------------------------------------------------------------------------------
// blip_led()
// :holds the led on pin 13 'ON' for 'duration' microseconds.
//------------------------------------------------------------------------------
void blip_led(int duration) {
digitalWrite(LED, HIGH);
delayMicroseconds(duration);
digitalWrite(LED, LOW);
}
//------------------------------------------------------------------------------
// convert_string()
// : copies a C++ string into a normal char[] array and null terminates it.
//------------------------------------------------------------------------------
void convert_string(String inp, char *target) {
int i;
int len = inp.length();
for (i = 0; i < len; i++) {
*target++ = inp[i];
}
*target = '\0';
}
//------------------------------------------------------------------------------
// Non invasive version of 'get_xml_string()'
// strcpy_xml_string copies a null terminated substring that exists between the outer xml tags
// e.g. // from "<xmltag>this string will be returned</xmltag>"" it returns "this string will be returned" as a null terminated string.
// This function does not affect the source string.
// The tag or token, can be a string of any printable alphabetical ascii no more than 16 characters long.
// If the "<tag>"" doesn't exist or is not bounded by a complementary "</tag>", the function returns a '\0' delimiter to *tgt pointer of the target string
// TO DO: Limit the max number of chars allowrd to be moved (like strncpy).
//------------------------------------------------------------------------------
void strcpy_xml_string(const char *tag, char *src, char *tgt) {
char start_tag[MAX_XML_TAG + 2]; // local storage
char end_tag[MAX_XML_TAG + 3]; // for tag braces "<>" and "</>" plus the tag string itself
char *st; // pointer to start tag <>
char *et; // pointer to end tag </>
char *xml_body_start; // pointer to start of xml body
char *xml_body_end; // pointer to end of xml body
st = start_tag; // assign pointers to buffers
et = end_tag; //
strcpy(st, "<"); // construct both the start and end tags
strcat(st, tag);
strcat(st, ">");
strcpy(et, "</");
strcat(et, tag);
strcat(et, ">");
if ((strstr(src, st) != NULL) && (strstr(src, et) != NULL)) // verify that both tags are present in the arg *src string.
{
xml_body_start = strstr(src, st) + strlen(st); // point to xml body start.
xml_body_end = strstr(xml_body_start, et); // point to xml body end.
while (xml_body_start != xml_body_end) // perform a 'strcpy' on the partial substring
{
*tgt++ = *xml_body_start++;
}
}
*tgt = '\0'; // always set NULL at the end. If we fail to find a string, it sets the first element of 'tgt[0]' to NULL.
}
//------------------------------------------------------------------------------
// GCode parser section.
//------------------------------------------------------------------------------
void unpack_gcode_block(char *s) {
int i;
T_event event;
strncpy(gc_parser.gc_codeblock, s, 256);
extract_gcode_comments(gc_parser.gc_codeblock, gc_parser.comments);
strip_white_space(gc_parser.gc_codestrings, gc_parser.gc_codeblock);
extract_gcode_commands(&gc_parser);
clear_event(&event);
gc_parser.gc_errors.bits = 0;
gc_parser.g = 0; // G command count set to 0. Note that a maximum of 4 G values can be set in one gcode block.
gc_parser.m = 0; // M command count set to 0. Note that a maximum of 4 M values can be set in one gcode block.
strcpy(gc_parser.last_error_desc, "\0"); // put a null at the first element of the error description
for (i = 0; i < gc_parser.argc; i++) {
eval_parameter_string(gc_parser.args[i], &event); // build event parameters by rippling through the gcode block
}
if (gc_parser.gc_errors.bits == 0) event_queue.add(&event); // if there are no errors, add the event to the event_queue
}
//------------------------------------------------------------------------------
// Evaluates each parameter in a GCode block
//------------------------------------------------------------------------------
void eval_parameter_string(char *s, T_event *evt) {
char *c;
c = s;
while (*c) {
if (isalpha(*c)) {
switch (*c) {
case 'A':
{
evt->abc[0] = safe_float(c + 1);
evt->field_flags.a_changed = 1;
}
break;
case 'B':
{
evt->abc[1] = safe_float(c + 1);
evt->field_flags.b_changed = 1;
}
break;
case 'C':
{
evt->abc[2] = safe_float(c + 1);
evt->field_flags.c_changed = 1;
}
break;
case 'D':
{
evt->field_flags.d_changed = 1;
}
break;
case 'E':
{
evt->field_flags.e_changed = 1;
}
break;
case 'F':
{
evt->feed_rate = safe_float(c + 1);
evt->field_flags.f_changed = 1;
}
break;
case 'G':
{
evt->gcode_param[gc_parser.g++] = safe_int(c + 1);
evt->field_flags.g_changed = 1;
}
break;
case 'H':
{
evt->field_flags.h_changed = 1;
}
break;
case 'I':
{
evt->field_flags.i_changed = 1;
}
break;
case 'J':
{
evt->field_flags.j_changed = 1;
}
break;
case 'K':
{
evt->field_flags.k_changed = 1;
}
break;
case 'L':
{
evt->field_flags.l_changed = 1;
}
break;
case 'M':
{
evt->mcode_param[gc_parser.m++] = safe_int(c + 1);
evt->field_flags.m_changed = 1;
}
break;
case 'N':
{
evt->line_number = safe_int(c + 1);
evt->field_flags.n_changed = 1;
}
break;
case 'O':
{
evt->field_flags.o_changed = 1;
}
break;
case 'P':
{
evt->dwell_time = safe_int(c + 1);
evt->field_flags.p_changed = 1;
}
break;
case 'Q':
{
evt->field_flags.q_changed = 1;
}
break;
case 'R':
{
evt->radius = safe_float(c + 1);
evt->field_flags.r_changed = 1;
}
break;
case 'S':
{
evt->speed = safe_float(c + 1);
evt->field_flags.s_changed = 1;
}
break;
case 'T':
{
evt->tool_select = safe_int(c + 1);
evt->field_flags.t_changed = 1;
}
break;
case 'U':
{
evt->uvw[0] = safe_float(c + 1);
evt->field_flags.u_changed = 1;
}
break;
case 'V':
{
evt->uvw[1] = safe_float(c + 1);
evt->field_flags.v_changed = 1;
}
break;
case 'W':
{
evt->uvw[2] = safe_float(c + 1);
evt->field_flags.w_changed = 1;
}
break;
case 'X':
{
evt->xyz[0] = safe_float(c + 1);
evt->field_flags.x_changed = 1;
}
break;
case 'Y':
{
evt->xyz[1] = safe_float(c + 1);
evt->field_flags.y_changed = 1;
}
break;
case 'Z':
{
evt->xyz[2] = safe_float(c + 1);
evt->field_flags.z_changed = 1;
}
break;
default:
{
gc_parser.gc_errors.unknown_gc_token = 1;
gc_strerror();
}; // unknown gc token
}
}
c++;
}
}
//------------------------------------------------------------------------------
// Comment signifiers "%" or "//" - all chars to the right of the token are treated as a comment
// ...% This is a comment.
// ...// This is a comment.
// comment specifier "(" and ")" the field in between () is a comment
// ...(This is a comment).
// comment specifier "{" and "}" the field in between {} is a comment
// ...{This is a comment}.
//------------------------------------------------------------------------------
int extract_gcode_comments(char *source, char *target) {
char *s;
char *t;
char *n;
char *a;
char *b;
s = source; // make a local copy of the source pointer
t = target; // make a local copy of the target pointer
a = strstr(s, "//"); // test to see if the '//' token symbol is present in the source
if (a) {
n = a;
strncpy(t, a + 2, COMMENT_LENGTH); // copy the comment contents out of the source string. Skip the comment symbol "%" or "//"
*(n) = '\0'; // overwrite the / symbol with a null (0) in the source string (*n points to the first elementof the token in the source string)
return (-1);
}
a = strstr(s, "%"); // test to see if the '%' token symbol is present in the source
if (a) {
n = a; // make a copy of the pointer to the comment symbol
strncpy(t, a + 1, COMMENT_LENGTH); // copy the comment contents out of the source string. Skip the comment symbol "%" or "//"
*(n) = '\0'; // overwrite the % symbol with a null (0) in the source string (*n points to the first elementof the token in the source string)
return (-1);
}
a = strstr(s, "("); // test to see if the '(' token symbol is present in the source
if (a) {
b = strstr(s, ")"); // test to see if ')' is present
if (b) {
*(b) = '\0'; // replace ')' with '\0' (null) to allow comment copy within (------)
strncpy(t, a + 1, COMMENT_LENGTH);
*(a) = '\0'; // replace '(' with '\0' (null) to truncate comment out of source string.
return (-1);
}
}
a = strstr(s, "{"); // test to see if the '{' token symbol is present in the source
if (a) {
b = strstr(s, "}"); // test to see if '}' is present
if (b) {
*(b) = '\0'; // replace '}' with '\0' (null) to allow comment copy within {------}
strncpy(t, a + 1, COMMENT_LENGTH);
*(a) = '\0'; // replace '{' with '\0' (null) to truncate comment out of source string.
return (-1);
}
}
return (0);
}
//------------------------------------------------------------------------------
// A GCode block starts as a null terminated string and consists of a list of
// letters (A..Z) as commands, with numeric value arguments associated with each letter.
// As an example "X4.2". Here the variable 'X' has a value of '4.2' in a decimal format.
// This function copies a source string into target string and inserts null
// terminators after each parameter (just befor the next alphabetic character).
// Gcode parameters are now in the form of shorter strings "X10.0" "M00" "G01" "Z34.9"
// Each alphabetic value and associated argument value have a separator of 'null' terminating them.
// Summary:
// In "extract_gcode_commands()" the source GCode block is transformed into an array
// of (sub)strings constructed from the original supplied string.
// Because of the way the GC block is now formatted, parsing each block element is simplified
// by examining the first character, then selecting the right command based on the alphabetic
// character, incrementing the char pointer to point to the argument, then converting the argument value.
// Most arrguments are in decimal format, however the G, M, T, F values are integers.
//------------------------------------------------------------------------------
int extract_gcode_commands(T_gcparser *gcp) {
int f, i;
char *t;
char *s;
s = gcp->gc_codestrings; // a block of gcode
t = gcp->gc_codeparams; // an array of strings, each a gcode command within the block
i = 0;
f = 0;
while (*s) // start iterating through source string
{
if (isalpha(*s)) // if we find an alpha char
{ // insert a null before it to terminate a preceeding field.
if (f) *t++ = 0; // (but only after the first time through...)
f = 1; // flip the first time flag...
gcp->args[i++] = t; // add each pointer in an array[] (pointers to strings)
}
*t++ = toupper(*s++); // otherwise (by default) copy source char into dest; (also in uppercase).
}
*(t) = '\0'; // make sure to null terminate the last field.
gcp->argc = i; // report the number of parameter fields in the block
return (0);
}
//------------------------------------------------------------------------------
// "strip_white_space"
// Excludes controls chars or white space from source string, as it copies source to target.
// Why? Because we need to have each command and its arguments running together with no gaps or separators.
// e.g. 'X 1.2 G 01 M 6' becomes 'X1.2G01M6'
//------------------------------------------------------------------------------
void strip_white_space(char *target_string, char *source_string) {
char *s;
char *t;
s = source_string;
t = target_string;
if (strlen(s) == 0) *t = '\0';
while (*s) {
while (isvalidchar(*s) == 0) s++; // exclude spaces or other specific chars
*t++ = *s++;
}
*t = '\0';
}
//------------------------------------------------------------------------------
int isvalidchar(char c) // is the argument char in a list of valid chars?
{
if (isspace(c)) return (false);
else return (true);
}
//------------------------------------------------------------------------------
void gc_strerror() {
if (gc_parser.gc_errors.missing_block) { snprintf(gc_parser.last_error_desc, ERROR_DESC_LENGTH, "missing gc block on line %s", gc_parser.args[0]); }
if (gc_parser.gc_errors.block_too_long) { snprintf(gc_parser.last_error_desc, ERROR_DESC_LENGTH, "block too long on line %s", gc_parser.args[0]); }
if (gc_parser.gc_errors.unknown_gc_token) { snprintf(gc_parser.last_error_desc, ERROR_DESC_LENGTH, "unknown gc token on line %s", gc_parser.args[0]); }
if (gc_parser.gc_errors.bad_float_format) { snprintf(gc_parser.last_error_desc, ERROR_DESC_LENGTH, "bad float format on line %s", gc_parser.args[0]); }
if (gc_parser.gc_errors.bad_integer_format) { snprintf(gc_parser.last_error_desc, ERROR_DESC_LENGTH, "bad integer format on line %s", gc_parser.args[0]); }
if (gc_parser.gc_errors.too_many_arguments) { snprintf(gc_parser.last_error_desc, ERROR_DESC_LENGTH, "too many arguments on line %s", gc_parser.args[0]); }
if (gc_parser.gc_errors.missing_comment_brace) { snprintf(gc_parser.last_error_desc, ERROR_DESC_LENGTH, "missing comment brace on line %s", gc_parser.args[0]); }
}
//------------------------------------------------------------------------------
// converts string to a integer value with error detection.
//------------------------------------------------------------------------------
int safe_int(char *c) {
char *end;
int v = 0;
v = (int)strtol(c, &end, 10);
if (*end != '\0' || errno != 0) {
gc_parser.gc_errors.bad_integer_format = 1;
gc_strerror(); // convert error to string
errno = 0;
}
return (v);
}
//------------------------------------------------------------------------------
// converts string to a float value with error detection.
//------------------------------------------------------------------------------
float safe_float(char *c) {
char *end;
float f;
f = (float)strtod(c, &end);
if (*end != '\0' || errno != 0) {
gc_parser.gc_errors.bad_float_format = 1;
gc_strerror(); // convert gcode error to string.
errno = 0;
}
return (f);
}
//------------------------------------------------------------------------------
// Tests to see if there are characters in the string that would support a float format.
//------------------------------------------------------------------------------
int isfloatstr(char *s) {
register char *c;
c = s;
while (*c) {
if ((isdigit(*c)) || (*c == '.') || (*c == '+') || (*c == '-') || (*c == ' ') || (*c == 'E') || (*c == 'e')) c++;
else return (false);
}
return (true);
}
//------------------------------------------------------------------------------
void event_queue_init(void) {
int i;
event_queue.state.busy = 0;
event_queue.state.paused = 0;
event_queue.state.fault = 0;
event_queue.state.full = 0;
event_queue.head = 0;
event_queue.tail = 0;
event_queue.total = 0;
event_queue.function_mode = fm_null;
event_queue.count = &get_event_count;
event_queue.add = &add_event;
event_queue.next = &get_next;
for (i = 0; i < MAX_EVENT_QUEUE; i++) {
clear_event(&event_queue.events[i]);
}
}
//------------------------------------------------------------------------------
void clear_event(T_event *evt) {
int k;
evt->line_number = -1;
evt->gcode_param[0] = -1;
evt->gcode_param[1] = -1;
evt->gcode_param[2] = -1;
evt->gcode_param[3] = -1;
evt->mcode_param[0] = -1;
evt->mcode_param[1] = -1;
evt->mcode_param[2] = -1;
evt->mcode_param[3] = -1;
evt->coord_index = -1;
evt->dwell_time = 0;
evt->feed_rate = 0.0;
evt->speed = 0.0;
evt->op_mode = -1;
evt->tool_select = -1;
evt->field_flags.bits = 0; // clear all the bits asociated with fields passed in a gcode block
for (k = 0; k < MAX_EVENT_PARAMS; k++) {
evt->xyz[k] = 0.0;
evt->abc[k] = 0.0;
evt->uvw[k] = 0.0;
}
}
//------------------------------------------------------------------------------
// *get_next(); method returns a pointer to the next available 'T_event' in 'event_queue.events[]' buffer.
// At reset, tail = 0 is used to return the first 'next'.
// After incrementing the tail pointer we wrap the array boundary by anding it with (MAX_EVENT_QUEUE-1).
// MAX_EVENT_QUEUE must be a power of 2 for this to work. 8,16,32,64,128 etc.
// Note: currently it's 64 (1/06/2022)
// Syntax: newevent = event_queue.next();
// Side Effect: Resets the 'full' flag of the event queue if the event count is below a lower threshold.
// i.e. the buffer WILL accept new events.
//------------------------------------------------------------------------------
T_event *get_next(void) {
T_event *p;
if (event_queue.count() > 0) {
p = &event_queue.events[event_queue.tail];
event_queue.tail++;
event_queue.tail &= (MAX_EVENT_QUEUE - 1);
if (event_queue.count() < LOWER_EVENT_BUFFER_THRESHOLD) event_queue.state.full = 0;
} else p = NULL;
return (p);
}
//------------------------------------------------------------------------------
void copy_event_to_machine(T_event *evt) {
}
//------------------------------------------------------------------------------
void copy_next_event_to_machine(void) {
T_event *e;
if ((event_queue.state.paused == 0) && (event_queue.state.busy == 0) && (event_queue.state.fault == 0)) // copy the next event to the machine if 'paused' is false
{
e = event_queue.next(); // if next() is skipped, the buffer keeps the last event in the queue
if (e != NULL) // if there is an event in the queue, event 'e' will not be NULL
{
if (e->field_flags.x_changed) machine.xyz_position[0] = e->xyz[0]; // only update the values that have changed or are modal.
if (e->field_flags.y_changed) machine.xyz_position[1] = e->xyz[1];
if (e->field_flags.z_changed) machine.xyz_position[2] = e->xyz[2];
machine.state.changed = 1;
}
}
}
//------------------------------------------------------------------------------
// pushes a T_event onto the event_queue circular buffer.
// no returned value.
// Intially the head pointer will be 0.
// The first push will be to event_queue.events[0].
// After setting up the event content at [0], the head pointer is incremented to 1 ready for the
// next store operation.
// The head pointer wraps around the array boundary by anding with (MAX_EVENT_QUEUE-1).
// MAX_EVENT_QUEUE must be a power of 2 for this to work. 8,16,32,64,128 etc.
// syntax: event_queue.add(T_event *new_event);
// side effect: Sets the 'full' flag of the event queue if the event_queue.count() is above an upper threshold.
// i.e. the buffer will NOT accept new events.
//------------------------------------------------------------------------------
void add_event(T_event *evt) {
int i;
event_queue.events[event_queue.head].field_flags.bits = evt->field_flags.bits;
event_queue.events[event_queue.head].line_number = evt->line_number;
event_queue.events[event_queue.head].gcode_param[0] = evt->gcode_param[0];
event_queue.events[event_queue.head].gcode_param[1] = evt->gcode_param[1];
event_queue.events[event_queue.head].gcode_param[2] = evt->gcode_param[2];
event_queue.events[event_queue.head].gcode_param[3] = evt->gcode_param[3];
event_queue.events[event_queue.head].mcode_param[0] = evt->mcode_param[0];
event_queue.events[event_queue.head].mcode_param[1] = evt->mcode_param[1];
event_queue.events[event_queue.head].mcode_param[2] = evt->mcode_param[2];
event_queue.events[event_queue.head].mcode_param[3] = evt->mcode_param[3];
event_queue.events[event_queue.head].tool_select = evt->tool_select;
event_queue.events[event_queue.head].feed_rate = evt->feed_rate;
event_queue.events[event_queue.head].speed = evt->speed;
event_queue.events[event_queue.head].radius = evt->radius;
event_queue.events[event_queue.head].dwell_time = evt->dwell_time;
event_queue.events[event_queue.head].coord_index = evt->coord_index;
for (i = 0; i < MAX_EVENT_PARAMS; i++) {
event_queue.events[event_queue.head].xyz[i] = evt->xyz[i];
event_queue.events[event_queue.head].abc[i] = evt->abc[i];
event_queue.events[event_queue.head].uvw[i] = evt->uvw[i];
}
event_queue.head++;
event_queue.head &= (MAX_EVENT_QUEUE - 1);
if (event_queue.count() > UPPER_EVENT_BUFFER_THRESHOLD) event_queue.state.full = 1;
}
//------------------------------------------------------------------------------
// get_event_count calculates the number of events in the circular queue buffer.
// side effects: none
// syntax: some_value = event_queue.count();
//------------------------------------------------------------------------------
int get_event_count(void) {
unsigned int event_count;
if (event_queue.head >= event_queue.tail) event_count = event_queue.head - event_queue.tail;
else event_count = (MAX_EVENT_QUEUE - event_queue.tail) + event_queue.head;
return (event_count);
}
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void machine_init(void) {
int i;
machine.state.changed = 0;
machine.state.enabled = 0;
for (i = 0; i < N_AXES; i++) {
sprintf(machine.axes[i].name, "axis#%d", i);
machine.axes[i].axis_cal.offset_comp = 0.0;
machine.axes[i].axis_cal.scale_comp = 1.0;
machine.axes[i].axis_cal.gear_ratio = DEFAULT_GEAR_RATIO;
machine.axes[i].axis_cal.leadscrew_pitch = DEFAULT_LEAD_SCREW_PITCH;
machine.axes[i].axis_cal.steps_rev = DEFAULT_STEPS_REV;
machine.axes[i].axis_class = DEFAULT_AXIS_CLASS;
machine.axes[i].axis_cal.distance_perstep = 0.0;
machine.axes[i].acceleration_factor = 1.0;
}
for (i = 0; i < N_AXES; i++) {
// machine.ramp_envelope.lower_threshold[i] = 0.0; // each axis has an associated ramp calculation structure
// machine.ramp_envelope.upper_threshold[i] = 0.0;
// machine.ramp_envelope.ramp_distance[i] = 0.0;
// machine.ramp_envelope.ramp_gradient[i] = 0.0;
// machine.ramp_envelope.ramp_steps[i] = 0.0;
}
for (i = 0; i < N_TOOLS; i++) // initialize multiple tools
{
sprintf(machine.tools[i].name, "tool#%d", i);
machine.tools[i].params.xyz_comp[0] = 0.0;
machine.tools[i].params.xyz_comp[1] = 0.0;
machine.tools[i].params.xyz_comp[2] = 0.0;
machine.tools[i].params.xyz_offset[0] = 0.0;
machine.tools[i].params.xyz_offset[1] = 0.0;
machine.tools[i].params.xyz_offset[2] = 0.0;
machine.tools[i].params.radius = 0.0;
machine.tools[i].params.height = 0.0;
}
// initialize multiple origins.
// These are 'offsets' relative to the absolute coordinates of each set of axes
// They will give relative coordinates in each case.
for (i = 0; i < N_ORIGINS; i++) // initialize multiple origins. thse are offsets relative to the absolute coordinates
{
sprintf(machine.origins[i].name, "org#%d", i);
machine.origins[i].pt[0] = 0.0;
machine.origins[i].pt[1] = 0.0;
machine.origins[i].pt[2] = 0.0;
}
machine.move_rate = 0.0; // initialize all the operating parameters of the machine.
machine.traverse_rate = 0.0;
machine.feed_rate = 0.0;
machine.spindle_speed = 0.0;
machine.surface_speed = 0.0;
machine.tool_index = 0;
machine.origin_index = 0;
machine.state.use_ramp = 0;
machine.state.modal = 0;
machine.state.abs_mode = 0;
machine.state.use_tool_comp = 0;
machine.state.abs_mode = 0;
machine.state.units_type = DEFAULT_MACHINE_UNITS;
machine.xyz_position[0] = 0.0; // initialize all the absolute positions of each axis
machine.xyz_position[1] = 0.0;
machine.xyz_position[2] = 0.0;
machine.xyz_target[0] = 0.0;
machine.xyz_target[1] = 0.0;
machine.xyz_target[2] = 0.0;
machine.abc_position[0] = 0.0;
machine.abc_position[1] = 0.0;
machine.abc_position[2] = 0.0;
machine.abc_target[0] = 0.0;
machine.abc_target[1] = 0.0;
machine.abc_target[2] = 0.0;
machine.uvw_position[0] = 0.0;
machine.uvw_position[1] = 0.0;
machine.uvw_position[2] = 0.0;
machine.uvw_target[0] = 0.0;
machine.uvw_target[1] = 0.0;
machine.uvw_target[2] = 0.0;
}
//------------------------------------------------------------------------------
Reply With Quote
Originally Posted by BriComp
