+++ enhanced framework hardware interface

source/AVR_Studio/Soccer/Soccer.c

@@ -10,19 +10,29 @@ Sensor sensor;
 
 int main() {
 	//board.ledOn();
-	sleep(2);
+	sleep(1);
 	
 	uart1_puts("Hallo hier ist das RNMega128Funk-Board");
 
+	while(true)
+	{
+		uart1_puts("3");
+		sleep(1);
+	}
+	
+	int speed = 100;
 
-	int speed = 255;
+ 	/*for(int i=0;i<360;i+=5) {
-	board.motor(0, speed);
+		navigation.Aktualisieren(0,i,0);	
-	board.motor(1, speed);
+		msleep(1000);
-	board.motor(2, speed);
+	}*/
-
+	
-	sleep(2);
+	/*sleep(2);
-	navigation.Aktualisieren(180,0,speed);
+	board.motor(0,0);
-	while(true) { sleep(1); }
+	board.motor(1,0);
+	board.motor(2,0);
+	board.motor(3,0);
+	while(true) { sleep(1); }*/
 	/*board.motor(0,speed);
 	board.motor(2,speed);
 	sleep(2);
@@ -83,11 +93,16 @@ int main() {
 	while(true) {
 		navigation.Aktualisieren(0,0,speed);
 		sleep(2);
-		navigation.Aktualisieren(90,0,speed);
+		navigation.Aktualisieren(135,0,speed);
 		sleep(2);
 		navigation.Aktualisieren(180,0,speed);
 		sleep(2);
-		navigation.Aktualisieren(270,0,speed);
+		navigation.Aktualisieren(225,0,speed);
+		sleep(2);
+		board.motor(0,0);
+		board.motor(1,0);
+		board.motor(2,0);
+		board.motor(3,0);
 		sleep(2);
 	}
 	

source/AVR_Studio/Soccer/default/dep/Soccer.o.d

@@ -7,12 +7,9 @@ Soccer.o: ../Soccer.c ../global.h c:/winavr/bin/../avr/include/stdlib.h \
   c:/winavr/bin/../avr/include/avr/iom128.h \
   c:/winavr/bin/../avr/include/avr/portpins.h \
   c:/winavr/bin/../avr/include/avr/version.h \
-  c:/winavr/bin/../avr/include/avr/interrupt.h ../hal/uart.h \
+  c:/winavr/bin/../avr/include/avr/interrupt.h ../hal/../global.h \
-  ../hal/../global.h ../hal/keylcd.h ../hal/i2c.h \
+  ../hal/keylcd.h c:/winavr/bin/../avr/include/string.h ../hal/uart.h \
-  c:/winavr/bin/../avr/include/compat/twi.h \
+  ../navigation.h c:/winavr/bin/../avr/include/math.h ../sensor/sensor.h \
-  c:/winavr/bin/../avr/include/util/twi.h \
-  c:/winavr/bin/../avr/include/string.h ../navigation.h \
-  c:/winavr/bin/../avr/include/math.h ../sensor/sensor.h \
   ../sensor/../hal/board.h ../sensor/ballsensor.h ../sensor/position.h \
   ../sensor/../hal/maussensor.h ../sensor/../hal/../global.h \
   ../sensor/abstand.h ../sensor/../global.h
@@ -41,20 +38,14 @@ c:/winavr/bin/../avr/include/avr/version.h:
 
 c:/winavr/bin/../avr/include/avr/interrupt.h:
 
-../hal/uart.h:
-
 ../hal/../global.h:
 
 ../hal/keylcd.h:
 
-../hal/i2c.h:
-
-c:/winavr/bin/../avr/include/compat/twi.h:
-
-c:/winavr/bin/../avr/include/util/twi.h:
-
 c:/winavr/bin/../avr/include/string.h:
 
+../hal/uart.h:
+
 ../navigation.h:
 
 c:/winavr/bin/../avr/include/math.h:

source/AVR_Studio/Soccer/default/dep/board.o.d

@@ -8,8 +8,8 @@ board.o: ../hal/board.c ../hal/board.h \
   c:/winavr/bin/../avr/include/avr/version.h \
   c:/winavr/bin/../avr/include/avr/interrupt.h \
   c:/winavr/bin/../avr/include/stdlib.h \
-  c:\winavr\bin\../lib/gcc/avr/4.1.1/include/stddef.h ../hal/uart.h \
+  c:\winavr\bin\../lib/gcc/avr/4.1.1/include/stddef.h ../hal/../global.h \
-  ../hal/../global.h ../hal/../hal/board.h
+  ../hal/../hal/board.h
 
 ../hal/board.h:
 
@@ -33,8 +33,6 @@ c:/winavr/bin/../avr/include/stdlib.h:
 
 c:\winavr\bin\../lib/gcc/avr/4.1.1/include/stddef.h:
 
-../hal/uart.h:
-
 ../hal/../global.h:
 
 ../hal/../hal/board.h:

source/AVR_Studio/Soccer/default/dep/keylcd.o.d

@@ -1,52 +0,0 @@
-keylcd.o: ../hal/keylcd.c ../hal/keylcd.h ../hal/i2c.h ../hal/../global.h \
-  c:/winavr/bin/../avr/include/stdlib.h \
-  c:\winavr\bin\../lib/gcc/avr/4.1.1/include/stddef.h \
-  ../hal/../hal/board.h c:/winavr/bin/../avr/include/avr/io.h \
-  c:/winavr/bin/../avr/include/avr/sfr_defs.h \
-  c:/winavr/bin/../avr/include/inttypes.h \
-  c:/winavr/bin/../avr/include/stdint.h \
-  c:/winavr/bin/../avr/include/avr/iom128.h \
-  c:/winavr/bin/../avr/include/avr/portpins.h \
-  c:/winavr/bin/../avr/include/avr/version.h \
-  c:/winavr/bin/../avr/include/avr/interrupt.h ../hal/../hal/uart.h \
-  ../hal/../hal/../global.h c:/winavr/bin/../avr/include/compat/twi.h \
-  c:/winavr/bin/../avr/include/util/twi.h \
-  c:/winavr/bin/../avr/include/string.h
-
-../hal/keylcd.h:
-
-../hal/i2c.h:
-
-../hal/../global.h:
-
-c:/winavr/bin/../avr/include/stdlib.h:
-
-c:\winavr\bin\../lib/gcc/avr/4.1.1/include/stddef.h:
-
-../hal/../hal/board.h:
-
-c:/winavr/bin/../avr/include/avr/io.h:
-
-c:/winavr/bin/../avr/include/avr/sfr_defs.h:
-
-c:/winavr/bin/../avr/include/inttypes.h:
-
-c:/winavr/bin/../avr/include/stdint.h:
-
-c:/winavr/bin/../avr/include/avr/iom128.h:
-
-c:/winavr/bin/../avr/include/avr/portpins.h:
-
-c:/winavr/bin/../avr/include/avr/version.h:
-
-c:/winavr/bin/../avr/include/avr/interrupt.h:
-
-../hal/../hal/uart.h:
-
-../hal/../hal/../global.h:
-
-c:/winavr/bin/../avr/include/compat/twi.h:
-
-c:/winavr/bin/../avr/include/util/twi.h:
-
-c:/winavr/bin/../avr/include/string.h:

source/AVR_Studio/Soccer/hal/board.c

@@ -16,7 +16,7 @@ Board::Board() {
 	PORTC = 0; // Alle Low, kein Pollup
 
 	// Alle Ausgänge bis auf PD0+1(I2C) + 2+3(RS232)
-	DDRD = (1 << PD2) | (1 << PD3) | (1 << PD4) | (1 << PD5) | (1 << PD6)  | (1 << PD7);
+	DDRD = (1 << PD3) | (1 << PD4) | (1 << PD5) | (1 << PD6)  | (1 << PD7);
 	PORTD = (1 << PD0) | (1 << PD1); // Pollup-Widerstand an PD0+1 aktivieren
 
 	// PE5 ist eingang
@@ -33,11 +33,11 @@ Board::Board() {
 
 	// aktiviere Kanal A+B auf PWM1 mit 8Bit
 	TCCR1A = (1<< COM1A1) | (1<< COM1B1) | (1<< WGM10);
-	TCCR1B = (1<<ICNC1) | (1<<CS12) | (1<<CS10); // set clock/prescaler 1/1024 -> enable counter
+	TCCR1B = (1<<ICNC1) | (1<<CS11); // | (1<<CS12) | (1<<CS10); // set clock/prescaler 1/1024 -> enable counter
 
 	// aktiviere Kanal A+B auf PWM3 mit 8Bit
 	TCCR3A = (1<< COM3A1) | (1<< COM3B1) | (1<< WGM10);
-	TCCR3B =  (1<<ICNC3) | (1<<CS32) | (1<<CS30); // set clock/prescaler 1/1024 -> enable counter
+	TCCR3B =  (1<<ICNC3) | (1<<CS31); // | (1<<CS32) | (1<<CS30); // set clock/prescaler 1/1024 -> enable counter
 
 	// Schalte Motoren auf 0
 	motor(0,0);
@@ -49,8 +49,11 @@ Board::Board() {
 	PORTE &= ~(1 << PE6); // Pin2 low
 	PORTA |= (1 << PA2); // Pin1 high
 
-	// Uart-Interface einschalten
+	// Dribbler-richtung einstellen
-	uart1_init( 10); // 9600 BAUD bei 16MHz Atmel
+	DRIBBLER_PORT |= DRIBBLER_A; // Pin1 high
+	DRIBBLER_PORT &= ~DRIBBLER_B; // Pin2 low
+
+	
 
 	// aktiviere interrupt
 	sei();
@@ -202,27 +205,6 @@ void Board::motor(int i, int speed)
 			MOTOR2_PORT |= MOTOR2_A;//In 1 ein
 		}
 	}
-	else if(i == 3) // Dribbler... hier haben wir kein PWM
-	{
-		if(speed > 0)
-		{
-			DRIBBLER_PORT |= DRIBBLER_A;//In 1 ein
-			DRIBBLER_PORT &= ~DRIBBLER_B;//In 2 aus
-			DRIBBLER_PWMPORT |= DRIBBLER_PWM;
-		}
-		else if(speed < 0)
-		{
-			DRIBBLER_PORT |= DRIBBLER_B;//In 2 ein
-			DRIBBLER_PORT &= ~DRIBBLER_A;//In 1 aus
-			DRIBBLER_PWMPORT |= DRIBBLER_PWM;
-		}
-		else
-		{
-			DRIBBLER_PORT |= DRIBBLER_B;//In 2 ein
-			DRIBBLER_PORT |= DRIBBLER_A;//In 1 ein
-			DRIBBLER_PWMPORT &= ~DRIBBLER_PWM;
-		}
-	}
 }
 
 void Board::kicker() {
@@ -231,6 +213,19 @@ void Board::kicker() {
 	KICKER_AUS
 }
 
+void Board::dribblerOn() {
+	DRIBBLER_AN
+}
+
+void Board::dribblerOff() {
+	DRIBBLER_AUS
+}
+
+void Board::dribbler(bool status) {
+	if(status) dribblerOn();
+	else dribblerOff();
+}
+
 //PWM routine für den Beeper
 ISR (TIMER0_OVF_vect)
 {

source/AVR_Studio/Soccer/hal/board.h

@@ -4,7 +4,7 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <stdlib.h>
-#include "uart.h"
+//#include "uart.h"
 #include "../global.h"
 
 //#define abs(a) ((a < 0)? -a : a)
@@ -26,10 +26,12 @@
 #define MOTOR2_PWM OCR3A 
 #define MOTOR2_A (1 << 5)
 #define MOTOR2_B (1 << 4)
-
+
+
+// Dribbler
 #define DRIBBLER_PORT PORTD
-#define DRIBBLER_PWMPORT PORTA
+#define DRIBBLER_AN PORTA |= (1 << 5);
-#define DRIBBLER_PWM (1 << 5)
+#define DRIBBLER_AUS PORTA &= ~(1 << 5);
 #define DRIBBLER_A (1 << 6)
 #define DRIBBLER_B (1 << 7)
 
@@ -62,6 +64,10 @@ public:
 	void led(bool status);
 	void motor(int i, int speed);
 	void kicker();
+
+	void dribbler(bool status);
+	void dribblerOn();
+	void dribblerOff();
 };
 
 #endif

source/AVR_Studio/Soccer/hal/keylcd.c

@@ -1,7 +1,8 @@
 #include "keylcd.h"
 
-KeyLCD::KeyLCD() : i2c() {
+KeyLCD::KeyLCD() {
-
+	// Uart-Interface einschalten
+	uart1_init( 103); // 9600 BAUD bei 16MHz Atmel
 }
 
 KeyLCD::~KeyLCD() {
@@ -10,12 +11,7 @@ KeyLCD::~KeyLCD() {
 
 // Gibt Daten auf dem keyLCD aus
 void KeyLCD::print(char *data) {
-	int len=strlen(data);
+	uart1_puts(data);
-  uint8_t buf[len+1];
-
-  for (int i=0; i<len; i++) buf[i] = uint8_t(data[i]);
-
-  i2c.send(I2C_KEYLCD, buf, len);
 }
 
 // Löscht das keyLCD
@@ -23,6 +19,3 @@ void KeyLCD::clear() {
 	
 }
 
-uint8_t KeyLCD::error() {
-	return i2c.error();
-}

source/AVR_Studio/Soccer/hal/keylcd.h

@@ -1,22 +1,18 @@
 #ifndef _KEYLCD_H_
 #define _KEYLCD_H_
 
-#include "i2c.h"
+//#include "i2c.h"
 #include "string.h"
-
+#include "uart.h"
-#define I2C_KEYLCD 2
-#define LCD_CMD_PRINTSTR 0
 
 class KeyLCD 
 {
 private:
-	I2C i2c;
 public:
 	KeyLCD();
 	~KeyLCD();
 	
 	void print(char* data);
-	uint8_t error();
 	void clear();	
 };
 

source/AVR_Studio/Soccer/navigation.c

@@ -26,18 +26,18 @@ void Navigation::SetzeGeschwindigkeit(int nGeschwindigkeit) {
 
 
 // Aktualieren ohne Parameter
-void Navigation::Aktualisieren() {
+void Navigation::Aktualisieren() {	
 	// Richtung in x und y-Kompontente zerlegen
-	double y = cos(richtung); // richtung ist winkel
+	double y = cos((double)richtung*0.01745); // richtung ist winkel
-	double x = sin(richtung);
+	double x = sin((double)richtung*0.01745);
 	
 	// Abweichung der Ausrichtung ermitteln(als winkel)
 	int w = sensor.GetAusrichtung() - ausrichtung;
 
 	// Stärke der einzelnen Motoren berechnen	
-	double v0 = (-x-sqrt(3)*y)/2;
+	double v0 = (-x+sqrt(3)*y)/2;
 	double v1 = x;
-	double v2 = (-x+sqrt(3)*y)/2;
+	double v2 = (-x-sqrt(3)*y)/2;
 
 	// Ausgerechnete Stärke an die Motoren übergeben
 	board.motor(0,(int)((double)v0*geschwindigkeit +w));

source/Concept/Framework/atmega128io.cpp

@@ -1 +1,585 @@
-#include "atmega128io.h"
+#include "atmega128io.h"
+
+/*************************************************************************
+Title:    Interrupt UART library with receive/transmit circular buffers
+Author:   Peter Fleury <pfleury@gmx.ch>   http://jump.to/fleury
+File:     $Id: uart.c,v 1.5.2.10 2005/11/15 19:49:12 peter Exp $
+Software: AVR-GCC 3.3 
+Hardware: any AVR with built-in UART, 
+          tested on AT90S8515 at 4 Mhz and ATmega at 1Mhz
+
+DESCRIPTION:
+    An interrupt is generated when the UART has finished transmitting or
+    receiving a byte. The interrupt handling routines use circular buffers
+    for buffering received and transmitted data.
+    
+    The UART_RX_BUFFER_SIZE and UART_TX_BUFFER_SIZE variables define
+    the buffer size in bytes. Note that these variables must be a 
+    power of 2.
+    
+USAGE:
+    Refere to the header file uart.h for a description of the routines. 
+    See also example test_uart.c.
+
+NOTES:
+    Based on Atmel Application Note AVR306
+                    
+*************************************************************************/
+#include <avr/io.h>
+#include <avr/interrupt.h>
+//#include <avr/signal.h>
+#include <avr/pgmspace.h>
+
+/*
+ *  constants and macros
+ */
+
+/* size of RX/TX buffers */
+#define UART_RX_BUFFER_MASK ( UART_RX_BUFFER_SIZE - 1)
+#define UART_TX_BUFFER_MASK ( UART_TX_BUFFER_SIZE - 1)
+
+#if ( UART_RX_BUFFER_SIZE & UART_RX_BUFFER_MASK )
+#error RX buffer size is not a power of 2
+#endif
+#if ( UART_TX_BUFFER_SIZE & UART_TX_BUFFER_MASK )
+#error TX buffer size is not a power of 2
+#endif
+
+#if defined(__AVR_AT90S2313__) \
+ || defined(__AVR_AT90S4414__) || defined(__AVR_AT90S4434__) \
+ || defined(__AVR_AT90S8515__) || defined(__AVR_AT90S8535__) \
+ || defined(__AVR_ATmega103__)
+ /* old AVR classic or ATmega103 with one UART */
+ #define AT90_UART
+ #define UART0_RECEIVE_INTERRUPT   SIG_UART_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_UART_DATA
+ #define UART0_STATUS   USR
+ #define UART0_CONTROL  UCR
+ #define UART0_DATA     UDR  
+ #define UART0_UDRIE    UDRIE
+#elif defined(__AVR_AT90S2333__) || defined(__AVR_AT90S4433__)
+ /* old AVR classic with one UART */
+ #define AT90_UART
+ #define UART0_RECEIVE_INTERRUPT   SIG_UART_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_UART_DATA
+ #define UART0_STATUS   UCSRA
+ #define UART0_CONTROL  UCSRB
+ #define UART0_DATA     UDR 
+ #define UART0_UDRIE    UDRIE
+#elif  defined(__AVR_ATmega8__)  || defined(__AVR_ATmega16__) || defined(__AVR_ATmega32__) \
+  || defined(__AVR_ATmega8515__) || defined(__AVR_ATmega8535__) \
+  || defined(__AVR_ATmega323__)
+  /* ATmega with one USART */
+ #define ATMEGA_USART
+ #define UART0_RECEIVE_INTERRUPT   SIG_UART_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_UART_DATA
+ #define UART0_STATUS   UCSRA
+ #define UART0_CONTROL  UCSRB
+ #define UART0_DATA     UDR
+ #define UART0_UDRIE    UDRIE
+#elif defined(__AVR_ATmega163__) 
+  /* ATmega163 with one UART */
+ #define ATMEGA_UART
+ #define UART0_RECEIVE_INTERRUPT   SIG_UART_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_UART_DATA
+ #define UART0_STATUS   UCSRA
+ #define UART0_CONTROL  UCSRB
+ #define UART0_DATA     UDR
+ #define UART0_UDRIE    UDRIE
+#elif defined(__AVR_ATmega162__)
+ /* ATmega with two USART */
+ #define ATMEGA_USART0
+ #define ATMEGA_USART1
+ #define UART0_RECEIVE_INTERRUPT   SIG_USART0_RECV
+ #define UART1_RECEIVE_INTERRUPT   SIG_USART1_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_USART0_DATA
+ #define UART1_TRANSMIT_INTERRUPT  SIG_USART1_DATA
+ #define UART0_STATUS   UCSR0A
+ #define UART0_CONTROL  UCSR0B
+ #define UART0_DATA     UDR0
+ #define UART0_UDRIE    UDRIE0
+ #define UART1_STATUS   UCSR1A
+ #define UART1_CONTROL  UCSR1B
+ #define UART1_DATA     UDR1
+ #define UART1_UDRIE    UDRIE1
+#elif defined(__AVR_ATmega64__) || defined(__AVR_ATmega128__) 
+ /* ATmega with two USART */
+ #define ATMEGA_USART0
+ #define ATMEGA_USART1
+ #define UART0_RECEIVE_INTERRUPT   SIG_UART0_RECV
+ #define UART1_RECEIVE_INTERRUPT   SIG_UART1_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_UART0_DATA
+ #define UART1_TRANSMIT_INTERRUPT  SIG_UART1_DATA
+ #define UART0_STATUS   UCSR0A
+ #define UART0_CONTROL  UCSR0B
+ #define UART0_DATA     UDR0
+ #define UART0_UDRIE    UDRIE0
+ #define UART1_STATUS   UCSR1A
+ #define UART1_CONTROL  UCSR1B
+ #define UART1_DATA     UDR1
+ #define UART1_UDRIE    UDRIE1
+#elif defined(__AVR_ATmega161__)
+ /* ATmega with UART */
+ #error "AVR ATmega161 currently not supported by this libaray !"
+#elif defined(__AVR_ATmega169__) 
+ /* ATmega with one USART */
+ #define ATMEGA_USART
+ #define UART0_RECEIVE_INTERRUPT   SIG_USART_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_USART_DATA
+ #define UART0_STATUS   UCSRA
+ #define UART0_CONTROL  UCSRB
+ #define UART0_DATA     UDR
+ #define UART0_UDRIE    UDRIE
+#elif defined(__AVR_ATmega48__) ||defined(__AVR_ATmega88__) || defined(__AVR_ATmega168__)
+ #define ATMEGA_USART0
+ #define UART0_RECEIVE_INTERRUPT   SIG_USART_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_USART_DATA
+ #define UART0_STATUS   UCSR0A
+ #define UART0_CONTROL  UCSR0B
+ #define UART0_DATA     UDR0
+ #define UART0_UDRIE    UDRIE0
+#elif defined(__AVR_ATtiny2313__)
+ #define ATMEGA_USART
+ #define UART0_RECEIVE_INTERRUPT   SIG_USART0_RX 
+ #define UART0_TRANSMIT_INTERRUPT  SIG_USART0_UDRE
+ #define UART0_STATUS   UCSRA
+ #define UART0_CONTROL  UCSRB
+ #define UART0_DATA     UDR
+ #define UART0_UDRIE    UDRIE
+#else
+ #error "no UART definition for MCU available"
+#endif
+
+
+/*
+ *  module global variables
+ */
+static volatile unsigned char UART_TxBuf[UART_TX_BUFFER_SIZE];
+static volatile unsigned char UART_RxBuf[UART_RX_BUFFER_SIZE];
+static volatile unsigned char UART_TxHead;
+static volatile unsigned char UART_TxTail;
+static volatile unsigned char UART_RxHead;
+static volatile unsigned char UART_RxTail;
+static volatile unsigned char UART_LastRxError;
+
+#if defined( ATMEGA_USART1 )
+static volatile unsigned char UART1_TxBuf[UART_TX_BUFFER_SIZE];
+static volatile unsigned char UART1_RxBuf[UART_RX_BUFFER_SIZE];
+static volatile unsigned char UART1_TxHead;
+static volatile unsigned char UART1_TxTail;
+static volatile unsigned char UART1_RxHead;
+static volatile unsigned char UART1_RxTail;
+static volatile unsigned char UART1_LastRxError;
+#endif
+
+
+
+SIGNAL(UART0_RECEIVE_INTERRUPT)
+/*************************************************************************
+Function: UART Receive Complete interrupt
+Purpose:  called when the UART has received a character
+**************************************************************************/
+{
+    unsigned char tmphead;
+    unsigned char data;
+    unsigned char usr;
+    unsigned char lastRxError;
+ 
+ 
+    /* read UART status register and UART data register */ 
+    usr  = UART0_STATUS;
+    data = UART0_DATA;
+    
+    /* */
+#if defined( AT90_UART )
+    lastRxError = (usr & (_BV(FE)|_BV(DOR)) );
+#elif defined( ATMEGA_USART )
+    lastRxError = (usr & (_BV(FE)|_BV(DOR)) );
+#elif defined( ATMEGA_USART0 )
+    lastRxError = (usr & (_BV(FE0)|_BV(DOR0)) );
+#elif defined ( ATMEGA_UART )
+    lastRxError = (usr & (_BV(FE)|_BV(DOR)) );
+#endif
+        
+    /* calculate buffer index */ 
+    tmphead = ( UART_RxHead + 1) & UART_RX_BUFFER_MASK;
+    
+    if ( tmphead == UART_RxTail ) {
+        /* error: receive buffer overflow */
+        lastRxError = UART_BUFFER_OVERFLOW >> 8;
+    }else{
+        /* store new index */
+        UART_RxHead = tmphead;
+        /* store received data in buffer */
+        UART_RxBuf[tmphead] = data;
+    }
+    UART_LastRxError = lastRxError;   
+}
+
+
+SIGNAL(UART0_TRANSMIT_INTERRUPT)
+/*************************************************************************
+Function: UART Data Register Empty interrupt
+Purpose:  called when the UART is ready to transmit the next byte
+**************************************************************************/
+{
+    unsigned char tmptail;
+
+    
+    if ( UART_TxHead != UART_TxTail) {
+        /* calculate and store new buffer index */
+        tmptail = (UART_TxTail + 1) & UART_TX_BUFFER_MASK;
+        UART_TxTail = tmptail;
+        /* get one byte from buffer and write it to UART */
+        UART0_DATA = UART_TxBuf[tmptail];  /* start transmission */
+    }else{
+        /* tx buffer empty, disable UDRE interrupt */
+        UART0_CONTROL &= ~_BV(UART0_UDRIE);
+    }
+}
+
+
+/*************************************************************************
+Function: uart_init()
+Purpose:  initialize UART and set baudrate
+Input:    baudrate using macro UART_BAUD_SELECT()
+Returns:  none
+**************************************************************************/
+void uart_init(unsigned int baudrate)
+{
+    UART_TxHead = 0;
+    UART_TxTail = 0;
+    UART_RxHead = 0;
+    UART_RxTail = 0;
+    
+#if defined( AT90_UART )
+    /* set baud rate */
+    UBRR = (unsigned char)baudrate; 
+
+    /* enable UART receiver and transmmitter and receive complete interrupt */
+    UART0_CONTROL = _BV(RXCIE)|_BV(RXEN)|_BV(TXEN);
+
+#elif defined (ATMEGA_USART)
+    /* Set baud rate */
+    if ( baudrate & 0x8000 )
+    {
+    	 UART0_STATUS = (1<<U2X);  //Enable 2x speed 
+    	 baudrate &= ~0x8000;
+    }
+    UBRRH = (unsigned char)(baudrate>>8);
+    UBRRL = (unsigned char) baudrate;
+   
+    /* Enable USART receiver and transmitter and receive complete interrupt */
+    UART0_CONTROL = _BV(RXCIE)|(1<<RXEN)|(1<<TXEN);
+    
+    /* Set frame format: asynchronous, 8data, no parity, 1stop bit */
+    #ifdef URSEL
+    UCSRC = (1<<URSEL)|(3<<UCSZ0);
+    #else
+    UCSRC = (3<<UCSZ0);
+    #endif 
+    
+#elif defined (ATMEGA_USART0 )
+    /* Set baud rate */
+    if ( baudrate & 0x8000 ) 
+    {
+   		UART0_STATUS = (1<<U2X0);  //Enable 2x speed 
+   		baudrate &= ~0x8000;
+   	}
+    UBRR0H = (unsigned char)(baudrate>>8);
+    UBRR0L = (unsigned char) baudrate;
+
+    /* Enable USART receiver and transmitter and receive complete interrupt */
+    UART0_CONTROL = _BV(RXCIE0)|(1<<RXEN0)|(1<<TXEN0);
+    
+    /* Set frame format: asynchronous, 8data, no parity, 1stop bit */
+    #ifdef URSEL0
+    UCSR0C = (1<<URSEL0)|(3<<UCSZ00);
+    #else
+    UCSR0C = (3<<UCSZ00);
+    #endif 
+
+#elif defined ( ATMEGA_UART )
+    /* set baud rate */
+    if ( baudrate & 0x8000 ) 
+    {
+    	UART0_STATUS = (1<<U2X);  //Enable 2x speed 
+    	baudrate &= ~0x8000;
+    }
+    UBRRHI = (unsigned char)(baudrate>>8);
+    UBRR   = (unsigned char) baudrate;
+
+    /* Enable UART receiver and transmitter and receive complete interrupt */
+    UART0_CONTROL = _BV(RXCIE)|(1<<RXEN)|(1<<TXEN);
+
+#endif
+
+}/* uart_init */
+
+
+/*************************************************************************
+Function: uart_getc()
+Purpose:  return byte from ringbuffer  
+Returns:  lower byte:  received byte from ringbuffer
+          higher byte: last receive error
+**************************************************************************/
+unsigned int uart_getc(void)
+{    
+    unsigned char tmptail;
+    unsigned char data;
+
+
+    if ( UART_RxHead == UART_RxTail ) {
+        return UART_NO_DATA;   /* no data available */
+    }
+    
+    /* calculate /store buffer index */
+    tmptail = (UART_RxTail + 1) & UART_RX_BUFFER_MASK;
+    UART_RxTail = tmptail; 
+    
+    /* get data from receive buffer */
+    data = UART_RxBuf[tmptail];
+    
+    return (UART_LastRxError << 8) + data;
+
+}/* uart_getc */
+
+
+/*************************************************************************
+Function: uart_putc()
+Purpose:  write byte to ringbuffer for transmitting via UART
+Input:    byte to be transmitted
+Returns:  none          
+**************************************************************************/
+void uart_putc(unsigned char data)
+{
+    unsigned char tmphead;
+
+    
+    tmphead  = (UART_TxHead + 1) & UART_TX_BUFFER_MASK;
+    
+    while ( tmphead == UART_TxTail ){
+        ;/* wait for free space in buffer */
+    }
+    
+    UART_TxBuf[tmphead] = data;
+    UART_TxHead = tmphead;
+
+    /* enable UDRE interrupt */
+    UART0_CONTROL    |= _BV(UART0_UDRIE);
+
+}/* uart_putc */
+
+
+/*************************************************************************
+Function: uart_puts()
+Purpose:  transmit string to UART
+Input:    string to be transmitted
+Returns:  none          
+**************************************************************************/
+void uart_puts(const char *s )
+{
+    while (*s) 
+      uart_putc(*s++);
+
+}/* uart_puts */
+
+
+/*************************************************************************
+Function: uart_puts_p()
+Purpose:  transmit string from program memory to UART
+Input:    program memory string to be transmitted
+Returns:  none
+**************************************************************************/
+void uart_puts_p(const char *progmem_s )
+{
+    register char c;
+    
+    while ( (c = pgm_read_byte(progmem_s++)) ) 
+      uart_putc(c);
+
+}/* uart_puts_p */
+
+
+/*
+ * these functions are only for ATmegas with two USART
+ */
+#if defined( ATMEGA_USART1 )
+
+SIGNAL(UART1_RECEIVE_INTERRUPT)
+/*************************************************************************
+Function: UART1 Receive Complete interrupt
+Purpose:  called when the UART1 has received a character
+**************************************************************************/
+{
+    unsigned char tmphead;
+    unsigned char data;
+    unsigned char usr;
+    unsigned char lastRxError;
+ 
+ 
+    /* read UART status register and UART data register */ 
+    usr  = UART1_STATUS;
+    data = UART1_DATA;
+    
+    /* */
+    lastRxError = (usr & (_BV(FE1)|_BV(DOR1)) );
+        
+    /* calculate buffer index */ 
+    tmphead = ( UART1_RxHead + 1) & UART_RX_BUFFER_MASK;
+    
+    if ( tmphead == UART1_RxTail ) {
+        /* error: receive buffer overflow */
+        lastRxError = UART_BUFFER_OVERFLOW >> 8;
+    }else{
+        /* store new index */
+        UART1_RxHead = tmphead;
+        /* store received data in buffer */
+        UART1_RxBuf[tmphead] = data;
+    }
+    UART1_LastRxError = lastRxError;   
+}
+
+
+SIGNAL(UART1_TRANSMIT_INTERRUPT)
+/*************************************************************************
+Function: UART1 Data Register Empty interrupt
+Purpose:  called when the UART1 is ready to transmit the next byte
+**************************************************************************/
+{
+    unsigned char tmptail;
+
+    
+    if ( UART1_TxHead != UART1_TxTail) {
+        /* calculate and store new buffer index */
+        tmptail = (UART1_TxTail + 1) & UART_TX_BUFFER_MASK;
+        UART1_TxTail = tmptail;
+        /* get one byte from buffer and write it to UART */
+        UART1_DATA = UART1_TxBuf[tmptail];  /* start transmission */
+    }else{
+        /* tx buffer empty, disable UDRE interrupt */
+        UART1_CONTROL &= ~_BV(UART1_UDRIE);
+    }
+}
+
+
+/*************************************************************************
+Function: uart1_init()
+Purpose:  initialize UART1 and set baudrate
+Input:    baudrate using macro UART_BAUD_SELECT()
+Returns:  none
+**************************************************************************/
+void uart1_init(unsigned int baudrate)
+{
+    UART1_TxHead = 0;
+    UART1_TxTail = 0;
+    UART1_RxHead = 0;
+    UART1_RxTail = 0;
+    
+
+    /* Set baud rate */
+    if ( baudrate & 0x8000 ) 
+    {
+    	UART1_STATUS = (1<<U2X1);  //Enable 2x speed 
+      baudrate &= ~0x8000;
+    }
+    UBRR1H = (unsigned char)(baudrate>>8);
+    UBRR1L = (unsigned char) baudrate;
+
+    /* Enable USART receiver and transmitter and receive complete interrupt */
+    UART1_CONTROL = _BV(RXCIE1)|(1<<RXEN1)|(1<<TXEN1);
+    
+    /* Set frame format: asynchronous, 8data, no parity, 1stop bit */   
+    #ifdef URSEL1
+    UCSR1C = (1<<URSEL1)|(3<<UCSZ10);
+    #else
+    UCSR1C = (3<<UCSZ10);
+    #endif 
+}/* uart_init */
+
+
+/*************************************************************************
+Function: uart1_getc()
+Purpose:  return byte from ringbuffer  
+Returns:  lower byte:  received byte from ringbuffer
+          higher byte: last receive error
+**************************************************************************/
+unsigned int uart1_getc(void)
+{    
+    unsigned char tmptail;
+    unsigned char data;
+
+
+    if ( UART1_RxHead == UART1_RxTail ) {
+        return UART_NO_DATA;   /* no data available */
+    }
+    
+    /* calculate /store buffer index */
+    tmptail = (UART1_RxTail + 1) & UART_RX_BUFFER_MASK;
+    UART1_RxTail = tmptail; 
+    
+    /* get data from receive buffer */
+    data = UART1_RxBuf[tmptail];
+    
+    return (UART1_LastRxError << 8) + data;
+
+}/* uart1_getc */
+
+
+/*************************************************************************
+Function: uart1_putc()
+Purpose:  write byte to ringbuffer for transmitting via UART
+Input:    byte to be transmitted
+Returns:  none          
+**************************************************************************/
+void uart1_putc(unsigned char data)
+{
+    unsigned char tmphead;
+
+    
+    tmphead  = (UART1_TxHead + 1) & UART_TX_BUFFER_MASK;
+    
+    while ( tmphead == UART1_TxTail ){
+        ;/* wait for free space in buffer */
+    }
+    
+    UART1_TxBuf[tmphead] = data;
+    UART1_TxHead = tmphead;
+
+    /* enable UDRE interrupt */
+    UART1_CONTROL    |= _BV(UART1_UDRIE);
+
+}/* uart1_putc */
+
+
+/*************************************************************************
+Function: uart1_puts()
+Purpose:  transmit string to UART1
+Input:    string to be transmitted
+Returns:  none          
+**************************************************************************/
+void uart1_puts(const char *s )
+{
+    while (*s) 
+      uart1_putc(*s++);
+
+}/* uart1_puts */
+
+
+/*************************************************************************
+Function: uart1_puts_p()
+Purpose:  transmit string from program memory to UART1
+Input:    program memory string to be transmitted
+Returns:  none
+**************************************************************************/
+void uart1_puts_p(const char *progmem_s )
+{
+    register char c;
+    
+    while ( (c = pgm_read_byte(progmem_s++)) ) 
+      uart1_putc(c);
+
+}/* uart1_puts_p */
+
+
+#endif

source/Concept/Framework/atmega128io.h

@@ -1,12 +1,183 @@
 #ifndef _ATMEGA128IO_H
 #define _ATMEGA128IO_H
 
+#define UART_BAUD_RATE 9600
+
 #include <avr/io.h>
 #include <avr/interrupt.h>
 
-namespace hardware
+/************************************************************************
-{
+Title:    Interrupt UART library with receive/transmit circular buffers
+Author:   Peter Fleury <pfleury@gmx.ch>   http://jump.to/fleury
+File:     $Id: uart.h,v 1.7.2.5 2005/08/14 11:25:41 Peter Exp $
+Software: AVR-GCC 3.3
+Hardware: any AVR with built-in UART, tested on AT90S8515 at 4 Mhz
+Usage:    see Doxygen manual
+************************************************************************/
 
-};
+/** 
+ *  @defgroup pfleury_uart UART Library
+ *  @code #include <uart.h> @endcode
+ * 
+ *  @brief Interrupt UART library using the built-in UART with transmit and receive circular buffers. 
+ *
+ *  This library can be used to transmit and receive data through the built in UART. 
+ *
+ *  An interrupt is generated when the UART has finished transmitting or
+ *  receiving a byte. The interrupt handling routines use circular buffers
+ *  for buffering received and transmitted data.
+ *
+ *  The UART_RX_BUFFER_SIZE and UART_TX_BUFFER_SIZE constants define
+ *  the size of the circular buffers in bytes. Note that these constants must be a power of 2.
+ *  You may need to adapt this constants to your target and your application by adding 
+ *  CDEFS += -DUART_RX_BUFFER_SIZE=nn -DUART_RX_BUFFER_SIZE=nn to your Makefile.
+ *
+ *  @note Based on Atmel Application Note AVR306
+ *  @author Peter Fleury pfleury@gmx.ch  http://jump.to/fleury
+ */
+ 
+/**@{*/
+
+
+#if (__GNUC__ * 100 + __GNUC_MINOR__) < 304
+#error "This library requires AVR-GCC 3.4 or later, update to newer AVR-GCC compiler !"
+#endif
+
+
+/*
+** constants and macros
+*/
+
+/** @brief  UART Baudrate Expression
+ *  @param  xtalcpu  system clock in Mhz, e.g. 4000000L for 4Mhz          
+ *  @param  baudrate baudrate in bps, e.g. 1200, 2400, 9600     
+ */
+#define UART_BAUD_SELECT(baudRate,xtalCpu) ((xtalCpu)/((baudRate)*16l)-1)
+
+/** @brief  UART Baudrate Expression for ATmega double speed mode
+ *  @param  xtalcpu  system clock in Mhz, e.g. 4000000L for 4Mhz           
+ *  @param  baudrate baudrate in bps, e.g. 1200, 2400, 9600     
+ */
+#define UART_BAUD_SELECT_DOUBLE_SPEED(baudRate,xtalCpu) (((xtalCpu)/((baudRate)*8l)-1)|0x8000)
+
+
+/** Size of the circular receive buffer, must be power of 2 */
+#ifndef UART_RX_BUFFER_SIZE
+#define UART_RX_BUFFER_SIZE 32
+#endif
+/** Size of the circular transmit buffer, must be power of 2 */
+#ifndef UART_TX_BUFFER_SIZE
+#define UART_TX_BUFFER_SIZE 32
+#endif
+
+/* test if the size of the circular buffers fits into SRAM */
+#if ( (UART_RX_BUFFER_SIZE+UART_TX_BUFFER_SIZE) >= (RAMEND-0x60 ) )
+#error "size of UART_RX_BUFFER_SIZE + UART_TX_BUFFER_SIZE larger than size of SRAM"
+#endif
+
+/* 
+** high byte error return code of uart_getc()
+*/
+#define UART_FRAME_ERROR      0x0800              /* Framing Error by UART       */
+#define UART_OVERRUN_ERROR    0x0400              /* Overrun condition by UART   */
+#define UART_BUFFER_OVERFLOW  0x0200              /* receive ringbuffer overflow */
+#define UART_NO_DATA          0x0100              /* no receive data available   */
+
+
+/*
+** function prototypes
+*/
+
+/**
+   @brief   Initialize UART and set baudrate 
+   @param   baudrate Specify baudrate using macro UART_BAUD_SELECT()
+   @return  none
+*/
+extern void uart_init(unsigned int baudrate);
+
+
+/**
+ *  @brief   Get received byte from ringbuffer
+ *
+ * Returns in the lower byte the received character and in the 
+ * higher byte the last receive error.
+ * UART_NO_DATA is returned when no data is available.
+ *
+ *  @param   void
+ *  @return  lower byte:  received byte from ringbuffer
+ *  @return  higher byte: last receive status
+ *           - \b 0 successfully received data from UART
+ *           - \b UART_NO_DATA           
+ *             <br>no receive data available
+ *           - \b UART_BUFFER_OVERFLOW   
+ *             <br>Receive ringbuffer overflow.
+ *             We are not reading the receive buffer fast enough, 
+ *             one or more received character have been dropped 
+ *           - \b UART_OVERRUN_ERROR     
+ *             <br>Overrun condition by UART.
+ *             A character already present in the UART UDR register was 
+ *             not read by the interrupt handler before the next character arrived,
+ *             one or more received characters have been dropped.
+ *           - \b UART_FRAME_ERROR       
+ *             <br>Framing Error by UART
+ */
+extern unsigned int uart_getc(void);
+
+
+/**
+ *  @brief   Put byte to ringbuffer for transmitting via UART
+ *  @param   data byte to be transmitted
+ *  @return  none
+ */
+extern void uart_putc(unsigned char data);
+
+
+/**
+ *  @brief   Put string to ringbuffer for transmitting via UART
+ *
+ *  The string is buffered by the uart library in a circular buffer
+ *  and one character at a time is transmitted to the UART using interrupts.
+ *  Blocks if it can not write the whole string into the circular buffer.
+ * 
+ *  @param   s string to be transmitted
+ *  @return  none
+ */
+extern void uart_puts(const char *s );
+
+
+/**
+ * @brief    Put string from program memory to ringbuffer for transmitting via UART.
+ *
+ * The string is buffered by the uart library in a circular buffer
+ * and one character at a time is transmitted to the UART using interrupts.
+ * Blocks if it can not write the whole string into the circular buffer.
+ *
+ * @param    s program memory string to be transmitted
+ * @return   none
+ * @see      uart_puts_P
+ */
+extern void uart_puts_p(const char *s );
+
+/**
+ * @brief    Macro to automatically put a string constant into program memory
+ */
+#define uart_puts_P(__s)       uart_puts_p(PSTR(__s))
+
+
+
+/** @brief  Initialize USART1 (only available on selected ATmegas) @see uart_init */
+extern void uart1_init(unsigned int baudrate);
+/** @brief  Get received byte of USART1 from ringbuffer. (only available on selected ATmega) @see uart_getc */
+extern unsigned int uart1_getc(void);
+/** @brief  Put byte to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_putc */
+extern void uart1_putc(unsigned char data);
+/** @brief  Put string to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_puts */
+extern void uart1_puts(const char *s );
+/** @brief  Put string from program memory to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_puts_p */
+extern void uart1_puts_p(const char *s );
+/** @brief  Macro to automatically put a string constant into program memory */
+#define uart1_puts_P(__s)       uart1_puts_p(PSTR(__s))
+
+/**@}*/
 
 #endif

