From 2b427f4bdfc71b055b7db8ffe2b9dd0ea3e4cc9a Mon Sep 17 00:00:00 2001
From: Sebastian Reichel <sre@ring0.de>
Date: Sat, 11 Aug 2012 23:02:47 +0200
Subject: initial commit

---
 firmware/firmware.c | 170 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 170 insertions(+)
 create mode 100644 firmware/firmware.c

(limited to 'firmware/firmware.c')

diff --git a/firmware/firmware.c b/firmware/firmware.c
new file mode 100644
index 0000000..9a04d41
--- /dev/null
+++ b/firmware/firmware.c
@@ -0,0 +1,170 @@
+#include <avr/io.h>
+#include <util/delay.h>
+#include <util/delay.h>
+#include <avr/interrupt.h>
+
+/* 500Hz -> 2ms */
+#define MAXPULSFREQ         500
+
+/* timer1 value for a 2ms puls */
+#define TIMER_MAXPULS       F_CPU/MAXPULSFREQ
+
+/* min/max values */
+#define MINPULS 13500
+#define MAXPULS 30000
+
+/* baud rate for serial communication */
+#define BAUD 9600UL
+
+/* hardware settings for baud rate */
+#define UBRR_VAL ((F_CPU+BAUD*8)/(BAUD*16)-1)
+#define BAUD_REAL (F_CPU/(16*(UBRR_VAL+1)))
+#define BAUD_ERROR ((BAUD_REAL*1000)/BAUD)
+ 
+/* compile time check if baud rate is ok */
+#if ((BAUD_ERROR<990) || (BAUD_ERROR>1010))
+  #error systematic error of baud rate is > 1%
+#endif
+
+
+static uint16_t servo_pulslength[20];
+static uint8_t activated = 0;
+
+/* init IO pins */
+static inline void init_io() {
+	/* PB1 = left:red, PB2 = left:green, PB3 = left:blue */
+	DDRB  = (1 << PB1) | (1 << PB2) | (1 << PB3);
+
+	/* PD3 = right:red, PD5 = right:green, PD6 = right:blue */
+	DDRD  = (1 << PD3) | (1 << PD5) | (1 << PD6);
+
+	/* These pins are broken, so enable them */
+	DDRC = (1 << PC4) | (1 << PC5) | (1 << PC6);
+	PORTC = (1 << PC4) | (1 << PC5) | (1 << PC6);
+}
+
+/* INTERRUPT: timer for software pwm to control servos */
+ISR(TIMER1_OVF_vect) {
+	static uint8_t servo_indexhalf = 0;
+
+	switch(servo_indexhalf) {
+		case 0 : PORTB |=  (1 << PB1); break;
+		case 1 : PORTB &= ~(1 << PB1); break;
+		case 2 : PORTB |=  (1 << PB2); break;
+		case 3 : PORTB &= ~(1 << PB2); break;
+		case 4 : PORTB |=  (1 << PB3); break;
+		case 5 : PORTB &= ~(1 << PB3); break;
+		case 6 : PORTD |=  (1 << PD3); break;
+		case 7 : PORTD &= ~(1 << PD3); break;
+		case 8 : PORTD |=  (1 << PD5); break;
+		case 9 : PORTD &= ~(1 << PD5); break;
+		case 10: PORTD |=  (1 << PD6); break;
+		case 11: PORTD &= ~(1 << PD6); break;
+	}
+
+	/* set time for the next interrupt */
+	TCNT1 = servo_pulslength[servo_indexhalf];
+
+	servo_indexhalf++;
+
+	if(servo_indexhalf == 20)
+		servo_indexhalf = 0;
+}
+
+/* setup serial communication */
+static inline void init_uart() {
+	UBRR0   = UBRR_VAL;
+	UCSR0B |= (1 << TXEN0);
+	UCSR0B |= (1 << RXEN0);
+
+	/* Frame Format: Asynchron 8N1 */
+	UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
+}
+
+/* set servo speed */
+static inline void servo_set(uint8_t index, uint8_t value) {
+	if(!activated)
+		value = 0;
+
+	if (index < 6) {
+		uint16_t tmp = MINPULS+(MAXPULS-MINPULS)/256*value;
+		/* calculate hightime */
+		servo_pulslength[index<<1]=(-1)*tmp;
+		/* sum of low and hightime for one servo is 2ms */
+		servo_pulslength[(index<<1)+1] = (-1)*(TIMER_MAXPULS-tmp);
+	}
+}
+
+/* init servos, reset speed */
+static inline void init_servo() {
+	uint8_t  i;
+
+	/* disable all motors */
+	for(i=0;i<6;i++)
+		servo_set(i, 0);
+
+	/* init timer */
+	TCNT1   = 0 - 16000;
+	TCCR1A  = 0;
+	TCCR1B  = (1 << CS10);
+	TIMSK1 |= (1 << TOIE1);
+}
+
+/* get a single byte from serial device */
+static inline uint8_t uart_getc() {
+	while (!(UCSR0A & (1<<RXC0)));
+	return UDR0;
+}
+
+/* put a single byte to serial device */
+static inline void uart_putc(uint8_t c) {
+    while (!(UCSR0A & (1<<UDRE0)));
+    UDR0 = c;
+}
+
+int main() {
+	/* initialize hardware */
+	init_io();
+	init_servo();
+	init_uart();
+	sei();
+
+	uint8_t c;
+
+	while(1) {
+		c = uart_getc();
+
+		/* set speed for single engine */
+		if(c >= 0x01 && c <= 0x06) {
+			uint8_t speed = uart_getc();
+			servo_set(c-1, speed);
+		/* set speed for all engines */
+		} else if(c == 0xFF) {
+			uint8_t speed = uart_getc();
+			for(int i=0;i<6;i++)
+				servo_set(i, speed);
+		/* activate engines */
+		} else if(c == 0xCA) {
+			if(uart_getc() != 0xFE) continue;
+			if(uart_getc() != 0xBA) continue;
+			if(uart_getc() != 0xBE) continue;
+			activated=1;
+		/* disable engines */
+		} else if(c == 0xDE) {
+			if(uart_getc() != 0xAD) continue;
+			if(uart_getc() != 0xBA) continue;
+			if(uart_getc() != 0xBE) continue;
+			activated=0;
+			for(int i=0;i<6;i++)
+				servo_set(i, 0);
+		/* ignore unknown commands */
+		} else if(c != 0x00) {
+			continue;
+		}
+
+		/* acknowledge command */
+		uart_putc('.');
+	}
+
+	return 0;
+}
-- 
cgit v1.2.3