1. Trammell Hudson
  2. seven

Commits

Trammell Hudson  committed 4358f08 Draft

quad decoders on each axis using interrupts

  • Participants
  • Parent commits 3218c70
  • Branches default

Comments (0)

Files changed (1)

File servo.c

View file
 /** Position counters for each axis */
 static int16_t positions[6];
 
-void
-quad_decode(void)
+static inline void
+quad_decode(
+	const uint8_t axis,
+	const uint8_t x_port,
+	const uint8_t y_port
+)
 {
-	static uint8_t old_x;
-	uint8_t x = in(SERVO_QX_1) ? 1 : 0;
-	if (x == old_x)
-		return;
+	const uint8_t x = in(x_port) ? 1 : 0;
+	const uint8_t y = in(y_port) ? 1 : 0;
+	const uint8_t dir = x ^ y;
 
-	old_x = x;
-	int8_t y = in(SERVO_QY_1) ? 1 : 0;
-
-	uint8_t dir = x ^ y;
-	if (dir)
-		positions[0]++;
-	else
-		positions[0]--;
+	positions[axis] += dir ? 1 : -1;
 }
 
 
+ISR(INT0_vect)
+{
+	quad_decode(0, SERVO_QX_1, SERVO_QY_1);
+}
+
+ISR(INT1_vect)
+{
+	quad_decode(1, SERVO_QX_2, SERVO_QY_2);
+}
+
+ISR(INT2_vect)
+{
+	quad_decode(2, SERVO_QX_3, SERVO_QY_3);
+}
+
 static uint8_t
 print_position(
 	char * buf,
 	output_enable(0);
 
 	// Configure the quadrature decoder.
-	// This should be interrupt  driven instead of polled to
-	// avoid missing counts.
+	// Pull ups on all inputs to avoid spurious noise.
+	// Interrupts are enabled for any edge on each of the X inputs.
 	ddr(SERVO_QX_1, 0);
 	ddr(SERVO_QX_2, 0);
 	ddr(SERVO_QX_3, 0);
 	out(SERVO_QY_5, 1);
 	out(SERVO_QY_6, 1);
 
+	cbi(EICRA, ISC01); // 01 == any edge
+	sbi(EICRA, ISC00);
+	sbi(EIMSK, INT0);
+
+	cbi(EICRA, ISC11); // 01 == any edge
+	sbi(EICRA, ISC10);
+	sbi(EIMSK, INT1);
+
+	cbi(EICRA, ISC21); // 01 == any edge
+	sbi(EICRA, ISC20);
+	sbi(EIMSK, INT2);
+	
+
 	// ADC0 is used to track the current measurement
 	// but may not be working yet?
 	ADMUX = (1 << REFS0);
 		int c = usb_serial_getchar();
 		if (c == -1)
 		{
-			quad_decode();
-
 			if (bit_is_clear(ADCSRA, ADIF))
 				continue;
+
 			// current measurement is ready
 			uint16_t val = ADC;
 			sbi(ADCSRA, ADIF);