From d7ac8f546afb75bd2d626b5cc639939337560cf6 Mon Sep 17 00:00:00 2001
From: meyma <devnull@localhost>
Date: Thu, 22 Feb 2007 14:08:01 +0000
Subject: We have a SRF10, not SRF05

---
 source/Concept/Framework/atmega128io.c             | 172 +++++++++++++++++++-
 source/Concept/Framework/atmega128io.h             |  95 ++++++++++-
 .../Framework/modules/input/distance_sensor.c      | 176 ++++++++++++++++-----
 .../Framework/modules/input/distance_sensor.h      | 102 +++++++-----
 4 files changed, 470 insertions(+), 75 deletions(-)

diff --git a/source/Concept/Framework/atmega128io.c b/source/Concept/Framework/atmega128io.c
index b1d6f4d..94e0f94 100755
--- a/source/Concept/Framework/atmega128io.c
+++ b/source/Concept/Framework/atmega128io.c
@@ -580,6 +580,174 @@ void uart1_puts_p(const char *progmem_s )
       uart1_putc(c);
 
 }/* uart1_puts_p */
-
-
+
+/////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////
+// TWI-Driver
+
+
+/*!
+ * TWI Bus initialsieren
+ * @return Resultat der Aktion
+ */
+
+int8 Init_TWI(void){
+	TWAR = OWN_ADR;							/*!< Eigenen Slave Adresse setzen */
+	TWBR = 12;                    			/*!< Setze Baudrate auf 100 KHz  */
+											/*!< 4 MHz xtal */
+	TWCR = (1<<TWEN);						/*!< TWI-Interface einschalten */
+
+    return 1;
+}    
+
+/*!
+ * TWI Buss schliesen
+ * @return Resultat der Aktion
+ */
+ 
+int8 Close_TWI(void){
+	TWCR = (0<<TWEN);						/*!< TWI-Interface ausschalten */
+
+    return 0;
+}    
+
+/*!
+ * Warte auf TWI interrupt
+ */
+ 
+void Wait_TWI_int(void){
+	while (!(TWCR & (1<<TWINT)))
+	    ; 
+}    
+
+/*!
+ * Sende Start Sequence
+ * @return Resultat der Aktion
+ */
+
+uint8 Send_start(void){
+	TWCR = ((1<<TWINT)+(1<<TWSTA)+(1<<TWEN)); 		/*!< Sende START */
+	
+	Wait_TWI_int();									/*!< Warte auf TWI interrupt */
+
+    if((TWSR != START)&&(TWSR != REP_START))		/*!< Ist der Status ein Anderer als Start (0x08) oder wiederholter Start (0x10) */
+		return TWSR;								/*!< -> error  und Rueckgabe TWSR. */
+	return SUCCESS;									/*!< wenn OK Rueckgabe SUCCESS */
+}							
+
+/*!
+ * Sende Stop Sequence
+ */
+ 
+void Send_stop(void){
+	TWCR = ((1<<TWEN)+(1<<TWINT)+(1<<TWSTO));
+}        
+    
+/*!
+ * Hier wird der eigentliche TWI-Treiber angesprochen
+ * @param *data_pack Container mit den Daten fuer den Treiber
+ * @return Resultat der Aktion
+ */
+uint8 Send_to_TWI(tx_type *data_pack){
+	uint8 state,i,j;
+
+	state = SUCCESS;
+	
+	for(i=0;(data_pack[i].slave_adr != OWN_ADR)&&(state == SUCCESS);i++)	{
+		state = Send_start();
+		if (state == SUCCESS)				
+			state = Send_adr(data_pack[i].slave_adr);
+		
+		/*!
+		 * Abhaengig von W/R senden oder empfangen
+		 */
+		if(!(data_pack[i].slave_adr & R)) {
+			if (state == SUCCESS){
+				/*!
+				 * Wenn W bis alle Daten gesendet sind
+				 */
+				for(j=0;((j<data_pack[i].size)&&(state == SUCCESS));j++)
+					state = Send_byte(data_pack[i].data_ptr[j]);
+			}
+		}	
+		else{
+			if (state == MRX_ADR_NACK)	{
+				state = Send_start();
+			}
+			
+			if (state == SUCCESS){
+				/*!
+				 * Wenn R bis alle Daten empfangen sind
+				 */
+				for(j=0;((j<data_pack[i].size)&&(state == SUCCESS));j++){
+					/*!
+					 * Wenn wir keine Daten mehr erwarten NACK senden
+					 */
+					if(j == data_pack[i].size-1)
+						state = Get_byte(data_pack[i].data_ptr++,0);
+					else
+						state = Get_byte(data_pack[i].data_ptr++,1);
+				}
+			}
+		}					
+	  	Send_stop();
+	}
+ 	Close_TWI();
+  	  
+	return state;
+}
+
+/*!
+ * Sende ein Byte
+ * @param data das zu uebertragende Byte
+ */
+ 
+uint8 Send_byte(uint8 data){
+	Wait_TWI_int();
+	TWDR = data;
+ 	TWCR = ((1<<TWINT)+(1<<TWEN));
+	Wait_TWI_int();
+	if(TWSR != MTX_DATA_ACK)
+		return TWSR;																
+	return SUCCESS;
+}	
+
+/*!
+ * Sende Slave Adresse
+ * @param adr die gewuenschte Adresse
+ * @return Resultat der Aktion
+ */
+ 
+uint8 Send_adr(uint8 adr){
+	Wait_TWI_int();
+	TWDR = adr;
+	TWCR = ((1<<TWINT)+(1<<TWEN));
+	Wait_TWI_int();
+	if((TWSR != MTX_ADR_ACK)&&(TWSR != MRX_ADR_ACK))
+		return TWSR;
+	return SUCCESS;
+}	
+
+/*!
+ * Empfange ein Byte
+ * @param *rx_ptr Container fuer die Daten
+ * @param last_byte Flag ob noch Daten erwartet werden
+ * @return Resultat der Aktion
+ */
+ 
+uint8 Get_byte(uint8 *rx_ptr,uint8 last_byte){
+	Wait_TWI_int();
+	if(last_byte)
+		TWCR = ((1<<TWINT)+(1<<TWEA)+(1<<TWEN));
+	else
+		TWCR = ((1<<TWINT)+(1<<TWEN)); 			
+	Wait_TWI_int();
+	*rx_ptr = TWDR;
+ 	if(((TWSR == MRX_DATA_NACK)&&(last_byte == 0))||(TWSR == MRX_DATA_ACK))
+		return SUCCESS;	  
+	return TWSR;
+}
+

 #endif