source/Concept/Framework/default/Makefile

@@ -6,14 +6,14 @@
 PROJECT = RoboCode
 MCU = atmega128
 TARGET = RoboCode.elf
-CC = avr-gcc.exe
+CC = avr-g++.exe
 
 ## Options common to compile, link and assembly rules
 COMMON = -mmcu=$(MCU)
 
 ## Compile options common for all C compilation units.
 CFLAGS = $(COMMON)
-CFLAGS += -Wall -gdwarf-2 -O0
+CFLAGS += -Wall -gdwarf-2     -DF_CPU=16000000ULUL  -O3 -fsigned-char
 CFLAGS += -MD -MP -MT $(*F).o -MF dep/$(@F).d 
 
 ## Assembly specific flags

source/Concept/Framework/defines.h

@@ -57,6 +57,7 @@
 
 //Constants
 #define SPEED_PER_PWM 1
+#define DISTANCE_PER_VALUE 1
 
 //IO Module Names
 enum IOModuleNames
@@ -70,13 +71,20 @@ enum IOModuleNames
 	IO_ENGINE_DRIVE_LEFT = IO_ENGINE_START,
 	IO_ENGINE_DRIVE_RIGHT,
 	IO_ENGINE_DRIVE_BACK,
-	IO_ENGINE_DRIBBLER,
 
 	IO_ENGINE_END,
 
