From dc17a8cc5b452a69a2e8b543d33d4333643ae78f Mon Sep 17 00:00:00 2001
From: Michael Moon <triffid.hunter@gmail.com>
Date: Fri, 15 Jan 2010 18:53:44 +1100
Subject: [PATCH] time to save again

---
 mendel/mendel.c | 94 ++++++++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 86 insertions(+), 8 deletions(-)

diff --git a/mendel/mendel.c b/mendel/mendel.c
index 8116b3d..bf46518 100644
--- a/mendel/mendel.c
+++ b/mendel/mendel.c
@@ -11,9 +11,11 @@
 	machine variables
 */
 #define	AXIS_COUNT 5
+#define	AXIS_HOMING_COUNT	3
 
 //														steps per rev * mm per tooth * teeth per rev
 #define	STEPS_PER_MM					(3200 * 4.77 * 16)
+#define	STEPS_PER_IN					(STEPS_PER_MM * 25.4)
 
 #define	MAX_MM_PER_SEC				40
 #define	MAX_IMMED_MM_PER_SEC	20
@@ -35,6 +37,12 @@ uint8_t command;
 uint8_t command_digit;
 uint8_t axis;
 
+uint8_t	option_bitfield;
+#define	OPTION_RELATIVE						1
+#define	OPTION_SYNCHRONISE				2
+#define	OPTION_HOME_WHEN_COMPLETE	4
+#define	OPTION_UNIT_INCHES				8
+
 // volatile uint32_t xpos = 0;
 // volatile uint32_t ypos = 0;
 // volatile uint32_t zpos = 0;
@@ -52,8 +60,10 @@ struct axis {
 	volatile uint32_t	pos;
 	int32_t						target;
 	uint32_t					newtarget_mantissa;
-	uint8_t						newtarget_exp;
-	uint8_t						newtarget_sign;
+	uint8_t						newtarget_opt;
+#define	newtarget_opt_sign	0x80
+#define	newtarget_opt_set		0x40
+#define	newtarget_opt_exp		0x3F
 	uint16_t					speed;
 } axes[AXIS_COUNT];
 
@@ -118,28 +128,96 @@ int main (void)
 				if (axis_char_to_id(c) < 255) {
 					axis = axis_char_to_id(c);
 					axes[axis].newtarget_mantissa = 0;
-					axes[axis].newtarget_exp = 0;
+					axes[axis].newtarget_opt = 0;
 					state = STATE_WAIT_FOR_DATA_DIGIT;
 				}
 				break;
 			case STATE_WAIT_FOR_DATA_DIGIT:
 				if (c == '-') {
-					axes[axis].newtarget_sign = 1;
+					axes[axis].newtarget_opt |= newtarget_opt_sign;
 				}
 				if (c >= '0' && c <= '9') {
 					axes[axis].newtarget_mantissa = (axes[axis].newtarget_mantissa * 10) + (c - '0');
-					if (axes[axis].newtarget_exp)
-						axes[axis].newtarget_exp++;
+					if (axes[axis].newtarget_opt & newtarget_opt_exp)
+						axes[axis].newtarget_opt++;
 				}
 				if (c == '.') {
-					axes[axis].newtarget_exp = 1;
+					axes[axis].newtarget_opt |= 1;
 				}
 				break;
 		}
 		if (c == 13) {
+			if (command == 'G') {
+				uint8_t i;
+				switch (command_digit) {
+					//	G30 - go home via point
+					case 30:
+						option_bitfield |= OPTION_HOME_WHEN_COMPLETE;
+					// 	G0 - rapid, unsynchronised motion
+					case 0:
+						option_bitfield &= ~OPTION_SYNCHRONISE;
+						break;
+					//	G1 - synchronised motion
+					case 1:
+						option_bitfield |= OPTION_SYNCHRONISE;
+						break;
+					//	G2 - Arc Clockwise
+					//	G3 - Arc Counter-clockwise
+					//	G4 - Dwell
+					//	G20 - inches as units
+					//	G21 - mm as units
+					//	G28 - go home
+					case 28:
+						for (i = 0; i < AXIS_HOMING_COUNT; i++) {
+							option_bitfield &= ~OPTION_SYNCHRONISE;
+							axes[i].target = 0;
+						}
+						break;
+					//	G90 - absolute positioning
+					case 90:
+						option_bitfield &= ~OPTION_RELATIVE;
+						break;
+					//	G91 - relative positioning
+					case 91:
+						option_bitfield |= OPTION_RELATIVE;
+						break;
+					//	G92 - set home
+					case 92:
+						for (i = 0; i < AXIS_HOMING_COUNT; i++) {
+							axes[i].pos = axes[i].target = 0;
+						}
+						break;
+					// TODO: spit an error
+				}
+			}
+			else if (command == 'M') {
+				switch (command_digit) {
+					// TODO: spit an error
+				}
+			}
+			// update axes;
 			uint8_t i;
 			for (i = 0; i < AXIS_COUNT; i++) {
-				uint32_t n = axes[i].newtarget_mantissa;
+				if (axes[i].newtarget_opt & newtarget_opt_set) {
+					float n = axes[i].newtarget_mantissa;
+					uint8_t exp = axes[i].newtarget_opt & newtarget_opt_exp;
+					if (axes[i].newtarget_opt & newtarget_opt_sign)
+						n = -n;
+					if (exp == 1)
+						n /= 10;
+					if (exp == 2)
+						n /= 100;
+					if (exp == 3)
+						n /= 1000;
+					if (exp == 4)
+						n /= 10000;
+					if (exp == 5)
+						n /= 100000;
+					if (option_bitfield & OPTION_UNIT_INCHES)
+						axes[i].target = n * STEPS_PER_IN;
+					else
+						axes[i].target = n * STEPS_PER_MM;
+				}
 			}
 		}
 	}