diff --git a/source/Concept/Framework/atmega128io.h b/source/Concept/Framework/atmega128io.h
index ed526ae..a3384a4 100644
--- a/source/Concept/Framework/atmega128io.h
+++ b/source/Concept/Framework/atmega128io.h
@@ -178,6 +178,99 @@ 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))
 
-/**@}*/
+/**@}*/
+
+/////////////////////////////////////////////////////
+/////////////////////////////////////////////////////
+// TWI-Driver
+
+	/*!
+		Struktur Definition 
+		tx_type ist eine Datenstruktur um den TWI Treiber anzusprechen 
+		und behinhaltet folgende Informationen:
+		Slave Adresse + Datenrichtung
+		Anzahl der zu uebertragendenden Bytes (Senden oder Empfangen)
+		Pointer auf den Sende- oder Empfangspuffer
+	*/
+	typedef struct
+	{
+		uint8 slave_adr;				/*!< Slave Adresse and W/R byte */
+		uint8 size;						/*!< Anzahl der Bytes, die gesendet oder empfagen werden sollen */
+		uint8 *data_ptr;				/*!< Pointer zum Sende und Empfangs Puffer */
+	}tx_type;							
+	
+/*!
+ * Hier wird der eigentliche TWI-Treiber angesprochen
+ * @param *data_pack Container mit den Daten fuer den Treiber
+ * @return Resultat der Aktion
+ */
+ 	extern uint8 Send_to_TWI(tx_type *data_pack);	
+/*!
+ * Sende ein Byte
+ * @param data das zu uebertragende Byte
+ */
+	extern uint8 Send_byte(uint8 data);
+/*!
+ * Empfange ein Byte
+ * @param *rx_ptr Container f�r die Daten
+ * @param last_byte Flag ob noch Daten erwartet werden
+ * @return Resultat der Aktion
+ */
+	extern uint8 Get_byte(uint8 *rx_ptr, uint8 last_byte);
+/*!
+ * Sende Start Sequence
+ * @return Resultat der Aktion
+ */
+	extern uint8 Send_start(void);
+/*!
+ * Sende Slave Adresse
+ * @param adr die gewuenschte Adresse
+ * @return Resultat der Aktion
+ */
+	extern uint8 Send_adr(uint8 adr);
+/*!
+ * Sende Stop Sequence
+ */
+	extern void Send_stop(void);
+/*!
+ * Warte auf TWI interrupt
+ */
+	extern void Wait_TWI_int(void);
+
+/*!
+ * TWI Bus initialsieren
+ * @return Resultat der Aktion
+ */
+	extern int8 Init_TWI(void);
+/*!
+ * TWI Bus schliesen
+ * @return Resultat der Aktion
+ */
+	extern int8 Close_TWI(void);
+	
+	#define W 				0			/*!< Daten Transfer Richtung Schreiben */
+	#define R 				1			/*!< Daten Transfer Richtung Lesen */
+	#define OWN_ADR 		60  		/*!< Die eigene Slave Adresse */
+	#define SUCCESS 		0xFF		/*!< Status Code alles OK */
+	
+	/*!
+	TWI Stautus Register Definitionen
+	*/
+
+	/*!< Genereller Master Statuscode */
+	#define START		0x08					/*!< START wurde uebertragen */	
+	#define	REP_START	0x10					/*!< Wiederholter START wurde uebertragen */
+	/*!< Master Sender Statuscode	 */										
+	#define	MTX_ADR_ACK		0x18				/*!< SLA+W wurde uebertragen und ACK empfangen */
+	#define	MTX_ADR_NACK	0x20				/*!< SLA+W wurde uebertragen und NACK empfangen */
+	#define	MTX_DATA_ACK	0x28				/*!< Datenbyte wurde uebertragen und ACK empfangen */			
+	#define	MTX_DATA_NACK	0x30				/*!< Datenbyte wurde uebertragen und NACK empfangen */		
+	#define	MTX_ARB_LOST	0x38				/*!< Schlichtung verloren in SLA+W oder Datenbytes */
+	/*!< Master Empfaenger Statuscode  */
+	#define	MRX_ARB_LOST	0x38				/*!< Schlichtung verloren in SLA+R oder NACK bit */
+	#define	MRX_ADR_ACK		0x40				/*!< SLA+R wurde uebertragen und ACK empfangen */
+	#define	MRX_ADR_NACK	0x48				/*!< SLA+R wurde uebertragen und NACK empfangen */	
+	#define	MRX_DATA_ACK	0x50				/*!< Datenbyte wurde empfangen und ACK gesendet */
+	#define	MRX_DATA_NACK	0x58				/*!< Datenbyte wurde empfangen und NACK gesendet */
 
 #endif