+	//Dribbler
+
+	IO_DRIBBLER_START = IO_ENGINE_END,
+
+	IO_DRIBBLER_MAIN = IO_DRIBBLER_START,
+
+	IO_DRIBBLER_END,
+
 	//Kicker
 
-	IO_KICKER_START = IO_ENGINE_END,
+	IO_KICKER_START = IO_DRIBBLER_END,
 
 	IO_KICKER_MAIN = IO_KICKER_START,
 
@@ -93,6 +101,10 @@ enum IOModuleNames
 	IO_SENSOR_IR_260_DEG,
 	IO_SENSOR_IR_300_DEG,
 	IO_SENSOR_IR_330_DEG,
+	IO_SENSOR_DISTANCE_0_DEG,
+	IO_SENSOR_DISTANCE_90_DEG,
+	IO_SENSOR_DISTANCE_180_DEG,
+	IO_SENSOR_DISTANCE_270_DEG,
 
 	IO_SENSOR_END,
 
@@ -104,8 +116,16 @@ enum IOModuleNames
 
 	IO_LED_END,
 
+	//Displays
+
+	IO_DISPLAY_START = IO_LED_END,
+
+	IO_DISPLAY_MAIN = IO_DISPLAY_START,
+
+	IO_DISPLAY_END,
+
 	//General
