Commits

Trammell Hudson committed 3218c70 Draft

drive 1-3 works

Comments (0)

Files changed (1)

 
 
 #define SERVO_ENABLE		0xD7
-#define SERVO_REF_POS		0xB5
 
-#define SERVO_QUAD_X		0xD0
-#define SERVO_QUAD_Y		0xF0
-//#define SERVO_QUAD_INDEX	0xF5 // not used yet
+#define SERVO_AN_1		0xB5
+#define SERVO_QX_1		0xD0
+#define SERVO_QY_1		0xF0
+
+#define SERVO_AN_2		0xB6
+#define SERVO_QX_2		0xD1
+#define SERVO_QY_2		0xF1
+
+#define SERVO_AN_3		0xB7
+#define SERVO_QX_3		0xD2
+#define SERVO_QY_3		0xF2
+
+#define SERVO_AN_4		0xC6
+#define SERVO_QX_4		0xD3
+#define SERVO_QY_4		0xF3
+
+#define SERVO_AN_5		0xC5
+#define SERVO_QX_5		0xE7
+#define SERVO_QY_5		0xF4
+
+#define SERVO_AN_6		0xC4
+#define SERVO_QX_6		0xE6
+#define SERVO_QY_6		0xF5
 
 #define LED			0xD6
 
 
 void
 output_drive(
+	uint8_t axis,
 	int8_t val
 )
 {
 	}
 #endif
 
-	OCR1A = output = val*4 + 512; // offset from EEPROM instead?
+	const uint16_t output = val * 4 + 512;
+
+	if (axis == 0)
+		OCR1A = output;
+	else
+	if (axis == 1)
+		OCR1B = output;
+	else
+	if (axis == 2)
+		OCR1C = output;
+	else
+	if (axis == 3)
+		OCR3A = output;
+	else
+	if (axis == 4)
+		OCR3B = output;
+	else
+	if (axis == 5)
+		OCR3C = output;
 }
 
 
 		// pull the enable down to ground; output is already 0
 		ddr(SERVO_ENABLE, 1);
 		led(1);
-	} else {
-		// let the enable float and bring the ref to neutral
-		// \todo: flash the led
-		ddr(SERVO_ENABLE, 0);
-		output_drive(0);
-		led(0);
+		return;
 	}
+
+	// let the enable float and bring the ref to neutral
+	ddr(SERVO_ENABLE, 0);
+	for (int i = 0 ; i < 6 ; i++)
+		output_drive(i, 0);
+	led(0);
 }
 
 
-static int16_t position;
-static uint8_t index_count;
+/** Position counters for each axis */
+static int16_t positions[6];
 
 void
 quad_decode(void)
 {
-#if 0
-	static uint8_t old_index;
-	if (in(SERVO_QUAD_INDEX))
-	{
-		if (old_index == 0)
-			index_count++;
-		old_index = 1;
-	} else {
-		old_index = 0;
-	}
-#endif
-
 	static uint8_t old_x;
-	uint8_t x = in(SERVO_QUAD_X) ? 1 : 0;
+	uint8_t x = in(SERVO_QX_1) ? 1 : 0;
 	if (x == old_x)
 		return;
 
 	old_x = x;
-	uint8_t y = in(SERVO_QUAD_Y) ? 1 : 0;
-
-#if 0
-	char buf[] = {
-		x ? '1' : '0',
-		y ? '1' : '0',
-		'\r',
-		'\n',
-	};
-	usb_serial_write(buf, sizeof(buf));
-#endif
+	int8_t y = in(SERVO_QY_1) ? 1 : 0;
 
 	uint8_t dir = x ^ y;
 	if (dir)
-		position++;
+		positions[0]++;
 	else
-		position--;
+		positions[0]--;
+}
+
+
+static uint8_t
+print_position(
+	char * buf,
+	int16_t p_in
+)
+{
+	uint16_t p = p_in;
+	if (p == 0)
+	{
+		*buf++ = ' ';
+	} else
+	if (p > 0)
+	{
+		*buf++ = '+';
+	} else {
+		p = -p_in;
+		*buf++ = '-';
+	}
+
+	*buf++ = hexdigit(p >> 12);
+	*buf++ = hexdigit(p >>  8);
+	*buf++ = hexdigit(p >>  4);
+	*buf++ = hexdigit(p >>  0);
+
+	return 5;
 }
 
 
 	sbi(TCCR1A, COM1A1);
 	cbi(TCCR1A, COM1A0);
 
+	sbi(TCCR1A, COM1B1);
+	cbi(TCCR1A, COM1B0);
+
+	sbi(TCCR1A, COM1C1);
+	cbi(TCCR1A, COM1C0);
+
 	// Configure clock 3 at clk/1
 	cbi(TCCR1B, CS12);
 	cbi(TCCR1B, CS11);
 	sbi(TCCR3A, COM3A1);
 	cbi(TCCR3A, COM3A0);
 
+	sbi(TCCR3A, COM3B1);
+	cbi(TCCR3A, COM3B0);
+
+	sbi(TCCR3A, COM3C1);
+	cbi(TCCR3A, COM3C0);
+
 	// Configure clock 3 at clk/1
 	cbi(TCCR3B, CS32);
 	cbi(TCCR3B, CS31);
 	ddr(SERVO_ENABLE, 0); // not enabled
 	out(SERVO_ENABLE, 0);
 
