1 files changed, 140 insertions, 36 deletions

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();

 }