-	IO_END = IO_LED_END,
+	IO_END = IO_DISPLAY_END,
 };
 
 #endif

source/Concept/Framework/display.cpp

@@ -0,0 +1 @@
+#include "display.h"

source/Concept/Framework/display.h

@@ -0,0 +1,60 @@
+#ifndef _DISPLAY_H
+#define _DISPLAY_H
+
+#include "stdafx.h"
+
+class Display : public IO_Module
+{
+public:
+	Display()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+	}
+
+	Display(uint32 displayId)
+	{
+		this->parent = NULL;
+		this->moduleId = displayId;
+		
+		switch(displayId)
+		{
+			case IO_DISPLAY_MAIN:
+				msleep(500);
+				uart1_init(103);//9600 BAUD at 16MHz Atmel
+				sleep(2);
+				break;
+			default:
+				break;
+		}
+	}
+
+protected:
+	//Hardware
+	volatile uint8* hardwarePort;
+	volatile uint16* pwmSpeed;
+	uint8 pinForward;
+	uint8 pinReverse;
+
+public:
+	void Print(char* newString)
+	{
+		switch(moduleId)
+		{
+			case IO_DISPLAY_MAIN:
+				uart1_puts(newString);
+				break;
+			default:
+				break;
+		}
+	}
+
+	void Print(int32 newInteger)
+	{
+		char buffer[12];
+		ltoa(newInteger, buffer, 10);
+		Print(buffer);
+	}
+};
+
+#endif