-	// And configure the analog reference as output, pull down on both
-	// This way they are both at zero
-	//ddr(SERVO_REF_NEG, 0); // leave as Hi-Z
-	//out(SERVO_REF_NEG, 0); // leave as Hi-Z
+	ddr(SERVO_AN_1, 1);
+	ddr(SERVO_AN_2, 1);
+	ddr(SERVO_AN_3, 1);
+	ddr(SERVO_AN_4, 1);
+	ddr(SERVO_AN_5, 1);
+	ddr(SERVO_AN_6, 1);
 
-	ddr(SERVO_REF_POS, 1);
-	out(SERVO_REF_POS, 0);
+	out(SERVO_AN_1, 0);
+	out(SERVO_AN_2, 0);
+	out(SERVO_AN_3, 0);
+	out(SERVO_AN_4, 0);
+	out(SERVO_AN_5, 0);
+	out(SERVO_AN_6, 0);
 
-	// Initial value is to have them all at ground potential
-	output_drive(0);
+	// Initial value is to have them all at 512
+	// Enable == 0 will set them to a "0" output.
 	output_enable(0);
 
 	// Configure the quadrature decoder.
 	// This should be interrupt  driven instead of polled to
-	// avoid missing counts, but that will require a cusotm board
-	ddr(SERVO_QUAD_X, 0);
-	ddr(SERVO_QUAD_Y, 0);
-	//ddr(SERVO_QUAD_INDEX, 0);
-	out(SERVO_QUAD_X, 1);
-	out(SERVO_QUAD_Y, 1);
-	//out(SERVO_QUAD_INDEX, 1);
+	// avoid missing counts.
+	ddr(SERVO_QX_1, 0);
+	ddr(SERVO_QX_2, 0);
+	ddr(SERVO_QX_3, 0);
+	ddr(SERVO_QX_4, 0);
+	ddr(SERVO_QX_5, 0);
+	ddr(SERVO_QX_6, 0);
+
+	ddr(SERVO_QY_1, 0);
+	ddr(SERVO_QY_2, 0);
+	ddr(SERVO_QY_3, 0);
+	ddr(SERVO_QY_4, 0);
+	ddr(SERVO_QY_5, 0);
+	ddr(SERVO_QY_6, 0);
+
+	out(SERVO_QX_1, 1);
+	out(SERVO_QX_2, 1);
+	out(SERVO_QX_3, 1);
+	out(SERVO_QX_4, 1);
+	out(SERVO_QX_5, 1);
+	out(SERVO_QX_6, 1);
+
+	out(SERVO_QY_1, 1);
+	out(SERVO_QY_2, 1);
+	out(SERVO_QY_3, 1);
+	out(SERVO_QY_4, 1);
+	out(SERVO_QY_5, 1);
+	out(SERVO_QY_6, 1);
 
 	// ADC0 is used to track the current measurement
 	// but may not be working yet?
 			sbi(ADCSRA, ADIF);
 			sbi(ADCSRA, ADSC);
 
-			char buf[32];
+			char buf[80];
 			uint8_t off = 0;
-			buf[off++] = hexdigit(output >> 8);
-			buf[off++] = hexdigit(output >> 4);
-			buf[off++] = hexdigit(output >> 0);
+
+			off += print_position(buf+off, positions[0]);
 			buf[off++] = ' ';
+			off += print_position(buf+off, positions[1]);
+			buf[off++] = ' ';
+			off += print_position(buf+off, positions[2]);
+			buf[off++] = ' ';
+			off += print_position(buf+off, positions[3]);
+			buf[off++] = ' ';
+			off += print_position(buf+off, positions[4]);
+			buf[off++] = ' ';
+			off += print_position(buf+off, positions[5]);
 
-			uint16_t p = position;
-			if (p == 0)
-			{
-				buf[off++] = ' ';
-			} else
-			if (p > 0)
-			{
-				buf[off++] = '+';
-			} else {
-				p = -p;
-				buf[off++] = '-';
-			}
-
-			buf[off++] = hexdigit(p >> 12);
-			buf[off++] = hexdigit(p >>  8);
-			buf[off++] = hexdigit(p >>  4);
-			buf[off++] = hexdigit(p >>  0);
-			buf[off++] = ' ';
-			buf[off++] = hexdigit(index_count >> 12);
-			buf[off++] = hexdigit(index_count >>  8);
-			buf[off++] = hexdigit(index_count >>  4);
 			buf[off++] = '\r';
 			buf[off++] = '\n';
+
 			usb_serial_write(buf, off);
 			continue;
 		}
 		if (c == ' ')
 		{
 			// stop everything and disable the output
-			output_drive(0);
 			output_enable(0);
 			val = 0;
 			sign = 1;
 		if (c == '\r')
 		{
 			send_str(PSTR("+\r\n"));
-			output_drive(sign ? val : -val);
+			output_drive(1, sign ? val : -val);
 			val = 0;
 			sign = 1;
 			continue;