diff --git a/source/Concept/Framework/modules/input/distance_sensor.c b/source/Concept/Framework/modules/input/distance_sensor.c
index e6fd3f8..1ab3755 100755
--- a/source/Concept/Framework/modules/input/distance_sensor.c
+++ b/source/Concept/Framework/modules/input/distance_sensor.c
@@ -1,41 +1,145 @@
 #include "distance_sensor.h"
+
+/*!
+ * SRF10 initialsieren
+ */
+ 
+void Distance_Sensor::srf10_init(void){
+	srf10_set_range(SRF10_MAX_RANGE);
+	//srf10_set_range(6);	//Mit diesem Wert muss man spielen um das Optimum zu ermitteln
+return;
+}
+
+/*!
+ * Verstaerkungsfaktor setzen
+ * @param gain Verstaerkungsfaktor
+ */
+ 
+void Distance_Sensor::srf10_set_gain(unsigned char gain){
+    if(gain>16) { gain=16; }
+
+	uint8 temp[2];
+	uint8 state;
+	tx_type tx_frame[2];
+	
+	state = SUCCESS;
+
+	tx_frame[0].slave_adr = this->slaveAddr+W;
+	tx_frame[0].size = 2;
+	tx_frame[0].data_ptr = temp;
+	tx_frame[0].data_ptr[0] = 1;
+	tx_frame[0].data_ptr[1] = gain;
+
+	tx_frame[1].slave_adr = OWN_ADR;
+
+	state = Send_to_TWI(tx_frame);
+}
+
+/*!
+ * Reichweite setzen, hat auch Einfluss auf die Messdauer
+ * @param millimeters Reichweite in mm
+ */
+ 
+void Distance_Sensor::srf10_set_range(unsigned int millimeters){
+	uint8 temp[2];
+	uint8 state;
+	tx_type tx_frame[2];
+	
+	state = SUCCESS;
+
+    millimeters= (millimeters/43); 
+
+	tx_frame[0].slave_adr = this->slaveAddr+W;
+	tx_frame[0].size = 2;
+	tx_frame[0].data_ptr = temp;
+	tx_frame[0].data_ptr[0] = 2;
+	tx_frame[0].data_ptr[1] = millimeters;
+
+	tx_frame[1].slave_adr = OWN_ADR;
+
+	state = Send_to_TWI(tx_frame);
+}
+
+/*!
+ * Messung ausloesen
+ * @param metric_unit 0x50 in Zoll, 0x51 in cm, 0x52 ms
+ * @return Resultat der Aktion
+ */
+
+uint8 Distance_Sensor::srf10_ping(uint8 metric_unit){
+	uint8 temp[2];
+	uint8 state;
+	tx_type tx_frame[2];
+	
+	state = SUCCESS;
+
+	tx_frame[0].slave_adr = this->slaveAddr+W;
+	tx_frame[0].size = 2;
+	tx_frame[0].data_ptr = temp;
+	tx_frame[0].data_ptr[0] = SRF10_COMMAND;
+	tx_frame[0].data_ptr[1] = metric_unit;
+
+	tx_frame[1].slave_adr = OWN_ADR;
+
+	state = Send_to_TWI(tx_frame);
+	
+	return state;
+}
+
+/*!
+ * Register auslesen
+ * @param srf10_register welches Register soll ausgelsen werden
+ * @return Inhalt des Registers
+ */
+
+uint8 Distance_Sensor::srf10_read_register(uint8 srf10_register){
+	uint8 temp;
+	uint8 value;
+	uint8 state;
+	tx_type tx_frame[3];
+
+	state = SUCCESS;
+	value = 0;
+
+	tx_frame[0].slave_adr = this->slaveAddr+W;
+	tx_frame[0].size = 1;
+	tx_frame[0].data_ptr = &temp;
+	tx_frame[0].data_ptr[0] = srf10_register;
+
+	tx_frame[1].slave_adr = this->slaveAddr+R;
+	tx_frame[1].size = 1;
+	tx_frame[1].data_ptr = &value;
+
+	tx_frame[2].slave_adr = OWN_ADR;
+
+	state = Send_to_TWI(tx_frame);
+	
+	return value;
+}
+
+/*!
+ * Messung starten Ergebniss aufbereiten und zurueckgeben
+ * @return Messergebniss
+ */
+
+uint16 Distance_Sensor::srf10_get_measure(){
+	char hib;
+	char lob;
+	char state;
+
+	state = SUCCESS;
+
+	state = srf10_ping(SRF10_CENTIMETERS);
+	usleep(10);									//Optimierungs Potential
+	lob=srf10_read_register(SRF10_LOB);
+	usleep(10);									//Optimierungs Potential
+	hib=srf10_read_register(SRF1sr0_HIB);
+	
+	return (hib*256)+lob;
+}
 
 //-----------------------------------------------------------------------------
