--- /dev/null
+// 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_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++)
+ commands[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('$', instruction, size)&&(
+ 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;
+ }
+
+ targetPosServo = fp.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);
+ servo.write(targetPosServo);
+ //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);
+ goto_machine_zero();
+ 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);
+ goto_machine_zero();
+ 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);
+ }
+ }
+ 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);
+ }
+ }
+ if(has_command('$', instruction, size)){
+
+ code = search_string('$', instruction, size);
+ switch(code){
+ case 1:
+ //set XYZ STEP PIN
+ if (has_command('X', instruction, size)){
+ X_STEP_PIN = search_string('X', instruction, size);
+ pinMode(X_STEP_PIN,OUTPUT);
+ digitalWrite(X_STEP_PIN,LOW);
+ }
+ if (has_command('Y', instruction, size)){
+ Y_STEP_PIN = search_string('Y', instruction, size);
+ pinMode(Y_STEP_PIN,OUTPUT);
+ digitalWrite(Y_STEP_PIN,LOW);
+ }
+ if (has_command('Z', instruction, size)){
+ int TEMP_PIN = search_string('Z', instruction, size);
+
+ if(Z_STEP_PIN!=TEMP_PIN){
+ Z_STEP_PIN = TEMP_PIN;
+ if(Z_ENABLE_SERVO==1){
+ servo.attach(Z_STEP_PIN);
+ }else{
+ pinMode(Z_STEP_PIN,OUTPUT);
+ digitalWrite(Z_STEP_PIN,LOW);
+ }
+ }
+ }
+ break;
+ case 2:
+ //set XYZ DIR PIN
+ if (has_command('X', instruction, size)){
+ X_DIR_PIN = search_string('X', instruction, size);
+ pinMode(X_DIR_PIN,OUTPUT);
+ digitalWrite(X_DIR_PIN,LOW);
+ }
+ if (has_command('Y', instruction, size)){
+ Y_DIR_PIN = search_string('Y', instruction, size);
+ pinMode(Y_DIR_PIN,OUTPUT);
+ digitalWrite(Y_DIR_PIN,LOW);
+ }
+ if (has_command('Z', instruction, size)){
+ Z_DIR_PIN = search_string('Z', instruction, size);
+ pinMode(Z_DIR_PIN,OUTPUT);
+ digitalWrite(Z_DIR_PIN,LOW);
+ }
+ break;
+ case 3:
+ //set XYZ Min PIN
+ if (has_command('X', instruction, size)){
+ X_MIN_PIN = search_string('X', instruction, size);
+ pinMode(X_MIN_PIN,INPUT_PULLUP);
+ }
+ if (has_command('Y', instruction, size)){
+ Y_MIN_PIN = search_string('Y', instruction, size);
+ pinMode(Y_MIN_PIN,INPUT_PULLUP);
+ }
+ if (has_command('Z', instruction, size)){
+ Z_MIN_PIN = search_string('Z', instruction, size);
+ pinMode(Z_MIN_PIN,INPUT_PULLUP);
+ }
+ break;
+ case 4:
+ //set XYZ Max PIN
+ if (has_command('X', instruction, size)){
+ X_MAX_PIN = search_string('X', instruction, size);
+ pinMode(X_MAX_PIN,INPUT_PULLUP);
+ }
+ if (has_command('Y', instruction, size)){
+ Y_MAX_PIN = search_string('Y', instruction, size);
+ pinMode(Y_MAX_PIN,INPUT_PULLUP);
+ }
+ if (has_command('Z', instruction, size)){
+ Z_MAX_PIN = search_string('Z', instruction, size);
+ pinMode(Z_MAX_PIN,INPUT_PULLUP);
+ }
+ break;
+ case 5:
+ //ENABLE SERVO MOTOR FOR Z
+ if(has_command('Z',instruction,size)){
+ Z_ENABLE_SERVO = search_string('Z', instruction, size);
+
+ }
+ break;
+ case 6:
+ //set XYZ STEPS PER MM
+ if (has_command('X', instruction, size)){
+ X_STEPS_PER_MM = search_string('X', instruction, size);
+ x_units = X_STEPS_PER_MM;
+ Serial.println(x_units);
+ }
+ if (has_command('Y', instruction, size)){
+ Y_STEPS_PER_MM = search_string('Y', instruction, size);
+ y_units = Y_STEPS_PER_MM;
+ }
+ if (has_command('Z', instruction, size)){
+ Z_STEPS_PER_MM = search_string('Z', instruction, size);
+ z_units = Z_STEPS_PER_MM;
+ }
+ break;
+ case 7:
+ //set XYZ FEEDRATE
+ if (has_command('X', instruction, size)){
+ FAST_XY_FEEDRATE = search_string('X', instruction, size);
+ }else if (has_command('Y', instruction, size)){
+ FAST_XY_FEEDRATE = search_string('Y', instruction, size);
+ }
+ if (has_command('Z', instruction, size)){
+ FAST_Z_FEEDRATE = search_string('Z', instruction, size);
+ }
+ break;
+ case 8:
+ //set XYZ INVERT LIMIT SWITCH
+ if (has_command('S', instruction, size)){
+ SENSORS_INVERTING = search_string('S', instruction, size);
+ }
+ break;
+ }
+
+ }
+ //tell our host we're done.
+ if(code==0&&size==1){
+ Serial.println("start");
+ }else{
+ 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;
+}
--- /dev/null
+
+//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_PULLUP);
+ pinMode(X_MAX_PIN, INPUT_PULLUP);
+
+ pinMode(Y_STEP_PIN, OUTPUT);
+ pinMode(Y_DIR_PIN, OUTPUT);
+ pinMode(Y_ENABLE_PIN, OUTPUT);
+ pinMode(Y_MIN_PIN, INPUT_PULLUP);
+ pinMode(Y_MAX_PIN, INPUT_PULLUP);
+
+ pinMode(Z_STEP_PIN, OUTPUT);
+ pinMode(Z_DIR_PIN, OUTPUT);
+ pinMode(Z_ENABLE_PIN, OUTPUT);
+ pinMode(Z_MIN_PIN, INPUT_PULLUP);
+ pinMode(Z_MAX_PIN, INPUT_PULLUP);
+
+ //figure our stuff.
+ calculate_deltas();
+ goto_machine_zero();
+}
+
+void goto_machine_zero()
+{
+ Serial.println("init");
+ Serial.println("calibrate axis x");
+ move_to_max(X_MIN_PIN, X_STEP_PIN, X_DIR_PIN, 0);
+ Serial.println("calibrate axis y");
+ move_to_max(Y_MIN_PIN, Y_STEP_PIN, Y_DIR_PIN, 0);
+ Serial.println("ok");
+}
+
+void move_to_max(int limiter_pin, int stepper_pin, int stepper_dir_pin,int dir)
+{
+ /* Moves to the maximum possible position
+ */
+ Serial.println("go to max");
+ while(can_step(limiter_pin, limiter_pin, 0, 1, dir)){
+ do_step(stepper_pin, stepper_dir_pin, 0);
+ delay(1);
+ }
+ Serial.println("step back");
+ // slowly back unitl pin is released
+ while(!can_step(limiter_pin, limiter_pin, 0, 1, dir)){
+ do_step(stepper_pin, stepper_dir_pin, 1);
+ delay(100);
+ }
+ Serial.println("done");
+}
+
+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)
+ {
+ if(Z_ENABLE_SERVO==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 * 60000000.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);
+}
projects / livingdesktop.git / commitdiff