Commits

Trammell Hudson  committed 39ac921 Draft

Switch to a PWM output to avoid using resistors

  • Participants
  • Parent commits 2fd74e9

Comments (0)

Files changed (3)

 
 
 # Target file name (without extension).
-TARGET = servo
+TARGET = seven
 
 
 # List C source files here. (C dependencies are automatically generated.)

File servo.c

-/**
- * \file Servo driver.
- *
- * Generates a differential signal for controlling a servo motor.
- * Each teensy++ can control six servos.
- *
- * Gnd		White
- * Enable	Gray    D7 D7 D7 D7 D7 D7
- * Ref-		Purple
- * Ref+		Blue	B5 B6 B7 C6 C5 C4
- * Qx		Green	D0 D1 D2 D3 E7 E6
- * Qy		Yellow	F0 F1 F2 F3 F4 F5
- *
- * Joint #4:
- * Quadrature appers be about 30000 counts
- * Index counter has 4 counts at roughly 120 degrees
- *
- * Big plug Pinout:
- * J1+ == A_
- * J1- == J_ (or K_?)
- *
- * Feedback pinout:
- * Harness     Funky male (db style pin count)
- * DB9.9        1 black (gnd, only in channel 1)
- *              2 NC
- * 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; 4 counts
- *             10
- *             11
- *             12
- *             13
- *             14
- *             15 motor -
- * DB9.shield  NC
- *
- * Servo amp pinout:
- *
- * 1 GND		GND
- * 2 GND		PB0 (leave as Hi-Z)
- * 3 /RESET		PB1 (leave as Hi-Z)
- * 4 /NEG ENABLE	PB2
- * 5 /POS ENABLE	PB3
- * 6 /ENABLE		PB7
- * 7 REV ENAB POL	PD0
- * 8 +NORMAL		PD1
- * 9 CURR MON		PD2
- * 10 REF-		PD3 (leave as Hi-Z, Vcc/2 voltage divider)
- * 11 REF+		PC6 (OC3A)
- * 12 GND		PC7 (leave as Hi-Z)
- * 13 +15V (nc)
- * 14 +5V (nc)
- */
-
-#include <avr/io.h>
-#include <avr/pgmspace.h>
-#include <avr/interrupt.h>
-#include <stdint.h>
-#include <string.h>
-#include <util/delay.h>
-#include "usb_serial.h"
-#include "bits.h"
-
-
-#define SERVO_ENABLE		0xD7
-
-#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 send_str(const char *s);
-uint8_t recv_str(char *buf, uint8_t size);
-void parse_and_execute_command(const char *buf, uint8_t num);
-
-static uint8_t
-hexdigit(
-	uint8_t x
-)
-{
-	x &= 0xF;
-	if (x < 0xA)
-		return x + '0' - 0x0;
-	else
-		return x + 'A' - 0xA;
-}
-
-
-
-// Send a string to the USB serial port.  The string must be in
-// flash memory, using PSTR
-//
-void send_str(const char *s)
-{
-	char c;
-	while (1) {
-		c = pgm_read_byte(s++);
-		if (!c) break;
-		usb_serial_putchar(c);
-	}
-}
-
-
-static void
-led(
-	uint8_t val
-)
-{
-	if (val)
-		out(LED, 1);
-	else
-		out(LED, 0);
-}
-
-
-void
-output_drive(
-	uint8_t axis,
-	int8_t val
-)
-{
-#if 0
-	// If the sign is changing, set output to zero and wait for
-	// it to change.
-	uint8_t cur_sign = !in(SERVO_REF_NEG);
-
-	if (cur_sign && val < 0)
-	{
-		OCR1A = 0;
-		_delay_ms(10);
-	} else
-	if (!cur_sign && val >= 0)
-	{
-		OCR1A = 1023;
-		_delay_ms(10);
-	}
-#endif
-
-	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;
-}
-
-
-static uint8_t output_enabled;
-
-void
-output_enable(
-	uint8_t val
-)
-{
-	output_enabled = val;
-
-	// if disabling, let the enable pin float.  Otherwise it will
-	// be pulled to ground.
-	ddr(SERVO_ENABLE, val);
-
-	// Generate zero-drive commands, always, just in case
-	for (int i = 0 ; i < 6 ; i++)
-		output_drive(i, 0);
-
-	// Indicate our drive status on the LED.
-	led(val);
-}
-
-
-/** Position counters for each axis */
-typedef struct
-{
-	int16_t position; // must access with interrupts locked
-	int16_t last_position;
-	int8_t velocity;
-	int16_t command;
-} servo_t;
-
-static servo_t servos[6];
-
-static int16_t
-servo_position(
-	uint8_t i
-)
-{
-	cli();
-	int16_t p = servos[i].position;
-	sei();
-	return p;
-}
-
-
-static inline void
-quad_decode(
-	const uint8_t axis,
-	const uint8_t x_port,
-	const uint8_t y_port
-)
-{
-	const uint8_t x = in(x_port) ? 1 : 0;
-	const uint8_t y = in(y_port) ? 1 : 0;
-	const uint8_t dir = x ^ y;
-
-	servos[axis].position += 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,
-	int16_t p_in
-)
-{
-	int16_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;
-}
-
-
-static void
-position_command(
-	uint8_t i,
-	int16_t command
-)
-{
-	servos[i].command = command;
-}
-
-
-static void
-servo_loop(
-	const uint8_t id
-)
-{
-	servo_t * const servo = &servos[id];
-
-	const int p_gain =  8;
-	const int d_gain = 64;
-
-	int16_t position = servo_position(id);
-
-	int16_t error = servo->command - position;
-	if (error < -300)
-		error = -300;
-	else
-	if (error > +300)
-		error = +300;
-
-	int16_t v_instant = position - servo->last_position;;
-	const int SCALE = 7;
-	int16_t velocity = servo->velocity = (servo->velocity * SCALE + v_instant) / (SCALE+1);
-	servo->last_position = position;
-
-	int16_t output = (error * p_gain) / 256 - (velocity * d_gain) / 256;
-;
-	if (output < -127)
-		output = -127;
-	else
-	if (output > 127)
-		output = 127;
-		
-	if (output_enabled)
-		output_drive(id, output);
-}
-
-
-static void
-position_loop(void)
-{
-	for (int i = 0 ; i < 6 ; i++)
-		servo_loop(i);
-}
-
-
-static void
-position_output(
-	const uint8_t sel_id
-)
-{
-	char buf[80];
-	uint8_t off = 0;
-
-	buf[off++] = hexdigit(sel_id);
-
-	for (int i = 0 ; i < 3 ; i++)
-	{
-		buf[off++] = ' ';
-		off += print_position(buf+off, servo_position(i));
-		off += print_position(buf+off, servos[i].velocity);
-	}
-
-	buf[off++] = '\r';
-	buf[off++] = '\n';
-
-	usb_serial_write(buf, off);
-}
-
-
-int
-main(void)
-{
-	// set for 16 MHz clock
-#define CPU_PRESCALE(n) (CLKPR = 0x80, CLKPR = (n))
-	CPU_PRESCALE(0);
-
-	// Disable the ADC
-	ADMUX = 0;
-
-	// initialize the USB, and then wait for the host
-	// to set configuration.  If the Teensy is powered
-	// without a PC connected to the USB port, this 
-	// will wait forever.
-	usb_init();
-
-	// LED is an output; will be pulled down once connected
-	ddr(LED, 1);
-	out(LED, 1);
-
-	// Timer 0 is used for a 64 Hz control loop timer.
-	// Clk/1024 == 15.625 KHz, count up to 244
-	// CTC mode resets the counter when it hits the top
-	TCCR0A = 0
-		| 1 << WGM01 // select CTC
-		| 0 << WGM00
-		;
-
-	TCCR0B = 0
-		| 0 << WGM02
-		| 1 << CS02 // select Clk/1024
-		| 0 << CS01
-		| 0 << CS00
-		;
-
-	OCR0A = 244;
-	sbi(TIFR0, OCF0A); // reset the overflow bit
-
-	// Configure OC1x in fast-PWM mode, 10-bit
-	// REF+ is OC1A
-	sbi(TCCR1B, WGM12);
-	sbi(TCCR1A, WGM11);
-	sbi(TCCR1A, WGM10);
-
-	// Configure output mode to clear on match, set at top
-	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(TCCR1B, CS10);
-
-
-	// Configure OC3x in fast-PWM mode, 10-bit
-	// REF+ is OC3A
-	sbi(TCCR3B, WGM32);
-	sbi(TCCR3A, WGM31);
-	sbi(TCCR3A, WGM30);
-
-	// Configure output mode to clear on match, set at top
-	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);
-	sbi(TCCR3B, CS30);
-
-
-	// Configure all of the enables.
-	// POS and NEG enable are both outputs, driven low.
-	// The final enable is left floating when disabled.  When enabled
-	// it will also be driven low.
-	ddr(SERVO_ENABLE, 0); // not enabled
-	out(SERVO_ENABLE, 0);
-
-	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);
-
-	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 512
-	// Enable == 0 will set them to a "0" output.
-	output_enable(0);
-
-	// Configure the quadrature decoder.
-	// 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);
-	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);
-
-	// try turning off pullups
-	out(SERVO_QX_1, 0);
-	out(SERVO_QX_2, 0);
-	out(SERVO_QX_3, 0);
-	out(SERVO_QX_4, 0);
-	out(SERVO_QX_5, 0);
-	out(SERVO_QX_6, 0);
-
-	out(SERVO_QY_1, 0);
-	out(SERVO_QY_2, 0);
-	out(SERVO_QY_3, 0);
-	out(SERVO_QY_4, 0);
-	out(SERVO_QY_5, 0);
-	out(SERVO_QY_6, 0);
-
-	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);
-	ADCSRA = 0
-		| (1 << ADEN)
-		| (1 << ADSC)
-		| (1 << ADPS2)
-		| (1 << ADPS1)
-		| (1 << ADPS0)
-		;
-
-	while (!usb_configured())
-		;
-
-	_delay_ms(1000);
-
-	// wait for the user to run their terminal emulator program
-	// which sets DTR to indicate it is ready to receive.
-	while (!(usb_serial_get_control() & USB_SERIAL_DTR))
-		;
-
-	// discard anything that was received prior.  Sometimes the
-	// operating system or other software will send a modem
-	// "AT command", which can still be buffered.
-	usb_serial_flush_input();
-
-	send_str(PSTR("servo driver\r\n"));
-	uint16_t val = 0;
-	uint8_t sign = 1;
-	uint8_t phase = 0;
-	uint8_t servo_id = 1;
-
-	while (1)
-	{
-		if (bit_is_set(TIFR0, OCF0A))
-		{
-			sbi(TIFR0, OCF0A); // reset the bit
-			position_loop();
-			if ((phase++ & 0xF) == 0)
-				position_output(servo_id);
-		}
-
-		int c = usb_serial_getchar();
-		if (c == -1)
-		{
-			continue;
-		}
-
-		if (c == '!')
-		{
-			// copy the current positions into the commands
-			// this avoids any jumps
-			for (int i = 0 ;i < 6 ; i++)
-			{
-				position_command(i, servo_position(i));
-				output_drive(i, 0);
-			}
-
-			output_enable(1);
-			continue;
-		}
-
-		if (c == ' ')
-		{
-			// stop everything and disable the output
-			output_enable(0);
-			val = 0;
-			sign = 1;
-			continue;
-		}
-
-		if (c == '-')
-		{
-			sign = 0;
-			continue;
-		}
-
-		if ('0' <= c && c <= '9')
-		{
-			val = (val << 4) | (c - '0' + 0x0);
-			continue;
-		}
-
-		if ('a' <= c && c <= 'f')
-		{
-			val = (val << 4) | (c - 'a' + 0xA);
-			continue;
-		}
-
-		if (c == '\n')
-			continue;
-
-		if (c == '=')
-		{
-			servo_id = val & 7;
-			val = 0;
-			sign = 1;
-			continue;
-		}
-
-		if (c == '\r')
-		{
-			send_str(PSTR("+\r\n"));
-			position_command(servo_id, sign ? val : -val);
-			val = 0;
-			sign = 1;
-			continue;
-		}
-
-		send_str(PSTR("?\r\n"));
-		val = 0;
-	}
-}
+/**
+ * \file Seven segment driver
+ *
+ */
+
+#include <avr/io.h>
+#include <avr/pgmspace.h>
+#include <avr/interrupt.h>
+#include <stdint.h>
+#include <string.h>
+#include <util/delay.h>
+#include "usb_serial.h"
+#include "bits.h"
+
+
+#define LED			0xD6
+
+
+void send_str(const char *s);
+uint8_t recv_str(char *buf, uint8_t size);
+void parse_and_execute_command(const char *buf, uint8_t num);
+
+static uint8_t
+hexdigit(
+	uint8_t x
+)
+{
+	x &= 0xF;
+	if (x < 0xA)
+		return x + '0' - 0x0;
+	else
+		return x + 'A' - 0xA;
+}
+
+
+
+// Send a string to the USB serial port.  The string must be in
+// flash memory, using PSTR
+//
+void send_str(const char *s)
+{
+	char c;
+	while (1) {
+		c = pgm_read_byte(s++);
+		if (!c) break;
+		usb_serial_putchar(c);
+	}
+}
+
+
+static void
+led(
+	uint8_t val
+)
+{
+	if (val)
+		out(LED, 1);
+	else
+		out(LED, 0);
+}
+
+
+/*  4
+ * 5 3
+ *  7
+ * 0 2
+ *  1 
+ *
+ * 6 is not used since D6 drives the onboard LED.
+ */
+static uint8_t digits[] = {
+	1 << 3 | 1 << 1 | 1 << 0 | 1 << 5 | 1 << 4 | 1 << 2, // 0
+	1 << 3 | 1 << 2, // 1
+	1 << 4 | 1 << 1 | 1 << 7 | 1 << 0 | 1 << 3, // 2
+	1 << 4 | 1 << 2 | 1 << 7 | 1 << 3 | 1 << 1, // 3
+	1 << 5 | 1 << 7 | 1 << 2 | 1 << 3, // 4
+	1 << 4 | 1 << 5 | 1 << 7 | 1 << 2 | 1 << 1, // 5
+	1 << 4 | 1 << 0 | 1 << 7 | 1 << 2 | 1 << 5 | 1 << 1, // 6
+	1 << 4 | 1 << 2 | 1 << 3, // 7
+	1 << 3 | 1 << 1 | 1 << 0 | 1 << 5 | 1 << 4 | 1 << 2 | 1 << 7, // 8
+	1 << 5 | 1 << 7 | 1 << 2 | 1 << 3 | 1 << 4, // 9
+	1 << 0, // chase
+	1 << 1, // chase
+	1 << 2, // chase
+	1 << 3, // chase
+	1 << 4, // chase
+	1 << 5, // chase
+};
+
+
+
+static void
+chase(
+	uint8_t count,
+	uint8_t delay
+)
+{
+	for (uint8_t i = 0 ; i < count ; i++)
+	{
+		uint8_t mask = 1;
+		for (uint8_t j = 0 ; j < 6 ; j++, mask <<= 1)
+		{
+			DDRC = mask;
+			_delay_ms(delay);
+		}
+	}
+}
+
+
+/** "Frame buffer" of output LEDs.
+ * Since we can't drive all LEDs simultaneously, instead draw them from
+ * this frame buffer.
+ */
+static uint8_t outputs[4];
+
+
+static void
+draw(void)
+{
+	uint8_t mask = 3;
+
+	for (uint8_t i = 0 ; i < 4 ; i++, mask <<= 2)
+	{
+		// skip output 6
+		//if (i == 6)
+			////mask <<= 1;
+
+		DDRB = outputs[0] & mask;
+		DDRC = outputs[1] & mask;
+		DDRD = outputs[2] & mask;
+		DDRF = outputs[3] & mask;
+		_delay_us(10);
+	}
+
+	DDRB = 0;
+	DDRC = 0;
+	DDRD = 0;
+	DDRF = 0;
+}
+
+
+int
+main(void)
+{
+	// set for 16 MHz clock
+#define CPU_PRESCALE(n) (CLKPR = 0x80, CLKPR = (n))
+	CPU_PRESCALE(0);
+
+	// Disable the ADC
+	ADMUX = 0;
+
+	// initialize the USB, and then wait for the host
+	// to set configuration.  If the Teensy is powered
+	// without a PC connected to the USB port, this 
+	// will wait forever.
+	usb_init();
+
+	// LED is an output; will be pulled down once connected
+	ddr(LED, 1);
+	out(LED, 1);
+
+	while (!usb_configured())
+		;
+
+	_delay_ms(1000);
+
+	// wait for the user to run their terminal emulator program
+	// which sets DTR to indicate it is ready to receive.
+	while (!(usb_serial_get_control() & USB_SERIAL_DTR))
+		;
+
+	// discard anything that was received prior.  Sometimes the
+	// operating system or other software will send a modem
+	// "AT command", which can still be buffered.
+	usb_serial_flush_input();
+
+	send_str(PSTR("seven segment\r\n"));
+	uint8_t val = 9;
+	DDRC = 1 << 7;
+	PORTC = 0;
+
+
+	while (1)
+	{
+		int c = usb_serial_getchar();
+		if (c != -1)
+		{
+			continue;
+		}
+
+		if (val == 10)
+		{
+			//chase(4, 100);
+			val = 0;
+		}
+
+		const uint8_t output = digits[val++];
+		outputs[0] = outputs[1] = outputs[2] = outputs[3] = output;
+
+		for (uint16_t i = 0 ; i < 20000 ; i++)
+			draw();
+	}
+}