void write_data(){
struct Lnode *actual_node = NULL;
struct Lnode *next_node = NULL;
struct Lnode *ptr = NULL;
int i, *Outputs;
uint32_t value;
clear_vector(inputs_vector_state, 14, 0);
clear_vector(written, 4, 0);
inputs_vector_state[7] = 1;
inputs_vector_values[7] = 11;
store_count = 0;
store_values_r3[0] = 0;
store_values_r3[1] = SH_HEX;
store_values_r3[2] = BIN_HEX;
store_values_r4[0] = key_instructions.r_w_addr + 8;
store_values_r4[1] = key_instructions.r_w_addr + 4;
store_values_r4[2] = key_instructions.r_w_addr;
for(actual_node = payload->tail; actual_node != NULL; actual_node = actual_node->prev){
next_node = actual_node->prev;
if(next_node == NULL) break;
for(ptr = (GETPOINTER(next_node, payload_gadget_t))->gadget;
ptr != NULL && ptr->next != NULL; ptr = ptr->next); //'ret'
Outputs = (GETPOINTER(ptr, Gadget_t))->Outputs;
if(Outputs[0] && !written[0]){
written[0] = 1;
inputs_vector_state[0] = 1;
inputs_vector_values[0] = key_instructions.r_w_addr;
}
if(Outputs[1] && !written[1]){
written[1] = 1;
inputs_vector_state[1] = 1;
inputs_vector_values[1] = key_instructions.r_w_addr + 8;
}
if(Outputs[2] && !written[2]){
written[2] = 1;
inputs_vector_state[2] = 1;
inputs_vector_values[2] = 0;
}
for(i = 14; i >= 0; i--){
if(Outputs[i]){
value = getValue(i);
actual_node = insert_word(value, actual_node);
}
}
if(Outputs[7] && !written[3]){
written[3] = 1;
inputs_vector_values[7] = WORD_PADDING;
}
process_inputs(GETPOINTER(next_node, payload_gadget_t)->gadget);
}
}
void plan_init()
{
block_buffer_head = 0;
block_buffer_tail = 0;
plan_set_acceleration_manager_enabled(TRUE);
clear_vector(position);
}
// Initialize and start the stepper motor subsystem
void stepper_init() {
// Configure directions of interface pins
STEPPING_DDR |= (STEPPING_MASK | DIRECTION_MASK);
STEPPING_PORT = (STEPPING_PORT & ~(STEPPING_MASK | DIRECTION_MASK)) | INVERT_MASK;
// waveform generation = 0100 = CTC
TCCR1B &= ~(1<<WGM13);
TCCR1B |= (1<<WGM12);
TCCR1A &= ~(1<<WGM11);
TCCR1A &= ~(1<<WGM10);
// output mode = 00 (disconnected)
TCCR1A &= ~(3<<COM1A0);
TCCR1A &= ~(3<<COM1B0);
// Configure Timer 2
TCCR2A = 0; // Normal operation
TCCR2B = 0; // Disable timer until needed.
TIMSK2 |= (1<<TOIE2); // Enable Timer2 interrupt flag
adjust_speed(MINIMUM_STEPS_PER_MINUTE);
clear_vector(stepper_position);
stepper_set_position( CONFIG_X_ORIGIN_OFFSET,
CONFIG_Y_ORIGIN_OFFSET,
CONFIG_Z_ORIGIN_OFFSET );
acceleration_tick_counter = 0;
current_block = NULL;
stop_requested = false;
stop_status = STATUS_OK;
busy = false;
// start in the idle state
// The stepper interrupt gets started when blocks are being added.
stepper_go_idle();
}
EXPORT_SYM enum sl_error
sl_clear_flat_map(struct sl_flat_map* map)
{
if(!map)
return SL_INVALID_ARGUMENT;
SL(clear_flat_set(map->key_set));
SL(clear_vector(map->data_list));
return SL_NO_ERROR;
}
void clear_point_struct(point_struct *Pt)
/***************************************************************\
* USAGE: clear the memory *
\***************************************************************/
{ // clear the memory
clear_vector(Pt->origX);
clear_vector(Pt->x);
clear_vector(Pt->Nx);
mpf_clear(Pt->norm_x);
clear_number(Pt->origAlpha);
clear_number(Pt->origBeta);
clear_number(Pt->origGamma);
clear_number(Pt->alpha);
clear_number(Pt->beta);
clear_number(Pt->gamma);
Pt->isApproxSoln = Pt->isActive = Pt->isReal = 0;
return;
}
void plan_init() {
block_buffer_head = 0;
block_buffer_tail = 0;
plan_set_acceleration_manager_enabled(true);
clear_vector(position);
clear_vector_double(previous_unit_vec);
previous_nominal_speed = 0.0;
memset (&startpoint, 0, sizeof(startpoint));
}
double *set_vector_by_axis(double value, uint8_t axis)
{
clear_vector(vector);
switch (axis) {
case (X): vector[X] = value; break;
case (Y): vector[Y] = value; break;
case (Z): vector[Z] = value; break;
case (A): vector[A] = value; break;
case (B): vector[B] = value; break;
case (C): vector[C] = value;
}
return (vector);
}
float *set_vector_by_axis(float value, uint8_t axis)
{
clear_vector(vector);
switch (axis) {
case (AXIS_X): vector[AXIS_X] = value; break;
case (AXIS_Y): vector[AXIS_Y] = value; break;
case (AXIS_Z): vector[AXIS_Z] = value; break;
case (AXIS_A): vector[AXIS_A] = value; break;
case (AXIS_B): vector[AXIS_B] = value; break;
case (AXIS_C): vector[AXIS_C] = value;
}
return (vector);
}
void planner_init() {
block_buffer_head = 0;
block_buffer_tail = 0;
block_buffer_tail_write = 0;
raster_buffer_next = 0;
raster_buffer_count = 0;
clear_vector(position);
planner_set_position( CONFIG_X_ORIGIN_OFFSET,
CONFIG_Y_ORIGIN_OFFSET,
CONFIG_Z_ORIGIN_OFFSET );
position_update_requested = false;
clear_vector_double(previous_unit_vec);
previous_nominal_speed = 0.0;
}
void resolve_dependences(){
struct Lnode *payload_node = NULL;
payload_gadget_t *payload_node_struct = NULL;
/* Local state of pending inputs (previous iteration) */
unsigned char inputs_vector_state_LOCAL[14];
unsigned char looping = 1;
int i;
while(looping){
copy_vector(inputs_vector_state_LOCAL, inputs_vector_state, 14);
clear_vector(inputs_vector_state, 14, 0);
inputs_vector_state[7] = 1;
inputs_vector_values[7] = 11;
inputs_vector_refs[7] = 0;
for(payload_node = payload->tail; payload_node != NULL; payload_node = payload_node->prev){
payload_node_struct = GETPOINTER(payload_node, payload_gadget_t);
process_outputs(payload_node_struct->gadget, payload_node);
process_inputs(payload_node_struct->gadget);
for(i = 0; i < 14; i++){
if(inputs_vector_state[i] && inputs_vector_state_LOCAL[i]){
// Input not provided //
if(payload_node != payload->head || !store_writes_r(i)){
// Fix it
if(check_my_outputs(i)){
// Auxiliar gadget (which writes into i) writes also into another pending input //
inputs_vector_refs[i] = payload_node;
}
else{
payload_node = write_auxiliar_gadget(i, payload_node);
}
}
}
}
}
looping = 0;
for(i = 0; i < 14; i++){
if(inputs_vector_state[i] && !store_writes_r(i)){
write_auxiliar_gadget(i, payload_node);
looping = 1;
}
}
}
}
uint8_t cm_straight_probe(float target[], bool flags[])
{
// trap zero feed rate condition
if (fp_ZERO(cm.gm.feed_rate)) {
return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED);
}
// error if no axes specified
if (!flags[AXIS_X] && !flags[AXIS_Y] && !flags[AXIS_Z]) {
return (STAT_GCODE_AXIS_IS_MISSING);
}
// set probe move endpoint
copy_vector(pb.target, target); // set probe move endpoint
copy_vector(pb.flags, flags); // set axes involved on the move
clear_vector(cm.probe_results); // clear the old probe position.
// NOTE: relying on probe_result will not detect a probe to 0,0,0.
cm.probe_state = PROBE_WAITING; // wait until planner queue empties before completing initialization
pb.func = _probing_init; // bind probing initialization function
return (STAT_OK);
}
void MainWindow::load() {
clear_vector();
#ifdef Q_OS_ANDROID
QString savedfile = QString(STORAGE_FILE);
#else
QString savedfile = QString(getenv("HOME")) + QString("/") + QString(STORAGE_FILE);
#endif
QFile file(savedfile);
file.open(QIODevice::ReadOnly | QIODevice::Truncate);
union conv record;
while (file.read(record.data,sizeof(PackedPerson))==sizeof(PackedPerson)) {
PersonWidget *p = new PersonWidget();
p->setPacked(record.person);
people_vector.push_back(p);
ui->frame->layout()->addWidget(p);
QObject::connect(p, &PersonWidget::changed, this, &MainWindow::store);
}
file.close();
}
bool TextureAtlas::create( GLuint* ahandle, int& width, int& height )
{
if( internalTextures.size() < 1 ){
Debug::debug( Debug::GRAPHICS, "Textures is missing.\n" );
return false;
}
// Sort textures in buffer
sort( internalTextures.begin(), internalTextures.end(), compareTextureProxies );
// Calculate texture positions and draw to texture
if( !buildMap( width, height ) ){
Debug::debug( Debug::GRAPHICS, "Cannot build atlas map.\n" );
return false;
}
if( !build( ahandle, width, height ) ){
Debug::debug( Debug::GRAPHICS, "Cannot draw atlas.\n" );
return false;
}
// Push textures to render and clear array
RenderManager::PushTextures( internalTextures, *ahandle );
clear_vector( &internalTextures );
return true;
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace). Comments and block delete
// characters have been removed.
uint8_t gc_execute_line(char *line) {
uint8_t char_counter = 0;
char letter;
double value;
double unit_converted_value;
double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
uint8_t radius_mode = false;
uint8_t absolute_override = false; /* 1 = absolute motion for this block only {G53} */
uint8_t next_action = NEXT_ACTION_DEFAULT; /* The action that will be taken by the parsed line */
double target[3], offset[3];
double p = 0, r = 0;
int int_value;
gc.status_code = STATUS_OK;
// Pass 1: Commands
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
switch(letter) {
case 'G':
switch(int_value) {
case 0: gc.motion_mode = MOTION_MODE_SEEK; break;
case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
case 4: next_action = NEXT_ACTION_DWELL; break;
case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break;
case 53: absolute_override = true; break;
case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
case 90: gc.absolute_mode = true; break;
case 91: gc.absolute_mode = false; break;
case 92: next_action = NEXT_ACTION_SET_COORDINATE_OFFSET; break;
case 93: gc.inverse_feed_rate_mode = true; break;
case 94: gc.inverse_feed_rate_mode = false; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'M':
switch(int_value) {
case 0: case 1: gc.program_flow = PROGRAM_FLOW_PAUSED; break;
case 2: case 30: case 60: gc.program_flow = PROGRAM_FLOW_COMPLETED; break;
case 3: gc.laser_enable = 1; break;
case 4: gc.laser_enable = 1; break;
case 5: gc.laser_enable = 0; break;
case 112: next_action = NEXT_ACTION_CANCEL; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'T': gc.tool = trunc(value); break;
}
if(gc.status_code) { break; }
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
char_counter = 0;
clear_vector(target);
clear_vector(offset);
memcpy(target, gc.position, sizeof(target)); // i.e. target = gc.position
// Pass 2: Parameters
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
unit_converted_value = to_millimeters(value);
switch(letter) {
case 'F':
if (unit_converted_value <= 0) { FAIL(STATUS_BAD_NUMBER_FORMAT); } // Must be greater than zero
if (gc.inverse_feed_rate_mode) {
inverse_feed_rate = unit_converted_value; // seconds per motion for this motion only
} else {
if (gc.motion_mode == MOTION_MODE_SEEK) {
gc.seek_rate = unit_converted_value;
} else {
gc.feed_rate = unit_converted_value; // millimeters per minute
}
}
break;
case 'I': case 'J': case 'K': offset[letter-'I'] = unit_converted_value; break;
case 'P': p = value; break;
case 'R': r = unit_converted_value; radius_mode = true; break;
case 'S': gc.nominal_laser_intensity = value; break;
case 'X': case 'Y': case 'Z':
if (gc.absolute_mode || absolute_override) {
target[letter - 'X'] = unit_converted_value;
} else {
target[letter - 'X'] += unit_converted_value;
}
break;
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
// Perform any physical actions
switch (next_action) {
case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(gc.position); break;
case NEXT_ACTION_DWELL: mc_dwell(p); break;
case NEXT_ACTION_SET_COORDINATE_OFFSET:
mc_set_current_position(target[X_AXIS], target[Y_AXIS], target[Z_AXIS]);
break;
case NEXT_ACTION_CANCEL:
//mc_emergency_stop();
// captain, we have a new target!
//st_get_position(&gc.position[X_AXIS], &gc.position[Y_AXIS], &gc.position[Z_AXIS]);
//mc_set_current_position(gc.position[X_AXIS], gc.position[Y_AXIS], gc.position[Z_AXIS]);
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, false, LASER_OFF);
//return; // totally bail
break;
case NEXT_ACTION_DEFAULT:
switch (gc.motion_mode) {
case MOTION_MODE_CANCEL: break;
case MOTION_MODE_SEEK:
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, false, LASER_OFF);
break;
case MOTION_MODE_LINEAR:
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
gc.nominal_laser_intensity);
break;
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
if (radius_mode) {
/*
We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == magnitude of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
d^2 == x^2 + y^2
h^2 == r^2 - (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
O <- [i,j]
- |
r - |
- |
- | h
- |
[0,0] -> C -----------------+--------------- T <- [x,y]
| <------ d/2 ---->|
C - Current position
T - Target position
O - center of circle that pass through both C and T
d - distance from C to T
r - designated radius
h - distance from center of CT to O
Expanding the equations:
d -> sqrt(x^2 + y^2)
h -> sqrt(4 * r^2 - x^2 - y^2)/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Which can be written:
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Which we for size and speed reasons optimize to:
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2
*/
// Calculate the change in position along each selected axis
double x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
double y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
clear_vector(offset);
double h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == -(h * 2 / d)
// If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any
// real CNC, and thus - for practical reasons - we will terminate promptly:
if(isnan(h_x2_div_d)) { FAIL(STATUS_FLOATING_POINT_ERROR); return(gc.status_code); }
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
the left hand circle will be generated - when it is negative the right hand circle is generated.
T <-- Target position
^
Clockwise circles with this center | Clockwise circles with this center will have
will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
\ | /
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
|
|
C <-- Current position */
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
if (r < 0) {
h_x2_div_d = -h_x2_div_d;
r = -r; // Finished with r. Set to positive for mc_arc
}
// Complete the operation by calculating the actual center of the arc
offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
} else { // Offset mode specific computations
r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
}
// Set clockwise/counter-clockwise sign for mc_arc computations
uint8_t isclockwise = false;
if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
// Trace the arc
mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
r, isclockwise, gc.nominal_laser_intensity);
break;
}
}
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
memcpy(gc.position, target, sizeof(double)*3); // gc.position[] = target[];
return(gc.status_code);
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace).
uint8_t gc_execute_line(char *line) {
uint8_t char_counter = 0;
char letter;
double value;
double unit_converted_value;
double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
int radius_mode = false;
uint8_t absolute_override = false; /* 1 = absolute motion for this block only {G53} */
uint8_t next_action = NEXT_ACTION_DEFAULT; /* The action that will be taken by the parsed line */
double target[3], offset[3];
double p = 0, r = 0;
int int_value;
clear_vector(target);
clear_vector(offset);
gc.status_code = STATUS_OK;
// Disregard comments and block delete
if (line[0] == '(') { return(gc.status_code); }
if (line[0] == '/') { char_counter++; } // ignore block delete
// Pass 1: Commands
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
switch(letter) {
case 'G':
switch(int_value) {
case 0: gc.motion_mode = MOTION_MODE_SEEK; break;
case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
#ifdef __AVR_ATmega328P__
case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
#endif
case 4: next_action = NEXT_ACTION_DWELL; break;
case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break;
case 53: absolute_override = true; break;
case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
case 90: gc.absolute_mode = true; break;
case 91: gc.absolute_mode = false; break;
case 92: next_action = NEXT_ACTION_SET_COORDINATE_OFFSET; break;
case 93: gc.inverse_feed_rate_mode = true; break;
case 94: gc.inverse_feed_rate_mode = false; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'M':
switch(int_value) {
case 0: case 1: gc.program_flow = PROGRAM_FLOW_PAUSED; break;
case 2: case 30: case 60: gc.program_flow = PROGRAM_FLOW_COMPLETED; break;
case 3: gc.spindle_direction = 1; break;
case 4: gc.spindle_direction = -1; break;
case 5: gc.spindle_direction = 0; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'T': gc.tool = trunc(value); break;
}
if(gc.status_code) { break; }
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
char_counter = 0;
clear_vector(offset);
memcpy(target, gc.position, sizeof(target)); // i.e. target = gc.position
// Pass 2: Parameters
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
unit_converted_value = to_millimeters(value);
switch(letter) {
case 'F':
if (gc.inverse_feed_rate_mode) {
inverse_feed_rate = unit_converted_value; // seconds per motion for this motion only
} else {
if (gc.motion_mode == MOTION_MODE_SEEK) {
gc.seek_rate = unit_converted_value/60;
} else {
gc.feed_rate = unit_converted_value/60; // millimeters pr second
}
}
break;
case 'I': case 'J': case 'K': offset[letter-'I'] = unit_converted_value; break;
case 'P': p = value; break;
case 'R': r = unit_converted_value; radius_mode = true; break;
case 'S': gc.spindle_speed = value; break;
case 'X': case 'Y': case 'Z':
if (gc.absolute_mode || absolute_override) {
target[letter - 'X'] = unit_converted_value;
} else {
target[letter - 'X'] += unit_converted_value;
}
break;
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
// Update spindle state
spindle_run(gc.spindle_direction, gc.spindle_speed);
// Perform any physical actions
switch (next_action) {
case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(gc.position); break; // Go home
case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break; // Dwell
case NEXT_ACTION_SET_COORDINATE_OFFSET: // Coordinate System offset
mc_set_current_position(target[X_AXIS], target[Y_AXIS], target[Z_AXIS]);
break;
case NEXT_ACTION_DEFAULT:
switch (gc.motion_mode) {
case MOTION_MODE_CANCEL: break;
case MOTION_MODE_SEEK:
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, false);
break;
case MOTION_MODE_LINEAR:
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode);
break;
#ifdef __AVR_ATmega328P__
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
if (radius_mode) {
/*
We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == magnitude of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
d^2 == x^2 + y^2
h^2 == r^2 - (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
O <- [i,j]
- |
r - |
- |
- | h
- |
[0,0] -> C -----------------+--------------- T <- [x,y]
| <------ d/2 ---->|
C - Current position
T - Target position
O - center of circle that pass through both C and T
d - distance from C to T
r - designated radius
h - distance from center of CT to O
Expanding the equations:
d -> sqrt(x^2 + y^2)
h -> sqrt(4 * r^2 - x^2 - y^2)/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Which can be written:
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Which we for size and speed reasons optimize to:
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2
*/
// Calculate the change in position along each selected axis
double x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
double y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
clear_vector(offset);
double h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == -(h * 2 / d)
// If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any
// real CNC, and thus - for practical reasons - we will terminate promptly:
if(isnan(h_x2_div_d)) { FAIL(STATUS_FLOATING_POINT_ERROR); return(gc.status_code); }
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
the left hand circle will be generated - when it is negative the right hand circle is generated.
T <-- Target position
^
Clockwise circles with this center | Clockwise circles with this center will have
will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
\ | /
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
|
|
C <-- Current position */
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
if (r < 0) { h_x2_div_d = -h_x2_div_d; }
// Complete the operation by calculating the actual center of the arc
offset[gc.plane_axis_0] = (x-(y*h_x2_div_d))/2;
offset[gc.plane_axis_1] = (y+(x*h_x2_div_d))/2;
}
/*
This segment sets up an clockwise or counterclockwise arc from the current position to the target position around
the center designated by the offset vector. All theta-values measured in radians of deviance from the positive
y-axis.
| <- theta == 0
* * *
* *
* *
* O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2)
* /
C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4))
*/
// calculate the theta (angle) of the current point
double theta_start = theta(-offset[gc.plane_axis_0], -offset[gc.plane_axis_1]);
// calculate the theta (angle) of the target point
double theta_end = theta(target[gc.plane_axis_0] - offset[gc.plane_axis_0] - gc.position[gc.plane_axis_0],
target[gc.plane_axis_1] - offset[gc.plane_axis_1] - gc.position[gc.plane_axis_1]);
// ensure that the difference is positive so that we have clockwise travel
if (theta_end < theta_start) { theta_end += 2*M_PI; }
double angular_travel = theta_end-theta_start;
// Invert angular motion if the g-code wanted a counterclockwise arc
if (gc.motion_mode == MOTION_MODE_CCW_ARC) {
angular_travel = angular_travel-2*M_PI;
}
// Find the radius
double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]);
// Calculate the motion along the depth axis of the helix
double depth = target[gc.plane_axis_2]-gc.position[gc.plane_axis_2];
// Trace the arc
mc_arc(theta_start, angular_travel, radius, depth, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
gc.position);
// Finish off with a line to make sure we arrive exactly where we think we are
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode);
break;
#endif
}
}
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
memcpy(gc.position, target, sizeof(double)*3); // gc.position[] = target[];
return(gc.status_code);
}
// Clear planner position vector. Called by homing routine.
void plan_clear_position()
{
clear_vector(pl.position);
}
void
w_players_in_game2::update_display(bool inFromDynamicWorld /* default=false */) {
// Start over - wipe out our local player-storage
clear_vector();
// Wipe out references to players through teams
for(int i = 0; i < NUMBER_OF_TEAM_COLORS; i++)
players_on_team[i].clear();
// Find the number of players
int num_players;
if(inFromDynamicWorld)
num_players = dynamic_world->player_count;
else
num_players = displaying_actual_information ? NetGetNumberOfPlayers() : 0;
// Fill in the entries
for(int i = 0; i < num_players; i++) {
player_entry2 thePlayerEntry;
int thePlayerTeam;
int thePlayerColor;
if(inFromDynamicWorld) {
// Get player information from dynamic_world
player_data* thePlayerData = get_player_data(i);
// Copy the player name. We will store it as a cstring...
strncpy(thePlayerEntry.player_name, thePlayerData->name, MAXIMUM_PLAYER_NAME_LENGTH + 1);
// Look up colors
thePlayerTeam = thePlayerData->team;
thePlayerColor = thePlayerData->color;
}
else {
// Get player information from topology
player_info* thePlayerInfo = (player_info*)NetGetPlayerData(i);
// Copy the player name. We will store it as a cstring...
strncpy(thePlayerEntry.player_name, thePlayerInfo->name, MAXIMUM_PLAYER_NAME_LENGTH + 1);
// Look up colors
thePlayerTeam = thePlayerInfo->team;
thePlayerColor = thePlayerInfo->color;
}
// Set the size of the text
thePlayerEntry.name_width = text_width(thePlayerEntry.player_name, font, style | styleShadow);
// Get the pixel-color for the player's team (for drawing the name)
thePlayerEntry.name_pixel_color = get_dialog_player_color(thePlayerTeam);
// Set up a player image for the player (funfun)
thePlayerEntry.player_image = new PlayerImage;
thePlayerEntry.player_image->setRandomFlatteringView();
thePlayerEntry.player_image->setPlayerColor(thePlayerColor);
thePlayerEntry.player_image->setTeamColor(thePlayerTeam);
// Add the player to our local storage area
player_entries.push_back(thePlayerEntry);
// Add a reference to the player through his team color
players_on_team[thePlayerTeam].push_back(i);
}
dirty = true;
}
w_players_in_game2::~w_players_in_game2() {
clear_vector();
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace). Comments and block delete
// characters have been removed. In this function, all units and positions are converted and
// exported to grbl's internal functions in terms of (mm, mm/min) and absolute machine
// coordinates, respectively.
uint8_t gc_execute_line(char *line)
{
/* -------------------------------------------------------------------------------------
STEP 1: Initialize parser block struct and copy current g-code state modes. The parser
updates these modes and commands as the block line is parser and will only be used and
executed after successful error-checking. The parser block struct also contains a block
values struct, word tracking variables, and a non-modal commands tracker for the new
block. This struct contains all of the necessary information to execute the block. */
memset(&gc_block, 0, sizeof(gc_block)); // Initialize the parser block struct.
memcpy(&gc_block.modal,&gc_state.modal,sizeof(gc_modal_t)); // Copy current modes
uint8_t axis_command = AXIS_COMMAND_NONE;
uint8_t axis_0, axis_1, axis_linear;
uint8_t coord_select = 0; // Tracks G10 P coordinate selection for execution
float coordinate_data[N_AXIS]; // Multi-use variable to store coordinate data for execution
float parameter_data[N_AXIS]; // Multi-use variable to store parameter data for execution
// Initialize bitflag tracking variables for axis indices compatible operations.
uint8_t axis_words = 0; // XYZ tracking
// Initialize command and value words variables. Tracks words contained in this block.
uint16_t command_words = 0; // G and M command words. Also used for modal group violations.
uint16_t value_words = 0; // Value words.
/* -------------------------------------------------------------------------------------
STEP 2: Import all g-code words in the block line. A g-code word is a letter followed by
a number, which can either be a 'G'/'M' command or sets/assigns a command value. Also,
perform initial error-checks for command word modal group violations, for any repeated
words, and for negative values set for the value words F, N, P, T, and S. */
uint8_t word_bit; // Bit-value for assigning tracking variables
uint8_t char_counter = 0;
char letter;
float value;
uint8_t int_value = 0;
uint8_t mantissa = 0; // NOTE: For mantissa values > 255, variable type must be changed to uint16_t.
while (line[char_counter] != 0) { // Loop until no more g-code words in line.
// Import the next g-code word, expecting a letter followed by a value. Otherwise, error out.
letter = line[char_counter];
if((letter < 'A') || (letter > 'Z')) { FAIL(STATUS_EXPECTED_COMMAND_LETTER); } // [Expected word letter]
char_counter++;
if (!read_float(line, &char_counter, &value)) { FAIL(STATUS_BAD_NUMBER_FORMAT); } // [Expected word value]
// Convert values to smaller uint8 significand and mantissa values for parsing this word.
// NOTE: Mantissa is multiplied by 100 to catch non-integer command values. This is more
// accurate than the NIST gcode requirement of x10 when used for commands, but not quite
// accurate enough for value words that require integers to within 0.0001. This should be
// a good enough comprimise and catch most all non-integer errors. To make it compliant,
// we would simply need to change the mantissa to int16, but this add compiled flash space.
// Maybe update this later.
int_value = trunc(value);
mantissa = round(100*(value - int_value)); // Compute mantissa for Gxx.x commands.
// NOTE: Rounding must be used to catch small floating point errors.
// Check if the g-code word is supported or errors due to modal group violations or has
// been repeated in the g-code block. If ok, update the command or record its value.
switch(letter) {
/* 'G' and 'M' Command Words: Parse commands and check for modal group violations.
NOTE: Modal group numbers are defined in Table 4 of NIST RS274-NGC v3, pg.20 */
case 'G':
// Determine 'G' command and its modal group
switch(int_value) {
case 1:
if (axis_command) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict]
axis_command = AXIS_COMMAND_MOTION_MODE;
gc_block.modal.motion = MOTION_MODE_LINEAR;
word_bit = MODAL_GROUP_G1;
break;
default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G command]
}
if (mantissa > 0) { FAIL(STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER); } // [Unsupported or invalid Gxx.x command]
// Check for more than one command per modal group violations in the current block
// NOTE: Variable 'word_bit' is always assigned, if the command is valid.
if ( bit_istrue(command_words,bit(word_bit)) ) { FAIL(STATUS_GCODE_MODAL_GROUP_VIOLATION); }
command_words |= bit(word_bit);
break;
case 'M':
// Determine 'M' command and its modal group
if (mantissa > 0) { FAIL(STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER); } // [No Mxx.x commands]
switch(int_value) {
case 0: case 2:
word_bit = MODAL_GROUP_M4;
switch(int_value) {
case 0: gc_block.modal.program_flow = PROGRAM_FLOW_PAUSED; break; // Program pause
case 2: gc_block.modal.program_flow = PROGRAM_FLOW_COMPLETED; break; // Program end and reset
}
break;
case 17: gc_block.modal.motor = MOTOR_ENABLE; break;
case 18: gc_block.modal.motor = MOTOR_DISABLE; break;
case 50: gc_block.modal.ldr = LDR_READ; break;
case 70: gc_block.modal.laser = LASER_DISABLE; break;
case 71: gc_block.modal.laser = LASER_ENABLE; break;
default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported M command]
}
// Check for more than one command per modal group violations in the current block
// NOTE: Variable 'word_bit' is always assigned, if the command is valid.
if ( bit_istrue(command_words,bit(word_bit)) ) { FAIL(STATUS_GCODE_MODAL_GROUP_VIOLATION); }
command_words |= bit(word_bit);
break;
// NOTE: All remaining letters assign values.
default:
/* Non-Command Words: This initial parsing phase only checks for repeats of the remaining
legal g-code words and stores their value. Error-checking is performed later since some
words (I,J,K,L,P,R) have multiple connotations and/or depend on the issued commands. */
switch(letter){
case 'F': word_bit = WORD_F; gc_block.values.f = value; break;
case 'T': word_bit = WORD_T; gc_block.values.t = int_value; break; // gc.values.t = int_value;
case 'X': word_bit = WORD_X; gc_block.values.xyz[X_AXIS] = value; axis_words |= (1<<X_AXIS); break;
/*case 'Y': word_bit = WORD_Y; gc_block.values.xyz[Y_AXIS] = value; axis_words |= (1<<Y_AXIS); break;
case 'Z': word_bit = WORD_Z; gc_block.values.xyz[Z_AXIS] = value; axis_words |= (1<<Z_AXIS); break;*/
default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND);
}
// NOTE: Variable 'word_bit' is always assigned, if the non-command letter is valid.
if (bit_istrue(value_words,bit(word_bit))) { FAIL(STATUS_GCODE_WORD_REPEATED); } // [Word repeated]
// Check for invalid negative values for words F, N, P, T, and S.
// NOTE: Negative value check is done here simply for code-efficiency.
if ( bit(word_bit) & (bit(WORD_F)|bit(WORD_N)|bit(WORD_P)|bit(WORD_T)|bit(WORD_S)) ) {
if (value < 0.0) { FAIL(STATUS_NEGATIVE_VALUE); } // [Word value cannot be negative]
}
value_words |= bit(word_bit); // Flag to indicate parameter assigned.
}
}
// Parsing complete!
/* -------------------------------------------------------------------------------------
STEP 3: Error-check all commands and values passed in this block. This step ensures all of
the commands are valid for execution and follows the NIST standard as closely as possible.
If an error is found, all commands and values in this block are dumped and will not update
the active system g-code modes. If the block is ok, the active system g-code modes will be
updated based on the commands of this block, and signal for it to be executed.
Also, we have to pre-convert all of the values passed based on the modes set by the parsed
block. There are a number of error-checks that require target information that can only be
accurately calculated if we convert these values in conjunction with the error-checking.
This relegates the next execution step as only updating the system g-code modes and
performing the programmed actions in order. The execution step should not require any
conversion calculations and would only require minimal checks necessary to execute.
*/
/* NOTE: At this point, the g-code block has been parsed and the block line can be freed.
NOTE: It's also possible, at some future point, to break up STEP 2, to allow piece-wise
parsing of the block on a per-word basis, rather than the entire block. This could remove
the need for maintaining a large string variable for the entire block and free up some memory.
To do this, this would simply need to retain all of the data in STEP 1, such as the new block
data struct, the modal group and value bitflag tracking variables, and axis array indices
compatible variables. This data contains all of the information necessary to error-check the
new g-code block when the EOL character is received. However, this would break Grbl's startup
lines in how it currently works and would require some refactoring to make it compatible.
*/
// [0. Non-specific/common error-checks and miscellaneous setup]:
// Determine implicit axis command conditions. Axis words have been passed, but no explicit axis
// command has been sent. If so, set axis command to current motion mode.
if (axis_words) {
if (!axis_command) { axis_command = AXIS_COMMAND_MOTION_MODE; } // Assign implicit motion-mode
}
// Check for valid line number N value.
if (bit_istrue(value_words,bit(WORD_N))) {
// Line number value cannot be less than zero (done) or greater than max line number.
if (gc_block.values.n > MAX_LINE_NUMBER) { FAIL(STATUS_GCODE_INVALID_LINE_NUMBER); } // [Exceeds max line number]
}
// bit_false(value_words,bit(WORD_N)); // NOTE: Single-meaning value word. Set at end of error-checking.
// Track for unused words at the end of error-checking.
// NOTE: Single-meaning value words are removed all at once at the end of error-checking, because
// they are always used when present. This was done to save a few bytes of flash. For clarity, the
// single-meaning value words may be removed as they are used. Also, axis words are treated in the
// same way. If there is an explicit/implicit axis command, XYZ words are always used and are
// are removed at the end of error-checking.
// [1. Comments ]: MSG's NOT SUPPORTED. Comment handling performed by protocol.
// [2. Set feed rate mode ]: G93 F word missing with G1,G2/3 active, implicitly or explicitly. Feed rate
// is not defined after switching to G94 from G93.
if (gc_block.modal.feed_rate == FEED_RATE_MODE_INVERSE_TIME) { // = G93
// NOTE: G38 can also operate in inverse time, but is undefined as an error. Missing F word check added here.
if (axis_command == AXIS_COMMAND_MOTION_MODE) {
if ((gc_block.modal.motion != MOTION_MODE_NONE) || (gc_block.modal.motion != MOTION_MODE_SEEK)) {
if (bit_isfalse(value_words,bit(WORD_F))) { FAIL(STATUS_GCODE_UNDEFINED_FEED_RATE); } // [F word missing]
}
}
// NOTE: It seems redundant to check for an F word to be passed after switching from G94 to G93. We would
// accomplish the exact same thing if the feed rate value is always reset to zero and undefined after each
// inverse time block, since the commands that use this value already perform undefined checks. This would
// also allow other commands, following this switch, to execute and not error out needlessly. This code is
// combined with the above feed rate mode and the below set feed rate error-checking.
// [3. Set feed rate ]: F is negative (done.)
// - In inverse time mode: Always implicitly zero the feed rate value before and after block completion.
// NOTE: If in G93 mode or switched into it from G94, just keep F value as initialized zero or passed F word
// value in the block. If no F word is passed with a motion command that requires a feed rate, this will error
// out in the motion modes error-checking. However, if no F word is passed with NO motion command that requires
// a feed rate, we simply move on and the state feed rate value gets updated to zero and remains undefined.
} else { // = G94
// - In units per mm mode: If F word passed, ensure value is in mm/min, otherwise push last state value.
if (gc_state.modal.feed_rate == FEED_RATE_MODE_UNITS_PER_MIN) { // Last state is also G94
if (bit_istrue(value_words,bit(WORD_F))) {
if (gc_block.modal.units == UNITS_MODE_INCHES) { gc_block.values.f *= MM_PER_INCH; }
} else {
gc_block.values.f = gc_state.feed_rate/60; // Push last state feed rate. Convert deg/min to deg/sec
}
} // Else, switching to G94 from G93, so don't push last state feed rate. Its undefined or the passed F word value.
}
// bit_false(value_words,bit(WORD_F)); // NOTE: Single-meaning value word. Set at end of error-checking.
// [4. Set spindle speed ]: S is negative (done.)
if (bit_isfalse(value_words,bit(WORD_S))) { gc_block.values.s = gc_state.spindle_speed; }
// bit_false(value_words,bit(WORD_S)); // NOTE: Single-meaning value word. Set at end of error-checking.
// [5. Select tool ]: NOT SUPPORTED. Only tracks value. T is negative (done.) Not an integer. Greater than max tool value.
// bit_false(value_words,bit(WORD_T)); // NOTE: Single-meaning value word. Set at end of error-checking.
// [6. Change tool ]: N/A
// [7. Spindle control ]: N/A
// [8. Coolant control ]: N/A
// [9. Enable/disable feed rate or spindle overrides ]: NOT SUPPORTED.
// [10. Dwell ]: P value missing. P is negative (done.) NOTE: See below.
if (gc_block.non_modal_command == NON_MODAL_DWELL) {
if (bit_isfalse(value_words,bit(WORD_P))) { FAIL(STATUS_GCODE_VALUE_WORD_MISSING); } // [P word missing]
bit_false(value_words,bit(WORD_P));
}
// [11. Set active plane ]: N/A
switch (gc_block.modal.plane_select) {
case PLANE_SELECT_XY:
axis_0 = X_AXIS;
axis_1 = Y_AXIS;
axis_linear = Z_AXIS;
break;
case PLANE_SELECT_ZX:
axis_0 = Z_AXIS;
axis_1 = X_AXIS;
axis_linear = Y_AXIS;
break;
default: // case PLANE_SELECT_YZ:
axis_0 = Y_AXIS;
axis_1 = Z_AXIS;
axis_linear = X_AXIS;
}
// [12. Set length units ]: N/A
// Pre-convert XYZ coordinate values to millimeters, if applicable.
uint8_t idx;
if (gc_block.modal.units == UNITS_MODE_INCHES) {
for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(idx)) ) {
gc_block.values.xyz[idx] *= MM_PER_INCH;
}
}
}
// [13. Cutter radius compensation ]: NOT SUPPORTED. Error, if G53 is active.
// [14. Cutter length compensation ]: G43 NOT SUPPORTED, but G43.1 and G49 are.
// [G43.1 Errors]: Motion command in same line.
// NOTE: Although not explicitly stated so, G43.1 should be applied to only one valid
// axis that is configured (in config.h). There should be an error if the configured axis
// is absent or if any of the other axis words are present.
if (axis_command == AXIS_COMMAND_TOOL_LENGTH_OFFSET ) { // Indicates called in block.
if (gc_block.modal.tool_length == TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC) {
if (axis_words ^ (1<<TOOL_LENGTH_OFFSET_AXIS)) { FAIL(STATUS_GCODE_G43_DYNAMIC_AXIS_ERROR); }
}
}
// [15. Coordinate system selection ]: *N/A. Error, if cutter radius comp is active.
// TODO: An EEPROM read of the coordinate data may require a buffer sync when the cycle
// is active. The read pauses the processor temporarily and may cause a rare crash. For
// future versions on processors with enough memory, all coordinate data should be stored
// in memory and written to EEPROM only when there is not a cycle active.
memcpy(coordinate_data,gc_state.coord_system,sizeof(gc_state.coord_system));
if ( bit_istrue(command_words,bit(MODAL_GROUP_G12)) ) { // Check if called in block
if (gc_block.modal.coord_select > N_COORDINATE_SYSTEM) { FAIL(STATUS_GCODE_UNSUPPORTED_COORD_SYS); } // [Greater than N sys]
if (gc_state.modal.coord_select != gc_block.modal.coord_select) {
if (!(settings_read_coord_data(gc_block.modal.coord_select,coordinate_data))) { FAIL(STATUS_SETTING_READ_FAIL); }
}
}
// [16. Set path control mode ]: NOT SUPPORTED.
// [17. Set distance mode ]: N/A. G90.1 and G91.1 NOT SUPPORTED.
// [18. Set retract mode ]: NOT SUPPORTED.
// [19. Remaining non-modal actions ]: Check go to predefined position, set G10, or set axis offsets.
// NOTE: We need to separate the non-modal commands that are axis word-using (G10/G28/G30/G92), as these
// commands all treat axis words differently. G10 as absolute offsets or computes current position as
// the axis value, G92 similarly to G10 L20, and G28/30 as an intermediate target position that observes
// all the current coordinate system and G92 offsets.
switch (gc_block.non_modal_command) {
case NON_MODAL_SET_COORDINATE_DATA:
// [G10 Errors]: L missing and is not 2 or 20. P word missing. (Negative P value done.)
// [G10 L2 Errors]: R word NOT SUPPORTED. P value not 0 to nCoordSys(max 9). Axis words missing.
// [G10 L20 Errors]: P must be 0 to nCoordSys(max 9). Axis words missing.
if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS) }; // [No axis words]
if (bit_isfalse(value_words,((1<<WORD_P)|(1<<WORD_L)))) { FAIL(STATUS_GCODE_VALUE_WORD_MISSING); } // [P/L word missing]
coord_select = trunc(gc_block.values.p); // Convert p value to int.
if (coord_select > N_COORDINATE_SYSTEM) { FAIL(STATUS_GCODE_UNSUPPORTED_COORD_SYS); } // [Greater than N sys]
if (gc_block.values.l != 20) {
if (gc_block.values.l == 2) {
if (bit_istrue(value_words,bit(WORD_R))) { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [G10 L2 R not supported]
} else { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [Unsupported L]
}
bit_false(value_words,(bit(WORD_L)|bit(WORD_P)));
// Determine coordinate system to change and try to load from EEPROM.
if (coord_select > 0) { coord_select--; } // Adjust P1-P6 index to EEPROM coordinate data indexing.
else { coord_select = gc_block.modal.coord_select; } // Index P0 as the active coordinate system
if (!settings_read_coord_data(coord_select,parameter_data)) { FAIL(STATUS_SETTING_READ_FAIL); } // [EEPROM read fail]
// Pre-calculate the coordinate data changes. NOTE: Uses parameter_data since coordinate_data may be in use by G54-59.
for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used.
// Update axes defined only in block. Always in machine coordinates. Can change non-active system.
if (bit_istrue(axis_words,bit(idx)) ) {
if (gc_block.values.l == 20) {
// L20: Update coordinate system axis at current position (with modifiers) with programmed value
parameter_data[idx] = gc_state.position[idx]-gc_state.coord_offset[idx]-gc_block.values.xyz[idx];
if (idx == TOOL_LENGTH_OFFSET_AXIS) { parameter_data[idx] -= gc_state.tool_length_offset; }
} else {
// L2: Update coordinate system axis to programmed value.
parameter_data[idx] = gc_block.values.xyz[idx];
}
}
}
break;
case NON_MODAL_SET_COORDINATE_OFFSET:
// [G92 Errors]: No axis words.
if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS); } // [No axis words]
// Update axes defined only in block. Offsets current system to defined value. Does not update when
// active coordinate system is selected, but is still active unless G92.1 disables it.
for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(idx)) ) {
gc_block.values.xyz[idx] = gc_state.position[idx]-coordinate_data[idx]-gc_block.values.xyz[idx];
if (idx == TOOL_LENGTH_OFFSET_AXIS) { gc_block.values.xyz[idx] -= gc_state.tool_length_offset; }
} else {
gc_block.values.xyz[idx] = gc_state.coord_offset[idx];
}
}
break;
default:
// At this point, the rest of the explicit axis commands treat the axis values as the traditional
// target position with the coordinate system offsets, G92 offsets, absolute override, and distance
// modes applied. This includes the motion mode commands. We can now pre-compute the target position.
// NOTE: Tool offsets may be appended to these conversions when/if this feature is added.
if (axis_command != AXIS_COMMAND_TOOL_LENGTH_OFFSET ) { // TLO block any axis command.
if (axis_words) {
for (idx=0; idx<N_AXIS; idx++) { // Axes indices are consistent, so loop may be used to save flash space.
if ( bit_isfalse(axis_words,bit(idx)) ) {
gc_block.values.xyz[idx] = gc_state.position[idx]; // No axis word in block. Keep same axis position.
} else {
// Update specified value according to distance mode or ignore if absolute override is active.
// NOTE: G53 is never active with G28/30 since they are in the same modal group.
if (gc_block.non_modal_command != NON_MODAL_ABSOLUTE_OVERRIDE) {
// Apply coordinate offsets based on distance mode.
if (gc_block.modal.distance == DISTANCE_MODE_ABSOLUTE) {
gc_block.values.xyz[idx] += coordinate_data[idx] + gc_state.coord_offset[idx];
if (idx == TOOL_LENGTH_OFFSET_AXIS) { gc_block.values.xyz[idx] += gc_state.tool_length_offset; }
} else { // Incremental mode
gc_block.values.xyz[idx] += gc_state.position[idx];
}
}
}
}
}
}
// Check remaining non-modal commands for errors.
switch (gc_block.non_modal_command) {
case NON_MODAL_GO_HOME_0:
// [G28 Errors]: Cutter compensation is enabled.
// Retreive G28 go-home position data (in machine coordinates) from EEPROM
if (!axis_words) { axis_command = AXIS_COMMAND_NONE; } // Set to none if no intermediate motion.
if (!settings_read_coord_data(SETTING_INDEX_G28,parameter_data)) { FAIL(STATUS_SETTING_READ_FAIL); }
break;
case NON_MODAL_GO_HOME_1:
// [G30 Errors]: Cutter compensation is enabled.
// Retreive G30 go-home position data (in machine coordinates) from EEPROM
if (!axis_words) { axis_command = AXIS_COMMAND_NONE; } // Set to none if no intermediate motion.
if (!settings_read_coord_data(SETTING_INDEX_G30,parameter_data)) { FAIL(STATUS_SETTING_READ_FAIL); }
break;
case NON_MODAL_SET_HOME_0: case NON_MODAL_SET_HOME_1:
// [G28.1/30.1 Errors]: Cutter compensation is enabled.
// NOTE: If axis words are passed here, they are interpreted as an implicit motion mode.
break;
case NON_MODAL_RESET_COORDINATE_OFFSET:
// NOTE: If axis words are passed here, they are interpreted as an implicit motion mode.
break;
case NON_MODAL_ABSOLUTE_OVERRIDE:
// [G53 Errors]: G0 and G1 are not active. Cutter compensation is enabled.
// NOTE: All explicit axis word commands are in this modal group. So no implicit check necessary.
if (!(gc_block.modal.motion == MOTION_MODE_SEEK || gc_block.modal.motion == MOTION_MODE_LINEAR)) {
FAIL(STATUS_GCODE_G53_INVALID_MOTION_MODE); // [G53 G0/1 not active]
}
break;
}
}
// [20. Motion modes ]:
if (gc_block.modal.motion == MOTION_MODE_NONE) {
// [G80 Errors]: Axis word exist and are not used by a non-modal command.
if ((axis_words) && (axis_command != AXIS_COMMAND_NON_MODAL)) {
FAIL(STATUS_GCODE_AXIS_WORDS_EXIST); // [No axis words allowed]
}
// Check remaining motion modes, if axis word are implicit (exist and not used by G10/28/30/92), or
// was explicitly commanded in the g-code block.
} else if ( axis_command == AXIS_COMMAND_MOTION_MODE ) {
if (gc_block.modal.motion == MOTION_MODE_SEEK) {
// [G0 Errors]: Axis letter not configured or without real value (done.)
// Axis words are optional. If missing, set axis command flag to ignore execution.
if (!axis_words) { axis_command = AXIS_COMMAND_NONE; }
// All remaining motion modes (all but G0 and G80), require a valid feed rate value. In units per mm mode,
// the value must be positive. In inverse time mode, a positive value must be passed with each block.
} else {
// Check if feed rate is defined for the motion modes that require it.
if (gc_block.values.f == 0.0) { FAIL(STATUS_GCODE_UNDEFINED_FEED_RATE); } // [Feed rate undefined]
switch (gc_block.modal.motion) {
case MOTION_MODE_LINEAR:
// [G1 Errors]: Feed rate undefined. Axis letter not configured or without real value.
// Axis words are optional. If missing, set axis command flag to ignore execution.
if (!axis_words) { axis_command = AXIS_COMMAND_NONE; }
break;
case MOTION_MODE_PROBE:
// [G38 Errors]: Target is same current. No axis words. Cutter compensation is enabled. Feed rate
// is undefined. Probe is triggered. NOTE: Probe check moved to probe cycle. Instead of returning
// an error, it issues an alarm to prevent further motion to the probe. It's also done there to
// allow the planner buffer to empty and move off the probe trigger before another probing cycle.
if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS); } // [No axis words]
if (gc_check_same_position(gc_state.position, gc_block.values.xyz)) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Invalid target]
break;
}
}
}
// [21. Program flow ]: No error checks required.
// [0. Non-specific error-checks]: Complete unused value words check,
// radius mode, or axis words that aren't used in the block.
bit_false(value_words,(bit(WORD_N)|bit(WORD_F)|bit(WORD_S)|bit(WORD_T))); // Remove single-meaning value words.
if (axis_command) { bit_false(value_words,(bit(WORD_X)|bit(WORD_Y)|bit(WORD_Z))); } // Remove axis words.
if (value_words) { FAIL(STATUS_GCODE_UNUSED_WORDS); } // [Unused words]
/* -------------------------------------------------------------------------------------
STEP 4: EXECUTE!!
Assumes that all error-checking has been completed and no failure modes exist. We just
need to update the state and execute the block according to the order-of-execution.
*/
// [1. Comments feedback ]: NOT SUPPORTED
// [2. Set feed rate mode ]:
gc_state.modal.feed_rate = gc_block.modal.feed_rate;
// [3. Set feed rate ]:
gc_state.feed_rate = gc_block.values.f*60; // Always copy this value. See feed rate error-checking. Convert deg/sec to deg/min
/*// [4. Set spindle speed ]:
if (gc_state.spindle_speed != gc_block.values.s) {
gc_state.spindle_speed = gc_block.values.s;
// Update running spindle only if not in check mode and not already enabled.
if (gc_state.modal.spindle != SPINDLE_DISABLE) { spindle_run(gc_state.modal.spindle, gc_state.spindle_speed); }
}*/
// [5. Select tool ]: NOT SUPPORTED. Only tracks tool value.
gc_state.tool = gc_block.values.t;
// [6. Change tool ]: NOT SUPPORTED
// [7. Spindle control ]: NOT SUPPORTED
// [8. Coolant control ]: NOT SUPPORTED
// [9. Enable/disable feed rate or spindle overrides ]: NOT SUPPORTED
// [10. Dwell ]:
if (gc_block.non_modal_command == NON_MODAL_DWELL) { mc_dwell(gc_block.values.p); }
// [11. Set active plane ]:
gc_state.modal.plane_select = gc_block.modal.plane_select;
// [12. Set length units ]: NOT SUPPORTED
// [13. Cutter radius compensation ]: NOT SUPPORTED
// [14. Cutter length compensation ]: G43.1 and G49 supported. G43 NOT SUPPORTED.
// NOTE: If G43 were supported, its operation wouldn't be any different from G43.1 in terms
// of execution. The error-checking step would simply load the offset value into the correct
// axis of the block XYZ value array.
if (axis_command == AXIS_COMMAND_TOOL_LENGTH_OFFSET ) { // Indicates a change.
gc_state.modal.tool_length = gc_block.modal.tool_length;
if (gc_state.modal.tool_length == TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC) { // G43.1
gc_state.tool_length_offset = gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS];
} else { // G49
gc_state.tool_length_offset = 0.0;
}
}
// [15. Coordinate system selection ]:
if (gc_state.modal.coord_select != gc_block.modal.coord_select) {
gc_state.modal.coord_select = gc_block.modal.coord_select;
memcpy(gc_state.coord_system,coordinate_data,sizeof(coordinate_data));
}
// [16. Set path control mode ]: NOT SUPPORTED
// [17. Set distance mode ]:
gc_state.modal.distance = gc_block.modal.distance;
// [18. Set retract mode ]: NOT SUPPORTED
// [19. Go to predefined position, Set G10, or Set axis offsets ]:
switch(gc_block.non_modal_command) {
case NON_MODAL_SET_COORDINATE_DATA:
settings_write_coord_data(coord_select,parameter_data);
// Update system coordinate system if currently active.
if (gc_state.modal.coord_select == coord_select) { memcpy(gc_state.coord_system,parameter_data,sizeof(parameter_data)); }
break;
case NON_MODAL_GO_HOME_0: case NON_MODAL_GO_HOME_1:
// Move to intermediate position before going home. Obeys current coordinate system and offsets
// and absolute and incremental modes.
if (axis_command) {
#ifdef USE_LINE_NUMBERS
mc_line(gc_block.values.xyz, -1.0, false, gc_block.values.n);
#else
mc_line(gc_block.values.xyz, -1.0, false);
#endif
}
#ifdef USE_LINE_NUMBERS
mc_line(parameter_data, -1.0, false, gc_block.values.n);
#else
mc_line(parameter_data, -1.0, false);
#endif
memcpy(gc_state.position, parameter_data, sizeof(parameter_data));
break;
case NON_MODAL_SET_HOME_0:
settings_write_coord_data(SETTING_INDEX_G28,gc_state.position);
break;
case NON_MODAL_SET_HOME_1:
settings_write_coord_data(SETTING_INDEX_G30,gc_state.position);
break;
case NON_MODAL_SET_COORDINATE_OFFSET:
memcpy(gc_state.coord_offset,gc_block.values.xyz,sizeof(gc_block.values.xyz));
break;
case NON_MODAL_RESET_COORDINATE_OFFSET:
clear_vector(gc_state.coord_offset); // Disable G92 offsets by zeroing offset vector.
break;
}
// [20. Motion modes ]:
// NOTE: Commands G10,G28,G30,G92 lock out and prevent axis words from use in motion modes.
// Enter motion modes only if there are axis words or a motion mode command word in the block.
gc_state.modal.motion = gc_block.modal.motion;
if (axis_command == AXIS_COMMAND_MOTION_MODE) {
switch (gc_state.modal.motion) {
case MOTION_MODE_SEEK:
#ifdef USE_LINE_NUMBERS
mc_line(gc_block.values.xyz, -1.0, false, gc_block.values.n);
#else
mc_line(gc_block.values.xyz, -1.0, false);
#endif
break;
case MOTION_MODE_LINEAR:
#ifdef USE_LINE_NUMBERS
mc_line(gc_block.values.xyz, gc_state.feed_rate, gc_state.modal.feed_rate, gc_block.values.n);
#else
mc_line(gc_block.values.xyz, gc_state.feed_rate, gc_state.modal.feed_rate);
#endif
break;
case MOTION_MODE_PROBE:
// NOTE: gc_block.values.xyz is returned from mc_probe_cycle with the updated position value. So
// upon a successful probing cycle, the machine position and the returned value should be the same.
#ifdef USE_LINE_NUMBERS
mc_probe_cycle(gc_block.values.xyz, gc_state.feed_rate, gc_state.modal.feed_rate, gc_block.values.n);
#else
mc_probe_cycle(gc_block.values.xyz, gc_state.feed_rate, gc_state.modal.feed_rate);
#endif
}
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_block.values.xyz)); // gc.position[] = target[];
}
// [21. Program flow ]:
// M0,M2: Perform non-running program flow actions. During a program pause, the buffer may
// refill and can only be resumed by the cycle start run-time command.
gc_state.modal.program_flow = gc_block.modal.program_flow;
if (gc_state.modal.program_flow) {
protocol_buffer_synchronize(); // Finish all remaining buffered motions. Program paused when complete.
sys.auto_start = false; // Disable auto cycle start. Forces pause until cycle start issued.
// If complete, reset to reload defaults (G92.2,G54,G17,G90,G94,M48,G40,M5,M9). Otherwise,
// re-enable program flow after pause complete, where cycle start will resume the program.
if (gc_state.modal.program_flow == PROGRAM_FLOW_COMPLETED) { mc_reset(); }
else { gc_state.modal.program_flow = PROGRAM_FLOW_RUNNING; }
}
// [22. Laser control ]:
gc_state.modal.laser = gc_block.modal.laser;
laser_run(gc_block.values.t, gc_block.modal.laser);
// [23. Motor control ]:
gc_state.modal.motor = gc_block.modal.motor;
if (gc_block.modal.motor == MOTOR_ENABLE) {
st_disable_on_idle(false);
st_wake_up();
}
else {
st_disable_on_idle(true);
st_go_idle();
}
// [23. LDR read ]:
if (gc_block.modal.ldr == LDR_READ){
print_ldr(gc_block.values.t);
}
// TODO: % to denote start of program. Sets auto cycle start?
return(STATUS_OK);
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace). Comments and block delete
// characters have been removed. All units and positions are converted and exported to grbl's
// internal functions in terms of (mm, mm/min) and absolute machine coordinates, respectively.
uint8_t gc_execute_line(char *line)
{
// If in alarm state, don't process. Immediately return with error.
// NOTE: Might not be right place for this, but also prevents $N storing during alarm.
if (sys.state == STATE_ALARM) { return(STATUS_ALARM_LOCK); }
uint8_t char_counter = 0;
char letter;
float value;
int int_value;
uint16_t modal_group_words = 0; // Bitflag variable to track and check modal group words in block
uint8_t axis_words = 0; // Bitflag to track which XYZ(ABC) parameters exist in block
float inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
uint8_t absolute_override = false; // true(1) = absolute motion for this block only {G53}
uint8_t non_modal_action = NON_MODAL_NONE; // Tracks the actions of modal group 0 (non-modal)
float target[4], offset[4];
clear_vector(target); // XYZ(ABC) axes parameters.
clear_vector(offset); // IJK Arc offsets are incremental. Value of zero indicates no change.
gc.status_code = STATUS_OK;
/* Pass 1: Commands and set all modes. Check for modal group violations.
NOTE: Modal group numbers are defined in Table 4 of NIST RS274-NGC v3, pg.20 */
uint8_t group_number = MODAL_GROUP_NONE;
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
switch(letter) {
case 'G':
// Set modal group values
switch(int_value) {
case 4: case 10: case 28: case 30: case 53: case 92: group_number = MODAL_GROUP_0; break;
case 0: case 1: case 2: case 3: case 80: group_number = MODAL_GROUP_1; break;
case 17: case 18: case 19: group_number = MODAL_GROUP_2; break;
case 90: case 91: group_number = MODAL_GROUP_3; break;
case 93: case 94: group_number = MODAL_GROUP_5; break;
case 20: case 21: group_number = MODAL_GROUP_6; break;
case 54: case 55: case 56: case 57: case 58: case 59: group_number = MODAL_GROUP_12; break;
}
// Set 'G' commands
switch(int_value) {
case 0: gc.motion_mode = MOTION_MODE_SEEK; break;
case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
case 4: non_modal_action = NON_MODAL_DWELL; break;
case 10: non_modal_action = NON_MODAL_SET_COORDINATE_DATA; break;
case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 28: case 30:
int_value = trunc(10*value); // Multiply by 10 to pick up Gxx.1
switch(int_value) {
case 280: non_modal_action = NON_MODAL_GO_HOME_0; break;
case 281: non_modal_action = NON_MODAL_SET_HOME_0; break;
case 300: non_modal_action = NON_MODAL_GO_HOME_1; break;
case 301: non_modal_action = NON_MODAL_SET_HOME_1; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 53: absolute_override = true; break;
case 54: case 55: case 56: case 57: case 58: case 59:
gc.coord_select = int_value-54;
break;
case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
case 90: gc.absolute_mode = true; break;
case 91: gc.absolute_mode = false; break;
case 92:
int_value = trunc(10*value); // Multiply by 10 to pick up G92.1
switch(int_value) {
case 920: non_modal_action = NON_MODAL_SET_COORDINATE_OFFSET; break;
case 921: non_modal_action = NON_MODAL_RESET_COORDINATE_OFFSET; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 93: gc.inverse_feed_rate_mode = true; break;
case 94: gc.inverse_feed_rate_mode = false; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'M':
// Set modal group values
switch(int_value) {
case 0: case 1: case 2: case 30: group_number = MODAL_GROUP_4; break;
case 3: case 4: case 5: group_number = MODAL_GROUP_7; break;
}
// Set 'M' commands
switch(int_value) {
case 0: gc.program_flow = PROGRAM_FLOW_PAUSED; break; // Program pause
case 1: break; // Optional stop not supported. Ignore.
case 2: case 30: gc.program_flow = PROGRAM_FLOW_COMPLETED; break; // Program end and reset
case 3: gc.spindle_direction = 1; break;
case 4: gc.spindle_direction = -1; break;
case 5: gc.spindle_direction = 0; break;
#ifdef ENABLE_M7
case 7: gc.coolant_mode = COOLANT_MIST_ENABLE; break;
#endif
case 8: gc.coolant_mode = COOLANT_FLOOD_ENABLE; break;
case 9: gc.coolant_mode = COOLANT_DISABLE; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
}
// Check for modal group multiple command violations in the current block
if (group_number) {
if ( bit_istrue(modal_group_words,bit(group_number)) ) {
FAIL(STATUS_MODAL_GROUP_VIOLATION);
} else {
bit_true(modal_group_words,bit(group_number));
}
group_number = MODAL_GROUP_NONE; // Reset for next command.
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
/* Pass 2: Parameters. All units converted according to current block commands. Position
parameters are converted and flagged to indicate a change. These can have multiple connotations
for different commands. Each will be converted to their proper value upon execution. */
float p = 0, r = 0;
uint8_t l = 0;
char_counter = 0;
while(next_statement(&letter, &value, line, &char_counter)) {
switch(letter) {
case 'G': case 'M': case 'N': break; // Ignore command statements and line numbers
case 'F':
if (value <= 0) { FAIL(STATUS_INVALID_STATEMENT); } // Must be greater than zero
if (gc.inverse_feed_rate_mode) {
inverse_feed_rate = to_millimeters(value); // seconds per motion for this motion only
} else {
gc.feed_rate = to_millimeters(value); // millimeters per minute
}
break;
case 'I': case 'J': case 'K': offset[letter-'I'] = to_millimeters(value); break;
case 'L': l = trunc(value); break;
case 'P': p = value; break;
case 'R': r = to_millimeters(value); break;
case 'S':
if (value < 0) { FAIL(STATUS_INVALID_STATEMENT); } // Cannot be negative
// TBD: Spindle speed not supported due to PWM issues, but may come back once resolved.
// gc.spindle_speed = value;
break;
case 'T':
if (value < 0) { FAIL(STATUS_INVALID_STATEMENT); } // Cannot be negative
gc.tool = trunc(value);
break;
case 'X': target[X_AXIS] = to_millimeters(value); bit_true(axis_words,bit(X_AXIS)); break;
case 'Y': target[Y_AXIS] = to_millimeters(value); bit_true(axis_words,bit(Y_AXIS)); break;
case 'Z': target[Z_AXIS] = to_millimeters(value); bit_true(axis_words,bit(Z_AXIS)); break;
case 'C': target[C_AXIS] = to_millimeters(value); bit_true(axis_wors,bit(C_AXIS)); break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
/* Execute Commands: Perform by order of execution defined in NIST RS274-NGC.v3, Table 8, pg.41.
NOTE: Independent non-motion/settings parameters are set out of this order for code efficiency
and simplicity purposes, but this should not affect proper g-code execution. */
// ([F]: Set feed and seek rates.)
// TODO: Seek rates can change depending on the direction and maximum speeds of each axes. When
// max axis speed is installed, the calculation can be performed here, or maybe in the planner.
if (sys.state != STATE_CHECK_MODE) {
// ([M6]: Tool change should be executed here.)
// [M3,M4,M5]: Update spindle state
spindle_run(gc.spindle_direction);
// [*M7,M8,M9]: Update coolant state
coolant_run(gc.coolant_mode);
}
// [G54,G55,...,G59]: Coordinate system selection
if ( bit_istrue(modal_group_words,bit(MODAL_GROUP_12)) ) { // Check if called in block
float coord_data[N_AXIS];
if (!(settings_read_coord_data(gc.coord_select,coord_data))) { return(STATUS_SETTING_READ_FAIL); }
memcpy(gc.coord_system,coord_data,sizeof(coord_data));
}
// [G4,G10,G28,G30,G92,G92.1]: Perform dwell, set coordinate system data, homing, or set axis offsets.
// NOTE: These commands are in the same modal group, hence are mutually exclusive. G53 is in this
// modal group and do not effect these actions.
switch (non_modal_action) {
case NON_MODAL_DWELL:
if (p < 0) { // Time cannot be negative.
FAIL(STATUS_INVALID_STATEMENT);
} else {
// Ignore dwell in check gcode modes
if (sys.state != STATE_CHECK_MODE) { mc_dwell(p); }
}
break;
case NON_MODAL_SET_COORDINATE_DATA:
int_value = trunc(p); // Convert p value to int.
if ((l != 2 && l != 20) || (int_value < 0 || int_value > N_COORDINATE_SYSTEM)) { // L2 and L20. P1=G54, P2=G55, ...
FAIL(STATUS_UNSUPPORTED_STATEMENT);
} else if (!axis_words && l==2) { // No axis words.
FAIL(STATUS_INVALID_STATEMENT);
} else {
if (int_value > 0) { int_value--; } // Adjust P1-P6 index to EEPROM coordinate data indexing.
else { int_value = gc.coord_select; } // Index P0 as the active coordinate system
float coord_data[N_AXIS];
if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
uint8_t i;
// Update axes defined only in block. Always in machine coordinates. Can change non-active system.
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(i)) ) {
if (l == 20) {
coord_data[i] = gc.position[i]-target[i]; // L20: Update axis current position to target
} else {
coord_data[i] = target[i]; // L2: Update coordinate system axis
}
}
}
settings_write_coord_data(int_value,coord_data);
// Update system coordinate system if currently active.
if (gc.coord_select == int_value) { memcpy(gc.coord_system,coord_data,sizeof(coord_data)); }
}
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_GO_HOME_0: case NON_MODAL_GO_HOME_1:
// Move to intermediate position before going home. Obeys current coordinate system and offsets
// and absolute and incremental modes.
if (axis_words) {
// Apply absolute mode coordinate offsets or incremental mode offsets.
uint8_t i;
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if ( bit_istrue(axis_words,bit(i)) ) {
if (gc.absolute_mode) {
target[i] += gc.coord_system[i] + gc.coord_offset[i];
} else {
target[i] += gc.position[i];
}
} else {
target[i] = gc.position[i];
}
}
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[C_AXIS], settings.default_seek_rate, false);
}
// Retreive G28/30 go-home position data (in machine coordinates) from EEPROM
float coord_data[N_AXIS];
if (non_modal_action == NON_MODAL_GO_HOME_1) {
if (!settings_read_coord_data(SETTING_INDEX_G30 ,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
} else {
if (!settings_read_coord_data(SETTING_INDEX_G28 ,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
}
mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], coord_data[C_AXIS], settings.default_seek_rate, false);
memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[];
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_SET_HOME_0: case NON_MODAL_SET_HOME_1:
if (non_modal_action == NON_MODAL_SET_HOME_1) {
settings_write_coord_data(SETTING_INDEX_G30,gc.position);
} else {
settings_write_coord_data(SETTING_INDEX_G28,gc.position);
}
break;
case NON_MODAL_SET_COORDINATE_OFFSET:
if (!axis_words) { // No axis words
FAIL(STATUS_INVALID_STATEMENT);
} else {
// Update axes defined only in block. Offsets current system to defined value. Does not update when
// active coordinate system is selected, but is still active unless G92.1 disables it.
uint8_t i;
for (i=0; i<=2; i++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(i)) ) {
gc.coord_offset[i] = gc.position[i]-gc.coord_system[i]-target[i];
}
}
}
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_RESET_COORDINATE_OFFSET:
clear_vector(gc.coord_offset); // Disable G92 offsets by zeroing offset vector.
break;
}
// [G0,G1,G2,G3,G80]: Perform motion modes.
// NOTE: Commands G10,G28,G30,G92 lock out and prevent axis words from use in motion modes.
// Enter motion modes only if there are axis words or a motion mode command word in the block.
if ( bit_istrue(modal_group_words,bit(MODAL_GROUP_1)) || axis_words ) {
// G1,G2,G3 require F word in inverse time mode.
if ( gc.inverse_feed_rate_mode ) {
if (inverse_feed_rate < 0 && gc.motion_mode != MOTION_MODE_CANCEL) {
FAIL(STATUS_INVALID_STATEMENT);
}
}
// Absolute override G53 only valid with G0 and G1 active.
if ( absolute_override && !(gc.motion_mode == MOTION_MODE_SEEK || gc.motion_mode == MOTION_MODE_LINEAR)) {
FAIL(STATUS_INVALID_STATEMENT);
}
// Report any errors.
if (gc.status_code) { return(gc.status_code); }
// Convert all target position data to machine coordinates for executing motion. Apply
// absolute mode coordinate offsets or incremental mode offsets.
// NOTE: Tool offsets may be appended to these conversions when/if this feature is added.
uint8_t i;
for (i=0; i<=3; i++) { // Axes indices are consistent, so loop may be used to save flash space.
if ( bit_istrue(axis_words,bit(i)) ) {
if (!absolute_override) { // Do not update target in absolute override mode
if (gc.absolute_mode) {
target[i] += gc.coord_system[i] + gc.coord_offset[i]; // Absolute mode
} else {
target[i] += gc.position[i]; // Incremental mode
}
}
} else {
target[i] = gc.position[i]; // No axis word in block. Keep same axis position.
}
}
switch (gc.motion_mode) {
case MOTION_MODE_CANCEL:
if (axis_words) { FAIL(STATUS_INVALID_STATEMENT); } // No axis words allowed while active.
break;
case MOTION_MODE_SEEK:
if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);}
else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[C_AXIS], settings.default_seek_rate, false); }
break;
case MOTION_MODE_LINEAR:
// TODO: Inverse time requires F-word with each statement. Need to do a check. Also need
// to check for initial F-word upon startup. Maybe just set to zero upon initialization
// and after an inverse time move and then check for non-zero feed rate each time. This
// should be efficient and effective.
if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);}
else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[C_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); }
break;
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
// Check if at least one of the axes of the selected plane has been specified. If in center
// format arc mode, also check for at least one of the IJK axes of the selected plane was sent.
if ( !( bit_false(axis_words,bit(gc.plane_axis_2)) ) ||
( !r && !offset[gc.plane_axis_0] && !offset[gc.plane_axis_1] ) ) {
FAIL(STATUS_INVALID_STATEMENT);
} else {
if (r != 0) { // Arc Radius Mode
/*
We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == magnitude of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
d^2 == x^2 + y^2
h^2 == r^2 - (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
O <- [i,j]
- |
r - |
- |
- | h
- |
[0,0] -> C -----------------+--------------- T <- [x,y]
| <------ d/2 ---->|
C - Current position
T - Target position
O - center of circle that pass through both C and T
d - distance from C to T
r - designated radius
h - distance from center of CT to O
Expanding the equations:
d -> sqrt(x^2 + y^2)
h -> sqrt(4 * r^2 - x^2 - y^2)/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Which can be written:
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Which we for size and speed reasons optimize to:
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2
*/
// Calculate the change in position along each selected axis
float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
clear_vector(offset);
// First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller
// than d. If so, the sqrt of a negative number is complex and error out.
float h_x2_div_d = 4 * r*r - x*x - y*y;
if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return(gc.status_code); }
// Finish computing h_x2_div_d.
h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d)
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
the left hand circle will be generated - when it is negative the right hand circle is generated.
T <-- Target position
^
Clockwise circles with this center | Clockwise circles with this center will have
will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
\ | /
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
|
|
C <-- Current position */
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
if (r < 0) {
h_x2_div_d = -h_x2_div_d;
r = -r; // Finished with r. Set to positive for mc_arc
}
// Complete the operation by calculating the actual center of the arc
offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
} else { // Arc Center Format Offset Mode
r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
}
// Set clockwise/counter-clockwise sign for mc_arc computations
uint8_t isclockwise = false;
if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
// Trace the arc
mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
r, isclockwise);
}
break;
}
// Report any errors.
if (gc.status_code) { return(gc.status_code); }
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
memcpy(gc.position, target, sizeof(target)); // gc.position[] = target[];
}
// M0,M1,M2,M30: Perform non-running program flow actions. During a program pause, the buffer may
// refill and can only be resumed by the cycle start run-time command.
if (gc.program_flow) {
plan_synchronize(); // Finish all remaining buffered motions. Program paused when complete.
sys.auto_start = false; // Disable auto cycle start. Forces pause until cycle start issued.
// If complete, reset to reload defaults (G92.2,G54,G17,G90,G94,M48,G40,M5,M9). Otherwise,
// re-enable program flow after pause complete, where cycle start will resume the program.
if (gc.program_flow == PROGRAM_FLOW_COMPLETED) { mc_reset(); }
else { gc.program_flow = PROGRAM_FLOW_RUNNING; }
}
return(gc.status_code);
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace). Comments and block delete
// characters have been removed.
uint8_t gcode_execute_line(char *line) {
uint8_t char_counter = 0;
char letter;
double value;
int int_value;
double unit_converted_value;
uint8_t next_action = NEXT_ACTION_NONE;
double target[3];
double p = 0.0;
int cs = 0;
int l = 0;
bool got_actual_line_command = false; // as opposed to just e.g. G1 F1200
gc.status_code = STATUS_OK;
//// Pass 1: Commands
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
switch(letter) {
case 'G':
switch(int_value) {
case 0: gc.motion_mode = next_action = NEXT_ACTION_SEEK; break;
case 1: gc.motion_mode = next_action = NEXT_ACTION_FEED; break;
case 4: next_action = NEXT_ACTION_DWELL; break;
case 10: next_action = NEXT_ACTION_SET_COORDINATE_OFFSET; break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 30: next_action = NEXT_ACTION_HOMING_CYCLE; break;
case 54: gc.offselect = OFFSET_G54; break;
case 55: gc.offselect = OFFSET_G55; break;
case 90: gc.absolute_mode = true; break;
case 91: gc.absolute_mode = false; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'M':
switch(int_value) {
case 7: next_action = NEXT_ACTION_AIR_ENABLE;break;
case 8: next_action = NEXT_ACTION_GAS_ENABLE;break;
case 9: next_action = NEXT_ACTION_AIRGAS_DISABLE;break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
}
if (gc.status_code) { break; }
}
// bail when errors
if (gc.status_code) { return gc.status_code; }
char_counter = 0;
memcpy(target, gc.position, sizeof(target)); // i.e. target = gc.position
//// Pass 2: Parameters
while(next_statement(&letter, &value, line, &char_counter)) {
if (gc.inches_mode) {
unit_converted_value = value * MM_PER_INCH;
} else {
unit_converted_value = value;
}
switch(letter) {
case 'F':
if (unit_converted_value <= 0) { FAIL(STATUS_BAD_NUMBER_FORMAT); }
if (gc.motion_mode == NEXT_ACTION_SEEK) {
gc.seek_rate = unit_converted_value;
} else {
gc.feed_rate = unit_converted_value;
}
break;
case 'X': case 'Y': case 'Z':
if (gc.absolute_mode) {
target[letter - 'X'] = unit_converted_value;
} else {
target[letter - 'X'] += unit_converted_value;
}
got_actual_line_command = true;
break;
case 'P': // dwelling seconds or CS selector
if (next_action == NEXT_ACTION_SET_COORDINATE_OFFSET) {
cs = trunc(value);
} else {
p = value;
}
break;
case 'S':
gc.nominal_laser_intensity = value;
break;
case 'L': // G10 qualifier
l = trunc(value);
break;
}
}
// bail when error
if (gc.status_code) { return(gc.status_code); }
//// Perform any physical actions
switch (next_action) {
case NEXT_ACTION_SEEK:
if (got_actual_line_command) {
planner_line( target[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS],
target[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS],
target[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS],
gc.seek_rate, 0 );
}
break;
case NEXT_ACTION_FEED:
if (got_actual_line_command) {
planner_line( target[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS],
target[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS],
target[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS],
gc.feed_rate, gc.nominal_laser_intensity );
}
break;
case NEXT_ACTION_DWELL:
planner_dwell(p, gc.nominal_laser_intensity);
break;
// case NEXT_ACTION_STOP:
// planner_stop(); // stop and cancel the remaining program
// gc.position[X_AXIS] = stepper_get_position_x();
// gc.position[Y_AXIS] = stepper_get_position_y();
// gc.position[Z_AXIS] = stepper_get_position_z();
// planner_set_position(gc.position[X_AXIS], gc.position[Y_AXIS], gc.position[Z_AXIS]);
// // move to table origin
// target[X_AXIS] = 0;
// target[Y_AXIS] = 0;
// target[Z_AXIS] = 0;
// planner_line( target[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS],
// target[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS],
// target[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS],
// gc.seek_rate, 0 );
// break;
case NEXT_ACTION_HOMING_CYCLE:
stepper_homing_cycle();
// now that we are at the physical home
// zero all the position vectors
clear_vector(gc.position);
clear_vector(target);
planner_set_position(0.0, 0.0, 0.0);
// move head to g54 offset
gc.offselect = OFFSET_G54;
target[X_AXIS] = 0;
target[Y_AXIS] = 0;
target[Z_AXIS] = 0;
planner_line( target[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS],
target[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS],
target[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS],
gc.seek_rate, 0 );
break;
case NEXT_ACTION_SET_COORDINATE_OFFSET:
if (cs == OFFSET_G54 || cs == OFFSET_G55) {
if (l == 2) {
//set offset to target, eg: G10 L2 P1 X15 Y15 Z0
gc.offsets[3*cs+X_AXIS] = target[X_AXIS];
gc.offsets[3*cs+Y_AXIS] = target[Y_AXIS];
gc.offsets[3*cs+Z_AXIS] = target[Z_AXIS];
// Set target in ref to new coord system so subsequent moves are calculated correctly.
target[X_AXIS] = (gc.position[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS]) - gc.offsets[3*cs+X_AXIS];
target[Y_AXIS] = (gc.position[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS]) - gc.offsets[3*cs+Y_AXIS];
target[Z_AXIS] = (gc.position[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS]) - gc.offsets[3*cs+Z_AXIS];
} else if (l == 20) {
// set offset to current pos, eg: G10 L20 P2
gc.offsets[3*cs+X_AXIS] = gc.position[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS];
gc.offsets[3*cs+Y_AXIS] = gc.position[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS];
gc.offsets[3*cs+Z_AXIS] = gc.position[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS];
target[X_AXIS] = 0;
target[Y_AXIS] = 0;
target[Z_AXIS] = 0;
}
}
break;
case NEXT_ACTION_AIRGAS_DISABLE:
planner_control_airgas_disable();
break;
case NEXT_ACTION_AIR_ENABLE:
planner_control_air_enable();
break;
case NEXT_ACTION_GAS_ENABLE:
planner_control_gas_enable();
break;
}
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
memcpy(gc.position, target, sizeof(double)*3); // gc.position[] = target[];
return gc.status_code;
}
int t_vector()
{
c_vector vt;
__c_vector(&vt, int_comparer);
printf("1.test create vector\n");
create_with_push_back(&vt);
print_vector(&vt);
reverse_print_vector(&vt);
clear_vector(&vt);
printf("\n\n2.test vector assign\n");
create_with_push_back(&vt);
vector_assign(&vt);
clear_vector(&vt);
printf("\n\n3.test erase vector\n");
erase_vector();
printf("\n\n4.test reserve vector\n");
create_with_push_back(&vt);
vector_reserve(&vt);
clear_vector(&vt);
printf("\n\n5.test front back\n");
create_with_push_back(&vt);
vector_front_back(&vt);
clear_vector(&vt);
printf("\n\n6.test swap\n");
create_with_push_back(&vt);
vector_swap(&vt);
clear_vector(&vt);
printf("\n\n7.test insert\n");
vector_insert(&vt);
clear_vector(&vt);
printf("\n\n8.test insert2\n");
create_with_push_back(&vt);
vector_insert2(&vt);
clear_vector(&vt);
printf("\n\n9.test fill insert\n");
create_with_push_back(&vt);
vector_fill_insert(&vt);
clear_vector(&vt);
printf("\n\n10.test resize\n");
create_with_push_back(&vt);
vector_resize(&vt);
clear_vector(&vt);
printf("\n\n11.test equal\n");
create_with_push_back(&vt);
vector_equal(&vt);
clear_vector(&vt);
printf("\n\n12.test less\n");
create_with_push_back(&vt);
vector_less(&vt);
clear_vector(&vt);
__c_rotcev(&vt);
printf("\n\nfinish testing vector!\n");
return 0;
}
void TextureAtlas::clean( )
{
clear_vector( &internalTextures );
}
