Squirrels have fuzzy tails.
0 Members and 1 Guest are viewing this topic.
// 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 128char 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 unitsfloat 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 varsbyte x_direction = 1;byte y_direction = 1;byte z_direction = 1;//init our string processingvoid init_process_string(){ //init our command for (byte i=0; i<COMMAND_SIZE; i++) palabra = 0; serial_count = 0;}//our feedrate variables.float feedrate = 0.0;long feedrate_micros = 0;//Read the string and execute instructionsvoid 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 itdouble search_string(char key, char instruction[], int string_size){ char temp[10] = ""; for (byte i=0; i<string_size; i++) { if (instruction == key) { i++; int k = 0; while (i < string_size && k < 10) { if (instruction == 0 || instruction == ' ') break; temp[k] = instruction; 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 == key) return true; } return false;}
//init our variableslong 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);}