++++++++simple gcodesender https://github.com/OttoHermansson/GcodeSender/downloads++++++
http://www.societyofrobots.com/robotforum/index.php?topic=9615.10;wap2
gcode interpreter colonello
// Arduino G-code Interpreter
// v1.0 Carlos guilarte y diego colonnello...
// Modificado para manejar tres ejes en un cnc chimbo... jejeje
#include <HardwareSerial.h>
//our command string
#define COMMAND_SIZE 128
#include "Arduino.h"
void setup();
void loop();
void init_process_string();
void process_string(char instruction[], int size);
double search_string(char key, char instruction[], int string_size);
bool has_command(char key, char instruction[], int string_size);
void init_steppers();
void dda_move(long micro_delay);
bool can_step(byte min_pin, byte max_pin, long current, long target, byte direction);
void do_step(byte pinA, byte pinB, byte dir);
bool read_switch(byte pin);
long to_steps(float steps_per_unit, float units);
void set_target(float x, float y, float z);
void set_position(float x, float y, float z);
void calculate_deltas();
long calculate_feedrate_delay(float feedrate);
long getMaxSpeed();
void disable_steppers();
char palabra[COMMAND_SIZE];
byte serial_count;
int no_data = 0;
void setup()
{
//Do startup stuff here
Serial.begin(19200);
Serial.println("start");
//other initialization.
init_process_string();
init_steppers();
}
void loop()
{
char c;
//keep it hot!
//read in characters if we got them.
if (Serial.available() > 0)
{
c = Serial.read();
no_data = 0;
//newlines are ends of commands.
if (c != '\n')
{
palabra[serial_count] = c;
serial_count++;
}
}
//mark no data.
else
{
no_data++;
delayMicroseconds(100);
}
//if theres a pause or we got a real command, do it
if (serial_count && (c == '\n' || no_data > 100))
{
//process our command!
process_string(palabra, serial_count);
//clear command.
init_process_string();
}
//no data? turn off steppers
if (no_data > 1000)
disable_steppers();
}
// define the parameters of our machine.
#define X_STEPS_PER_INCH 4800
#define X_STEPS_PER_MM 188.97
#define X_MOTOR_STEPS 200
#define Y_STEPS_PER_INCH 4800
#define Y_STEPS_PER_MM 188.97
#define Y_MOTOR_STEPS 200
#define Z_STEPS_PER_INCH 4800
#define Z_STEPS_PER_MM 188.97
#define Z_MOTOR_STEPS 200
//our maximum feedrates
#define FAST_XY_FEEDRATE 100
#define FAST_Z_FEEDRATE 100
// Units in curve section
#define CURVE_SECTION_INCHES 0.019685
#define CURVE_SECTION_MM 0.5
// Set to one if sensor outputs inverting (ie: 1 means open, 0 means closed)
// RepRap opto endstops are *not* inverting.
#define SENSORS_INVERTING 0
// How many temperature samples to take. each sample takes about 100 usecs.
/****************************************************************************************
* digital i/o pin assignment
*
* this uses the undocumented feature of Arduino - pins 14-19 correspond to analog 0-5
****************************************************************************************/
//cartesian bot pins
#define X_STEP_PIN 8
#define X_DIR_PIN 9
#define X_MIN_PIN 4
#define X_MAX_PIN 2
#define X_ENABLE_PIN 15
#define Y_STEP_PIN 10
#define Y_DIR_PIN 11
#define Y_MIN_PIN 3
#define Y_MAX_PIN 5
#define Y_ENABLE_PIN 15
#define Z_STEP_PIN 12
#define Z_DIR_PIN 13
#define Z_MIN_PIN 7
#define Z_MAX_PIN 6
#define Z_ENABLE_PIN 15
// our point structure to make things nice.
struct LongPoint {
long x;
long y;
long z;
};
struct FloatPoint {
float x;
float y;
float z;
};
FloatPoint current_units;
FloatPoint target_units;
FloatPoint delta_units;
FloatPoint current_steps;
FloatPoint target_steps;
FloatPoint delta_steps;
boolean abs_mode = false; //0 = incremental; 1 = absolute
//default to inches for units
float x_units = X_STEPS_PER_INCH;
float y_units = Y_STEPS_PER_INCH;
float z_units = Z_STEPS_PER_INCH;
float curve_section = CURVE_SECTION_INCHES;
//our direction vars
byte x_direction = 1;
byte y_direction = 1;
byte z_direction = 1;
//init our string processing
void init_process_string()
{
//init our command
for (byte i=0; i<COMMAND_SIZE; i++)
palabra[i] = 0;
serial_count = 0;
}
//our feedrate variables.
float feedrate = 0.0;
long feedrate_micros = 0;
//Read the string and execute instructions
void process_string(char instruction[], int size)
{
//the character / means delete block... used for comments and stuff.
if (instruction[0] == '/')
{
Serial.println("ok");
return;
}
//init baby!
FloatPoint fp;
fp.x = 0.0;
fp.y = 0.0;
fp.z = 0.0;
byte code = 0;;
//did we get a gcode?
if (
has_command('G', instruction, size) ||
has_command('X', instruction, size) ||
has_command('Y', instruction, size) ||
has_command('Z', instruction, size)
)
{
//which one?
code = (int)search_string('G', instruction, size);
// Get co-ordinates if required by the code type given
switch (code)
{
case 0:
case 1:
case 2:
case 3:
if(abs_mode)
{
//we do it like this to save time. makes curves better.
//eg. if only x and y are specified, we dont have to waste time looking up z.
if (has_command('X', instruction, size))
fp.x = search_string('X', instruction, size);
else
fp.x = current_units.x;
if (has_command('Y', instruction, size))
fp.y = search_string('Y', instruction, size);
else
fp.y = current_units.y;
if (has_command('Z', instruction, size))
fp.z = search_string('Z', instruction, size);
else
fp.z = current_units.z;
}
else
{
fp.x = search_string('X', instruction, size) + current_units.x;
fp.y = search_string('Y', instruction, size) + current_units.y;
fp.z = search_string('Z', instruction, size) + current_units.z;
}
break;
}
//do something!
switch (code)
{
//Rapid Positioning
//Linear Interpolation
//these are basically the same thing.
case 0:
case 1:
//set our target.
set_target(fp.x, fp.y, fp.z);
//do we have a set speed?
if (has_command('G', instruction, size))
{
//adjust if we have a specific feedrate.
if (code == 1)
{
//how fast do we move?
feedrate = search_string('F', instruction, size);
if (feedrate > 0)
feedrate_micros = calculate_feedrate_delay(feedrate);
//nope, no feedrate
else
feedrate_micros = getMaxSpeed();
}
//use our max for normal moves.
else
feedrate_micros = getMaxSpeed();
}
//nope, just coordinates!
else
{
//do we have a feedrate yet?
if (feedrate > 0)
feedrate_micros = calculate_feedrate_delay(feedrate);
//nope, no feedrate
else
feedrate_micros = getMaxSpeed();
}
//finally move.
dda_move(feedrate_micros);
break;
//Clockwise arc
case 2:
//Counterclockwise arc
case 3:
FloatPoint cent;
// Centre coordinates are always relative
cent.x = search_string('I', instruction, size) + current_units.x;
cent.y = search_string('J', instruction, size) + current_units.y;
float angleA, angleB, angle, radius, length, aX, aY, bX, bY;
aX = (current_units.x - cent.x);
aY = (current_units.y - cent.y);
bX = (fp.x - cent.x);
bY = (fp.y - cent.y);
if (code == 2) { // Clockwise
angleA = atan2(bY, bX);
angleB = atan2(aY, aX);
} else { // Counterclockwise
angleA = atan2(aY, aX);
angleB = atan2(bY, bX);
}
// Make sure angleB is always greater than angleA
// and if not add 2PI so that it is (this also takes
// care of the special case of angleA == angleB,
// ie we want a complete circle)
if (angleB <= angleA) angleB += 2 * M_PI;
angle = angleB - angleA;
radius = sqrt(aX * aX + aY * aY);
length = radius * angle;
int steps, s, step;
steps = (int) ceil(length / curve_section);
FloatPoint newPoint;
for (s = 1; s <= steps; s++) {
step = (code == 3) ? s : steps - s; // Work backwards for CW
newPoint.x = cent.x + radius * cos(angleA + angle * ((float) step / steps));
newPoint.y = cent.y + radius * sin(angleA + angle * ((float) step / steps));
set_target(newPoint.x, newPoint.y, fp.z);
// Need to calculate rate for each section of curve
if (feedrate > 0)
feedrate_micros = calculate_feedrate_delay(feedrate);
else
feedrate_micros = getMaxSpeed();
// Make step
dda_move(feedrate_micros);
}
break;
//Dwell
case 4:
delay((int)search_string('P', instruction, size));
break;
//Inches for Units
case 20:
x_units = X_STEPS_PER_INCH;
y_units = Y_STEPS_PER_INCH;
z_units = Z_STEPS_PER_INCH;
curve_section = CURVE_SECTION_INCHES;
calculate_deltas();
break;
//mm for Units
case 21:
x_units = X_STEPS_PER_MM;
y_units = Y_STEPS_PER_MM;
z_units = Z_STEPS_PER_MM;
curve_section = CURVE_SECTION_MM;
calculate_deltas();
break;
//go home.
case 28:
set_target(0.0, 0.0, 0.0);
dda_move(getMaxSpeed());
break;
//go home via an intermediate point.
case 30:
fp.x = search_string('X', instruction, size);
fp.y = search_string('Y', instruction, size);
fp.z = search_string('Z', instruction, size);
//set our target.
if(abs_mode)
{
if (!has_command('X', instruction, size))
fp.x = current_units.x;
if (!has_command('Y', instruction, size))
fp.y = current_units.y;
if (!has_command('Z', instruction, size))
fp.z = current_units.z;
set_target(fp.x, fp.y, fp.z);
}
else
set_target(current_units.x + fp.x, current_units.y + fp.y, current_units.z + fp.z);
//go there.
dda_move(getMaxSpeed());
//go home.
set_target(0.0, 0.0, 0.0);
dda_move(getMaxSpeed());
break;
//Absolute Positioning
case 90:
abs_mode = true;
break;
//Incremental Positioning
case 91:
abs_mode = false;
break;
//Set as home
case 92:
set_position(0.0, 0.0, 0.0);
break;
/*
//Inverse Time Feed Mode
case 93:
break; //TODO: add this
//Feed per Minute Mode
case 94:
break; //TODO: add this
*/
default:
Serial.print("huh? G");
Serial.println(code,DEC);
}
}
//find us an m code.
if (has_command('M', instruction, size))
{
code = search_string('M', instruction, size);
switch (code)
{
//TODO: this is a bug because search_string returns 0. gotta fix that.
case 0:
true;
break;
default:
Serial.print("Huh? M");
Serial.println(code);
}
}
//tell our host we're done.
Serial.println("ok");
// Serial.println(line, DEC);
}
//look for the number that appears after the char key and return it
double search_string(char key, char instruction[], int string_size)
{
char temp[10] = "";
for (byte i=0; i<string_size; i++)
{
if (instruction[i] == key)
{
i++;
int k = 0;
while (i < string_size && k < 10)
{
if (instruction[i] == 0 || instruction[i] == ' ')
break;
temp[k] = instruction[i];
i++;
k++;
}
return strtod(temp, NULL);
}
}
return 0;
}
//look for the command if it exists.
bool has_command(char key, char instruction[], int string_size)
{
for (byte i=0; i<string_size; i++)
{
if (instruction[i] == key)
return true;
}
return false;
}
//init our variables
long max_delta;
long x_counter;
long y_counter;
long z_counter;
bool x_can_step;
bool y_can_step;
bool z_can_step;
int milli_delay;
void init_steppers()
{
//turn them off to start.
disable_steppers();
//init our points.
current_units.x = 0.0;
current_units.y = 0.0;
current_units.z = 0.0;
target_units.x = 0.0;
target_units.y = 0.0;
target_units.z = 0.0;
pinMode(X_STEP_PIN, OUTPUT);
pinMode(X_DIR_PIN, OUTPUT);
pinMode(X_ENABLE_PIN, OUTPUT);
pinMode(X_MIN_PIN, INPUT);
pinMode(X_MAX_PIN, INPUT);
pinMode(Y_STEP_PIN, OUTPUT);
pinMode(Y_DIR_PIN, OUTPUT);
pinMode(Y_ENABLE_PIN, OUTPUT);
pinMode(Y_MIN_PIN, INPUT);
pinMode(Y_MAX_PIN, INPUT);
pinMode(Z_STEP_PIN, OUTPUT);
pinMode(Z_DIR_PIN, OUTPUT);
pinMode(Z_ENABLE_PIN, OUTPUT);
pinMode(Z_MIN_PIN, INPUT);
pinMode(Z_MAX_PIN, INPUT);
//figure our stuff.
calculate_deltas();
}
void dda_move(long micro_delay)
{
//enable our steppers
digitalWrite(X_ENABLE_PIN, HIGH);
digitalWrite(Y_ENABLE_PIN, HIGH);
digitalWrite(Z_ENABLE_PIN, HIGH);
//figure out our deltas
max_delta = max(delta_steps.x, delta_steps.y);
max_delta = max(delta_steps.z, max_delta);
//init stuff.
long x_counter = -max_delta/2;
long y_counter = -max_delta/2;
long z_counter = -max_delta/2;
//our step flags
bool x_can_step = 0;
bool y_can_step = 0;
bool z_can_step = 0;
if (micro_delay >= 16383)
milli_delay = micro_delay / 1000;
else
milli_delay = 0;
//do our DDA line!
do
{
x_can_step = can_step(X_MIN_PIN, X_MAX_PIN, current_steps.x, target_steps.x, x_direction);
y_can_step = can_step(Y_MIN_PIN, Y_MAX_PIN, current_steps.y, target_steps.y, y_direction);
z_can_step = can_step(Z_MIN_PIN, Z_MAX_PIN, current_steps.z, target_steps.z, z_direction);
if (x_can_step)
{
x_counter += delta_steps.x;
if (x_counter > 0)
{
do_step(X_STEP_PIN, X_DIR_PIN, x_direction);
x_counter -= max_delta;
if (x_direction)
current_steps.x++;
else
current_steps.x--;
}
}
if (y_can_step)
{
y_counter += delta_steps.y;
if (y_counter > 0)
{
do_step(Y_STEP_PIN, Y_DIR_PIN, y_direction);
y_counter -= max_delta;
if (y_direction)
current_steps.y++;
else
current_steps.y--;
}
}
if (z_can_step)
{
z_counter += delta_steps.z;
if (z_counter > 0)
{
do_step(Z_STEP_PIN, Z_DIR_PIN, z_direction);
z_counter -= max_delta;
if (z_direction)
current_steps.z++;
else
current_steps.z--;
}
}
//wait for next step.
if (milli_delay > 0)
delay(milli_delay);
else
delayMicroseconds(micro_delay);
}
while (x_can_step || y_can_step || z_can_step);
//set our points to be the same
current_units.x = target_units.x;
current_units.y = target_units.y;
current_units.z = target_units.z;
calculate_deltas();
}
bool can_step(byte min_pin, byte max_pin, long current, long target, byte direction)
{
//stop us if we're on target
if (target == current)
return false;
//stop us if we're at home and still going
else if (read_switch(min_pin) && !direction)
return false;
//stop us if we're at max and still going
else if (read_switch(max_pin) && direction)
return false;
//default to being able to step
return true;
}
void do_step(byte pinA, byte pinB, byte dir)
{
switch (dir << 2 | digitalRead(pinA) << 1 | digitalRead(pinB)) {
case 0: /* 0 00 -> 10 */
case 5: /* 1 01 -> 11 */
digitalWrite(pinA, HIGH);
break;
case 1: /* 0 01 -> 00 */
case 7: /* 1 11 -> 10 */
digitalWrite(pinB, LOW);
break;
case 2: /* 0 10 -> 11 */
case 4: /* 1 00 -> 01 */
digitalWrite(pinB, HIGH);
break;
case 3: /* 0 11 -> 01 */
case 6: /* 1 10 -> 00 */
digitalWrite(pinA, LOW);
break;
}
delayMicroseconds(5);
}
bool read_switch(byte pin)
{
//dual read as crude debounce
if ( SENSORS_INVERTING )
return !digitalRead(pin) && !digitalRead(pin);
else
return digitalRead(pin) && digitalRead(pin);
}
long to_steps(float steps_per_unit, float units)
{
return steps_per_unit * units;
}
void set_target(float x, float y, float z)
{
target_units.x = x;
target_units.y = y;
target_units.z = z;
calculate_deltas();
}
void set_position(float x, float y, float z)
{
current_units.x = x;
current_units.y = y;
current_units.z = z;
calculate_deltas();
}
void calculate_deltas()
{
//figure our deltas.
delta_units.x = abs(target_units.x - current_units.x);
delta_units.y = abs(target_units.y - current_units.y);
delta_units.z = abs(target_units.z - current_units.z);
//set our steps current, target, and delta
current_steps.x = to_steps(x_units, current_units.x);
current_steps.y = to_steps(y_units, current_units.y);
current_steps.z = to_steps(z_units, current_units.z);
target_steps.x = to_steps(x_units, target_units.x);
target_steps.y = to_steps(y_units, target_units.y);
target_steps.z = to_steps(z_units, target_units.z);
delta_steps.x = abs(target_steps.x - current_steps.x);
delta_steps.y = abs(target_steps.y - current_steps.y);
delta_steps.z = abs(target_steps.z - current_steps.z);
//what is our direction
x_direction = (target_units.x >= current_units.x);
y_direction = (target_units.y >= current_units.y);
z_direction = (target_units.z >= current_units.z);
//set our direction pins as well
digitalWrite(X_DIR_PIN, x_direction);
digitalWrite(Y_DIR_PIN, y_direction);
digitalWrite(Z_DIR_PIN, z_direction);
}
long calculate_feedrate_delay(float feedrate)
{
//how long is our line length?
float distance = sqrt(delta_units.x*delta_units.x + delta_units.y*delta_units.y + delta_units.z*delta_units.z);
long master_steps = 0;
//find the dominant axis.
if (delta_steps.x > delta_steps.y)
{
if (delta_steps.z > delta_steps.x)
master_steps = delta_steps.z;
else
master_steps = delta_steps.x;
}
else
{
if (delta_steps.z > delta_steps.y)
master_steps = delta_steps.z;
else
master_steps = delta_steps.y;
}
//calculate delay between steps in microseconds. this is sort of tricky, but not too bad.
//the formula has been condensed to save space. here it is in english:
// distance / feedrate * 60000000.0 = move duration in microseconds
// move duration / master_steps = time between steps for master axis.
return ((distance * 600000000.0) / feedrate) / master_steps;
}
long getMaxSpeed()
{
if (delta_steps.z > 0)
return calculate_feedrate_delay(FAST_Z_FEEDRATE);
else
return calculate_feedrate_delay(FAST_XY_FEEDRATE);
}
void disable_steppers()
{
//enable our steppers
digitalWrite(X_ENABLE_PIN, LOW);
digitalWrite(Y_ENABLE_PIN, LOW);
digitalWrite(Z_ENABLE_PIN, LOW);
}
int main(void)
{
init();
setup();
for (;;)
loop();
return 0;
}