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"

int costyx;

int costyy;

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(long current, long target);

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(9600);

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();

//Serial.println(c);

//newlines are ends of commands.

if (c != '\n')

{

palabra[serial_count] = c;

serial_count++;

}

}

//if theres a pause or we got a real command, do it

if (serial_count && (c == '\n'))

{

//Serial.println(palabra);

//process our command!

process_string(palabra, serial_count);

//clear command.

init_process_string();

}

}

// define the parameters of our machine.**************************************************************************************************************************************

#define X_STEPS_PER_INCH 4800

#define X_STEPS_PER_MM 400

#define X_MOTOR_STEPS 100

#define Y_STEPS_PER_INCH 4800

#define Y_STEPS_PER_MM 400

#define Y_MOTOR_STEPS 100

#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 500

#define FAST_Z_FEEDRATE 300

// 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 0//8

//#define X_DIR_PIN 0//9

//#define X_MIN_PIN 0/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 = true; //0 = incremental; 1 = absolute

//default to inches for units

float x_units = X_STEPS_PER_MM;

float y_units = Y_STEPS_PER_MM;

float z_units = Z_STEPS_PER_MM;

float curve_section = CURVE_SECTION_MM;

//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:

//Serial.println("aici");

//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;

DDRC=255;

DDRD=255;

// 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( current_steps.x, target_steps.x );

y_can_step = can_step( current_steps.y, target_steps.y);

z_can_step = can_step( current_steps.z, target_steps.z);

if (x_can_step)

{

x_counter += delta_steps.x;

if (x_counter > 0)

{

//Serial.print("current_steps.x=");

//Serial.println(current_steps.x);

//do_step(X_STEP_PIN, X_DIR_PIN, x_direction);

x_counter -= max_delta;

if (x_direction) {

costyx=costyx+1;

if (costyx > 8) costyx=1;

if (costyx==1) PORTD=0B100000;

if (costyx==2) PORTD=0B110000;

if (costyx==3) PORTD=0B010000;

if (costyx==4) PORTD=0B011000;

if (costyx==5) PORTD=0B001000;

if (costyx==6) PORTD=0B001100;

if (costyx==7) PORTD=0B000100;

if (costyx==8) PORTD=0B100100;

}

else {

costyx=costyx-1;

//Serial.println(PORTD,BIN);

if (costyx < 1) costyx=8;

if (costyx==1) PORTD=0B100000;

if (costyx==2) PORTD=0B110000;

if (costyx==3) PORTD=0B010000;

if (costyx==4) PORTD=0B011000;

if (costyx==5) PORTD=0B001000;

if (costyx==6) PORTD=0B001100;

if (costyx==7) PORTD=0B000100;

if (costyx==8) PORTD=0B100100;

}

if (x_direction)

current_steps.x++;

else

current_steps.x--;

}

}

if (y_can_step)

{

y_counter += delta_steps.y;

if (y_counter > 0)

{

// Serial.print("current_steps.y=");

// Serial.println(current_steps.y);

//do_step(Y_STEP_PIN, Y_DIR_PIN, y_direction);

y_counter -= max_delta;

if (y_direction){

costyy++;

if (costyy > 8) costyy=1;

if (costyy==1) PORTC=0B1000;

if (costyy==2) PORTC=0B1100;

if (costyy==3) PORTC=0B0100;

if (costyy==4) PORTC=0B0110;

if (costyy==5) PORTC=0B0010;

if (costyy==6) PORTC=0B0011;

if (costyy==7) PORTC=0B0001;

if (costyy==8) PORTC=0B1001;

}

else {

costyy--;

if (costyy < 1) costyy=8;

if (costyy==1) PORTC=0B1000;

if (costyy==2) PORTC=0B1100;

if (costyy==3) PORTC=0B0100;

if (costyy==4) PORTC=0B0110;

if (costyy==5) PORTC=0B0010;

if (costyy==6) PORTC=0B0011;

if (costyy==7) PORTC=0B0001;

if (costyy==8) PORTC=0B1001;

}

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( long current, long target)

{

//stop us if we're on target

if (target == current)

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;

}