-float Distance_Sensor::GetDistance()
+uint16 Distance_Sensor::GetDistance()
 {
-	uint32 result = 0;
-
-	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("Gen Pulse; Pin:",1,4);
-	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin));
-	
-	msleep(500);
-
-	//Generate pulse
-	*hardwareDDR |= pin;//Set pin output
-	*hardwarePort |= pin;//Activate port
-	usleep(10);//Wait for 10µs
-	*hardwareDDR &= ~pin;//Set pin input
-	*hardwarePort &= ~pin;//Deactivate port
-
-	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("Wait response; Pin:",1,4);	
-	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin));
-	
-	uint16 i;
-	//Wait for response
-	for(i = 0; (!(*hardwarePin & pin)) && (i < 1000); i++) { asm volatile("nop"); }
-	
-	//Calculate duration of response
-	while((*hardwarePin & pin)&&(result < 300000)) 
-	{
-		result++;
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
-	}
-
-	return (float(result) * DISTANCE_PER_VALUE);
+	return srf10_get_measure();
 }
diff --git a/source/Concept/Framework/modules/input/distance_sensor.h b/source/Concept/Framework/modules/input/distance_sensor.h
index 8e6aa21..9efa021 100755
--- a/source/Concept/Framework/modules/input/distance_sensor.h
+++ b/source/Concept/Framework/modules/input/distance_sensor.h
@@ -2,7 +2,21 @@
 #define _DISTANCE_SENSOR_H
 
 #include "../../stdafx.h"