source/Concept/Framework/distance_sensor.cpp

@@ -1 +1,26 @@
-#include "distance_sensor.h"
+#include "distance_sensor.h"
+
+//-----------------------------------------------------------------------------
+float Distance_Sensor::GetDistance()
+{
+	uint32 result = 0;
+
+	//Generate pulse
+	*hardwareDDR |= pin;//Set pin output
+	*hardwarePort |= pin;//Activate port
+	usleep(10);//Wait for 10µs
+	*hardwarePort &= ~pin;//Deactivate port
+	*hardwareDDR &= ~pin;//Set pin input
+
+	//Wait for response
+	while(!(*hardwarePin & pin));
+	
+	//Calculate duration of response
+	while(*hardwarePin & pin) 
+	{
+		result++;
+		asm volatile("nop");
+	}
+
+	return (float(result) * DISTANCE_PER_VALUE);
+}

source/Concept/Framework/distance_sensor.h

@@ -1,7 +1,7 @@
 #ifndef _DISTANCE_SENSOR_H
 #define _DISTANCE_SENSOR_H
 
-//#include "stdafx.h"
+#include "stdafx.h"
 #include "sensor.h"
 
 class Distance_Sensor : public Sensor
@@ -11,17 +11,61 @@ public:
 	{
 		this->parent = NULL;
 		this->moduleId = 0;
+		this->hardwarePort = NULL;
+		this->hardwareDDR = NULL;
+		this->hardwarePin = NULL;
+		this->pin = 0;
 	}
 
 	Distance_Sensor(uint32 sensorId)
 	{
 		this->parent = NULL;
 		this->moduleId = sensorId;
+
+		switch(sensorId)
+		{
+			case IO_SENSOR_DISTANCE_0_DEG:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pin = (1 << 0);
+				break;
+			case IO_SENSOR_DISTANCE_90_DEG:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pin = (1 << 1);
+				break;
+			case IO_SENSOR_DISTANCE_180_DEG:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pin = (1 << 2);
+				break;
+			case IO_SENSOR_DISTANCE_270_DEG:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pin = (1 << 3);
+				break;
+			default:
+				this->hardwarePort = NULL;
+				this->hardwareDDR = NULL;
+				this->hardwarePin = NULL;
+				this->pin = 0;
+				break;
+		}
 	}
 
 protected:
