From: Mjolnir <thomas.pietrzak@inria.fr>
Date: Wed, 19 Aug 2015 09:05:27 +0000 (+0200)
Subject: mouse arduino code
X-Git-Url: https://git.thomaspietrzak.com/?a=commitdiff_plain;h=3e2ca5335da917baf92c69eea68f66048583b2f2;p=livingdesktop.git

mouse arduino code
---

diff --git a/GCodeParser/GCodeParser.ino b/GCodeParser/GCodeParser.ino
new file mode 100644
index 0000000..c658da7
--- /dev/null
+++ b/GCodeParser/GCodeParser.ino
@@ -0,0 +1,162 @@
+/*
+  Pin numbers adapted
+*/
+
+#include <Servo.h>
+// define the parameters of our machine.
+float X_STEPS_PER_INCH = 48;
+float X_STEPS_PER_MM = 40;
+int X_MOTOR_STEPS   = 100;
+
+float Y_STEPS_PER_INCH = 48;
+float Y_STEPS_PER_MM  = 40;
+int Y_MOTOR_STEPS   = 100;
+
+float Z_STEPS_PER_INCH = 48;
+float Z_STEPS_PER_MM   = 40;
+int Z_MOTOR_STEPS    = 100;
+
+//our maximum feedrates
+long FAST_XY_FEEDRATE = 2000;
+long FAST_Z_FEEDRATE = 2000;
+
+// 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.
+int SENSORS_INVERTING = 1;
+
+// 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
+****************************************************************************************/
+/*
+int X_STEP_PIN = 8;
+int X_DIR_PIN = 11;
+int X_ENABLE_PIN = 4;
+int X_MIN_PIN = A4;
+int X_MAX_PIN = A5;
+
+int Y_STEP_PIN = 12;
+int Y_DIR_PIN = 13;
+int Y_ENABLE_PIN = 4;
+int Y_MIN_PIN = A1;
+int Y_MAX_PIN = A0;
+
+int Z_STEP_PIN = A3;
+int Z_DIR_PIN = 9;
+int Z_ENABLE_PIN = 4;
+int Z_MIN_PIN = A1;
+int Z_MAX_PIN = A0;
+int Z_ENABLE_SERVO = 1;*/
+
+int X_STEP_PIN = 10;
+int X_DIR_PIN = 11;
+int X_ENABLE_PIN = -1;
+int X_MIN_PIN = 17;
+int X_MAX_PIN = 16;
+
+int Y_STEP_PIN = 9;
+int Y_DIR_PIN = 3;
+int Y_ENABLE_PIN = -1;
+int Y_MIN_PIN = 12;
+int Y_MAX_PIN = 13;
+
+int Z_STEP_PIN = 15;
+int Z_DIR_PIN = -1;
+int Z_ENABLE_PIN = -1;
+int Z_MIN_PIN = -1;
+int Z_MAX_PIN = -1;
+int Z_ENABLE_SERVO = 1;
+
+#define COMMAND_SIZE 128
+
+char commands[COMMAND_SIZE];
+byte serial_count;
+int no_data = 0;
+
+Servo servo;
+
+int currentPosServo = 90;
+int targetPosServo = 90;
+bool comment = false;
+void setup()
+{
+	//Do startup stuff here
+	Serial.begin(115200);
+        if(Z_ENABLE_SERVO==1){
+          servo.attach(Z_STEP_PIN);
+        }
+	//other initialization.
+	init_process_string();
+	init_steppers();
+	process_string("G90",3);//Absolute Position
+        Serial.println("start");
+}
+
+void loop()
+{
+  
+	char c;
+	//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')
+		{
+			if(c==0x18){
+				Serial.println("Grbl 1.0");
+			}else{
+                          if (c == '('){
+                            comment = true;
+                          }
+                          // If we're not in comment mode, add it to our array.
+                          if (!comment)
+                          {
+                            commands[serial_count] = c;
+                    				serial_count++;
+                          }
+                          if (c == ')'){
+                            comment = false; // End of comment - start listening again
+                          }
+                        }
+				
+		}
+	}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(commands, serial_count);
+		//clear command.
+		init_process_string();
+	}
+
+	//no data?  turn off steppers
+	if (no_data > 1000){
+		disable_steppers();
+	}
+        }
+//        return;
+//                delay(5);
+//                int dPos = abs(currentPosServo-targetPosServo);
+//                if(currentPosServo<targetPosServo){
+//                   currentPosServo += dPos>8?6:1;
+//                }else if(currentPosServo>targetPosServo){
+//                   currentPosServo -= dPos>8?6:1;
+//                }
+                
+        
+}
diff --git a/GCodeParser/process_string.ino b/GCodeParser/process_string.ino
new file mode 100644
index 0000000..665ea61
--- /dev/null
+++ b/GCodeParser/process_string.ino
@@ -0,0 +1,488 @@
+// 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;
+}
diff --git a/GCodeParser/stepper_control.ino b/GCodeParser/stepper_control.ino
new file mode 100644
index 0000000..dc1f2da
--- /dev/null
+++ b/GCodeParser/stepper_control.ino
@@ -0,0 +1,325 @@
+
+//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);
+}