-#include "sensor.h"
+#include "sensor.h"
+
+#define SRF10_MIN_GAIN        0      /*!< setze Verstaerung auf 40 */
+#define SRF10_MAX_GAIN        16     /*!< setze Verstaerung auf 700 */
+#define SRF10_MIN_RANGE       0      /*!< Minimale Reichweite 43mm */
+#define SRF10_MAX_RANGE       5977   /*!< Maximale Reichweite 5977mm */
+
+#define SRF10_INCHES          0X50	 /*!< Messung in INCHES */
+#define SRF10_CENTIMETERS     0X51	 /*!< Messung in CM */
+#define SRF10_MICROSECONDS    0X52	 /*!< Messung in Millisekunden */
+
+#define SRF10_COMMAND         0		 /*!< W=Befehls-Register R=Firmware*/
+#define SRF10_LIGHT           1		 /*!< W=Verstaerkungsfaktor R=Nicht benutzt */
+#define SRF10_HIB             2		 /*!< W=Reichweite R=Messung High-Byte */
+#define SRF10_LOB             3		 /*!< W=Nicht benutzt R=Messung Low-Byte */
 
 class Distance_Sensor : public Sensor
 {
@@ -10,11 +24,8 @@ public:
 	Distance_Sensor()
 	{
 		this->parent = NULL;
-		this->moduleId = 0;
-		this->hardwarePort = NULL;
-		this->hardwareDDR = NULL;
-		this->hardwarePin = NULL;
-		this->pin = 0;
+		this->moduleId = 0;
+		this->slaveAddr = 0; // Set the I2C Slave-Adress
 	}
 
 	Distance_Sensor(uint32 sensorId)
@@ -24,45 +35,64 @@ public:
 
 		switch(sensorId)
 		{
-			case IO_SENSOR_DISTANCE_0_DEG:
-				this->hardwarePort = &PORTC;
-				this->hardwareDDR = &DDRC;
-				this->hardwarePin = &PINC;
-				this->pin = (1 << 0);
+			case IO_SENSOR_DISTANCE_0_DEG:
+				this->slaveAddr = 0xE0; // Set the I2C Slave-Adress
 				break;
-			case IO_SENSOR_DISTANCE_90_DEG:
-				this->hardwarePort = &PORTC;
-				this->hardwareDDR = &DDRC;
-				this->hardwarePin = &PINC;
-				this->pin = (1 << 1);
+			case IO_SENSOR_DISTANCE_90_DEG:
+				this->slaveAddr = 0xE2;
 				break;
-			case IO_SENSOR_DISTANCE_180_DEG:
-				this->hardwarePort = &PORTC;
-				this->hardwareDDR = &DDRC;
-				this->hardwarePin = &PINC;
-				this->pin = (1 << 2);
+			case IO_SENSOR_DISTANCE_180_DEG:
+				this->slaveAddr = 0xE4;
 				break;
-			case IO_SENSOR_DISTANCE_270_DEG:
-				this->hardwarePort = &PORTC;
-				this->hardwareDDR = &DDRC;
-				this->hardwarePin = &PINC;
-				this->pin = (1 << 3);
+			case IO_SENSOR_DISTANCE_270_DEG:
+				this->slaveAddr = 0xE6;
 				break;
-			default:
-				this->hardwarePort = NULL;
-				this->hardwareDDR = NULL;
-				this->hardwarePin = NULL;
-				this->pin = 0;
+			default:
+				this->slaveAddr = 0;
 				break;
 		}
 	}
 
 protected:
-	//Hardware
-	volatile uint8* hardwarePort;
-	volatile uint8* hardwareDDR;
-	volatile uint8* hardwarePin;
-	uint8 pin;
+	//Hardware
+	slaveAddr;
+
+	/*!
+ * SRF10 initialsieren
+ */
+extern void		srf10_init(void);
+
+/*!
+ * Verstaerkungsfaktor setzen
+ * @param gain Verstaerkungsfaktor
+ */
+extern void		srf10_set_gain(uint8 gain);
+
+/*!
+ * Reichweite setzen, hat auch Einfluss auf die Messdauer
+ * @param millimeters Reichweite in mm
+ */
+extern void		srf10_set_range(uint16 millimeters);
+
+/*!
+ * Messung ausloesen
+ * @param metric_unit 0x50 in Zoll, 0x51 in cm, 0x52 ms
+ * @return Resultat der Aktion
+ */
+extern uint8	srf10_ping(uint8 metric_unit);
+
+/*!
+ * Register auslesen
+ * @param srf10_register welches Register soll ausgelsen werden
+ * @return Inhalt des Registers
+ */
+extern uint8	srf10_read_register(uint8 SRF10_register);
+
+/*!
+ * Messung starten Ergebniss aufbereiten und zurueckgeben
+ * @return Messergebniss
+ */
+extern uint16	srf10_get_measure(void);
 
 public:
 	float GetDistance();
-- 
cgit v1.2.3