+	//Hardware
+	volatile uint8* hardwarePort;
+	volatile uint8* hardwareDDR;
+	volatile uint8* hardwarePin;
+	uint8 pin;
 
 public:
+	float GetDistance();
 };
 
 #endif

source/Concept/Framework/dribbler.cpp

@@ -0,0 +1 @@
+#include "dribbler.h"

source/Concept/Framework/dribbler.h

@@ -0,0 +1,118 @@
+#ifndef _DRIBBLER_H
+#define _DRIBBLER_H
+
+#include "stdafx.h"
+
+class Dribbler : public IO_Module
+{
+public:
+	Dribbler()
+	{
+		this->enabled = false;
+		this->curSpeed = 0;
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->hardwarePort = NULL;
+		this->portPower = NULL;
+		this->pinForward = 0;
+		this->pinReverse = 0;
+		this->pinPower = 0;
+	}
+
+	Dribbler(uint32 dribblerId)
+	{
+		this->enabled = false;
+		this->curSpeed = 1.0f;
+		this->parent = NULL;
+		this->moduleId = dribblerId;
+		
+		switch(dribblerId)
+		{
+			case IO_DRIBBLER_MAIN:
+				this->hardwarePort = &PORTD;
+				this->portPower = &PORTA;
+				this->pinForward = (1 << 6);
+				this->pinReverse = (1 << 7);
+				this->pinPower = (1 << 5);
+				break;
+			default:
+				this->hardwarePort = NULL;
+				this->portPower = NULL;
+				this->pinForward = 0;
+				this->pinReverse = 0;
+				this->pinPower = 0;
+				break;
+		}
+
+		UpdateDirection();
+	}
+
+protected:
+	bool enabled;
+	float curSpeed;
+
+	//Hardware
+	volatile uint8* hardwarePort;
+	volatile uint8* portPower;
+	uint8 pinForward;
+	uint8 pinReverse;
+	uint8 pinPower;
+
+	void UpdateDirection()
+	{
+		if(enabled)
+		{
+			if(curSpeed > 0)
+			{
+				*hardwarePort |= pinForward;
+				*hardwarePort &= ~pinReverse;
+			}
+			else if(curSpeed < 0)
+			{
+				*hardwarePort |= pinReverse;
+				*hardwarePort &= ~pinForward;
+			}
+			else
+			{
+				*hardwarePort |= pinForward;
+				*hardwarePort |= pinReverse;
+			}
+
+			*portPower |= pinPower;
+		}
+		else
+		{
+			*hardwarePort &= ~pinForward;
+			*hardwarePort &= ~pinReverse;
+
+			*portPower &= ~pinPower;
+		}
+	}
+
+public:
+	float GetSpeed()
+	{
+		return curSpeed;
+	}
+
+	void SetSpeed(float newSpeed)
+	{
+		curSpeed = newSpeed;
+
+		UpdateDirection();
+	}
+
+	bool GetEnabled()
+	{
+		return enabled;
+	}
+
+	void SetEnabled(bool newStatus)
+	{
+		enabled = newStatus;
+
+		UpdateDirection();
+	}
+};
+
+#endif

