Commits

Trammell Hudson  committed 0007169 Draft

Track index counter as well

  • Participants
  • Parent commits 1cef848

Comments (0)

Files changed (1)

  *
  * Generates a differential signal for controlling a servo motor.
  * Each teensy can control one servo.
- * Quadrature on the PUMA appears to be about 30000 counts for joint #4.
+ *
+ * Joint #4:
+ * Quadrature appers be about 30000 counts
+ * Index counter has 4 counts at roughly 120 degrees
  *
  * Big plug Pinout:
  * J1+ == A_
  *
  * Feedback pinout:
  * Harness     Funky male (db style pin count)
- * DB9.9        1 black (only in channel 1)
+ * DB9.9        1 black (gnd, only in channel 1)
  *              2 NC
- * DB9.8        3 red (only in channel 1)
- * DB9.1        4 orange -- quadrature
+ * DB9.8        3 red (+5v, only in channel 1)
+ * DB9.1        4 orange -- quadrature; 32k counts
  * DB9.4        5 yellow -- quadrature
  *              6 NC
  *              7 NC
  *              8 motor +
- * DB9.6        9 grey -- index encoder
+ * DB9.6        9 grey -- index encoder; 4 counts
  *             10
  *             11
  *             12
 
 
 static int16_t position;
+static uint8_t index_count;
 
 void
 quad_decode(void)
 {
+	static uint8_t old_index;
+	if (in(SERVO_QUAD_INDEX))
+	{
+		if (old_index == 0)
+			index_count++;
+		old_index = 1;
+	} else {
+		old_index = 0;
+	}
+
 	static uint8_t old_x;
 	uint8_t x = in(SERVO_QUAD_X) ? 1 : 0;
 	if (x == old_x)
 	// 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);
 
 	// ADC0 is used to track the current measurement
 	// but may not be working yet?
 			sbi(ADCSRA, ADIF);
 			sbi(ADCSRA, ADSC);
 
-			char buf[16];
+			char buf[32];
 			uint8_t off = 0;
 			buf[off++] = hexdigit(output >> 8);
 			buf[off++] = hexdigit(output >> 4);
 			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);