source/Concept/Framework/engine.cpp

@@ -1 +1 @@
-#include "engine.h"
+#include "engine.h"

source/Concept/Framework/engine.h

@@ -14,10 +14,8 @@ public:
 		this->moduleId = 0;
 		this->hardwarePort = NULL;
 		this->pwmSpeed = NULL;
-		this->pwmPort = NULL;
 		this->pinForward = 0;
 		this->pinReverse = 0;
-		this->pinPwm = 0;
 	}
 
 	Engine(uint32 engineId)
@@ -26,9 +24,6 @@ public:
 		this->curSpeed = 0;
 		this->parent = NULL;
 		this->moduleId = engineId;
-		this->pwmSpeed = NULL;
-		this->pwmPort = NULL;
-		this->pinPwm = 0;
 		
 		switch(engineId)
 		{
@@ -38,25 +33,18 @@ public:
 				this->pinForward = (1 << 0);
 				this->pinReverse = (1 << 1);
 				break;
-			case IO_ENGINE_DRIVE_RIGHT:
+			case IO_ENGINE_DRIVE_BACK:
 				this->hardwarePort = &PORTB;
 				this->pwmSpeed = &OCR1B;
 				this->pinForward = (1 << 2);
 				this->pinReverse = (1 << 3);
 				break;
-			case IO_ENGINE_DRIVE_BACK:
+			case IO_ENGINE_DRIVE_RIGHT:
 				this->hardwarePort = &PORTD;
 				this->pwmSpeed = &OCR3A;
 				this->pinForward = (1 << 5);
 				this->pinReverse = (1 << 4);
 				break;
-			case IO_ENGINE_DRIBBLER:
-				this->hardwarePort = &PORTD;
-				this->pwmPort = &PORTA;
-				this->pinForward = (1 << 6);
-				this->pinReverse = (1 << 7);
-				this->pinPwm = (1 << 5);
-				break;
 			default:
 				this->hardwarePort = NULL;
 				this->pwmSpeed = NULL;
@@ -77,9 +65,6 @@ protected:
 	volatile uint16* pwmSpeed;
 	uint8 pinForward;
 	uint8 pinReverse;
-	//Dribbler only
-	volatile uint8* pwmPort;
-	uint8 pinPwm;
 
 	void UpdateDirection()
 	{
@@ -118,18 +103,7 @@ public:
 	{
 		curSpeed = newSpeed;
 
-		if(pwmSpeed)
+		*pwmSpeed = abs(newSpeed / SPEED_PER_PWM);
-		{
-			*pwmSpeed = abs(newSpeed / SPEED_PER_PWM);
-		}
-		else if(pwmPort && (uint16)(abs(newSpeed / SPEED_PER_PWM)))
-		{
-			*pwmPort |= pinPwm;
-		}
-		else if(pwmPort)
-		{
-			*pwmPort &= ~pinPwm;
-		}
 
 		UpdateDirection();
 	}

source/Concept/Framework/io_module.cpp

@@ -1 +1 @@
-#include "io_module.h"
+#include "io_module.h"

source/Concept/Framework/ir_sensor.cpp

@@ -6,4 +6,4 @@ uint16 IR_Sensor::GetIRIntensity()
 	if(!parent) return 0;
 
 	return parent->GetADCValue(channel);
-}
+}

source/Concept/Framework/kicker.cpp

@@ -1 +1 @@
-#include "kicker.h"
+#include "kicker.h"

source/Concept/Framework/led.cpp

@@ -1 +1 @@
-#include "led.h"
+#include "led.h"

source/Concept/Framework/main.cpp

@@ -13,6 +13,14 @@ int main()
 		newEngine = NULL;
 	}
 
+	//Init Dribbler
+	for(uint8 i = IO_DRIBBLER_START; i < IO_DRIBBLER_END; i++)
+	{
+		Dribbler* newDribbler = new Dribbler(i);
+		localRobot->AddModule(newDribbler);
+		newDribbler = NULL;
+	}
+
 	//Init Kicker
 	for(uint8 i = IO_KICKER_START; i < IO_KICKER_END; i++)
 	{
@@ -41,6 +49,16 @@ int main()
 				newSensor = NULL;
 				break;
 			}
+			case IO_SENSOR_DISTANCE_0_DEG:
+			case IO_SENSOR_DISTANCE_90_DEG:
+			case IO_SENSOR_DISTANCE_180_DEG:
+			case IO_SENSOR_DISTANCE_270_DEG:
+			{
+				Distance_Sensor* newSensor = new Distance_Sensor(i);
+				localRobot->AddModule(newSensor);
+				newSensor = NULL;
+				break;
+			}
 			//Other cases
 			default:
 			{
@@ -60,6 +78,14 @@ int main()
 		newLed = NULL;
 	}
 
+	//Init Displays
+	for(uint8 i = IO_DISPLAY_START; i < IO_DISPLAY_END; i++)
+	{
+		Display* newDisplay = new Display(i);
+		localRobot->AddModule(newDisplay);
+		newDisplay = NULL;
+	}
+
 	//Run
 	while(true)
 	{

source/Concept/Framework/robot.cpp

@@ -18,7 +18,7 @@ Robot::Robot()
 	PORTC = 0;
 
 	//All output except PD0+1(I2C) + 2+3(RS232)
-	DDRD = (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6)  | (1 << 7);
+	DDRD = (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6)  | (1 << 7);
 	PORTD = (1 << 0) | (1 << 1);//Activate pullup at PD0+1
 
 	//PE5 for input
@@ -34,12 +34,12 @@ Robot::Robot()
 	PORTG = (1 << 0) | (1 << 1);
 
 	// activate channel A+B on PWM1 at 8Bit
-	TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << WGM10);
+	TCCR1A = (1<< COM1A1) | (1<< COM1B1) | (1<< WGM10);
-	TCCR1B = (1 <<ICNC1) | (1 <<CS12) | (1 <<CS10); // set clock/prescaler 1/1024 -> enable counter
+	TCCR1B = (1<<ICNC1) | (1<<CS11); // set clock/prescaler 1/8 -> enable counter
 
 	// activate Kanal A+B on PWM3 at 8Bit
-	TCCR3A = (1 << COM3A1) | (1 << COM3B1) | (1 << WGM10);
+	TCCR3A = (1<< COM3A1) | (1<< COM3B1) | (1<< WGM10);
-	TCCR3B =  (1 <<ICNC3) | (1 <<CS32) | (1 <<CS30); // set clock/prescaler 1/1024 -> enable counter
+	TCCR3B =  (1<<ICNC3) | (1<<CS31); // set clock/prescaler 1/8 -> enable counter
 
 	//Activate interrupt
 	sei();

source/Concept/Framework/stdafx.h

@@ -8,8 +8,10 @@
 #include "atmega128io.h"
 #include "tools.h"
 #include "io_module.h"
+#include "display.h"
 #include "sensor.h"
 #include "engine.h"
+#include "dribbler.h"
 #include "kicker.h"
 #include "led.h"
 #include "distance_sensor.h"

source/Concept/Framework/tools.h

@@ -6,6 +6,34 @@
 #ifndef new
 	void* operator new(size_t sz);
 	void operator delete(void* p);
-#endif
+#endif
+
+inline void sleep(int sec)
+{
+  for (int s=0; s<sec; s++) {
+    for (long int i=0; i<1405678; i++) {
+      asm volatile("nop"); 
+    }
+  }
+};
+
+inline void msleep(int msec)
+{
+  for (int s=0; s<msec; s++) {
+    for (long int i=0; i<1405; i++) {
+      asm volatile("nop"); 
+    }
+  }
+};
+
+inline void usleep(int usec)
+{
+	usec *= 100;
+  for (int s=0; s<usec; s++) {
+    for (long int i=0; i<1405; i++) {
+      asm volatile("nop"); 
+    }
+  }
+};
 
 #endif