3 files changed, 25 insertions, 3 deletions

diff --git a/source/Concept/Framework/modules/input/distance_sensor.c b/source/Concept/Framework/modules/input/distance_sensor.c
index 262d35f..e6fd3f8 100755
--- a/source/Concept/Framework/modules/input/distance_sensor.c
+++ b/source/Concept/Framework/modules/input/distance_sensor.c
@@ -5,21 +5,36 @@ float 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

-	*hardwarePort &= ~pin;//Deactivate port

 	*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(uint16 i = 0; (!(*hardwarePin & pin)) && (i < 1000); i++) { asm volatile("nop"); }

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

diff --git a/source/Concept/Framework/modules/input/ir_sensor.c b/source/Concept/Framework/modules/input/ir_sensor.c
index c34feed..12d74e0 100755
--- a/source/Concept/Framework/modules/input/ir_sensor.c
+++ b/source/Concept/Framework/modules/input/ir_sensor.c
@@ -5,5 +5,5 @@ uint16 IR_Sensor::GetIRIntensity()
 {
 	if(!parent) return 0;

 

-	return parent->GetADCValue(channel);

+	return min(parent->GetADCValue(channel) + this->intensityCorrection, 1023);

 }

diff --git a/source/Concept/Framework/modules/input/ir_sensor.h b/source/Concept/Framework/modules/input/ir_sensor.h
index 30e6ea4..74396ec 100755
--- a/source/Concept/Framework/modules/input/ir_sensor.h
+++ b/source/Concept/Framework/modules/input/ir_sensor.h
@@ -12,12 +12,14 @@ public:
 	{

 		this->parent = NULL;

 		this->moduleId = 0;

+		this->intensityCorrection = 0;

 	}

 

 	IR_Sensor(uint32 sensorId)

 	{

 		this->parent = NULL;

 		this->moduleId = sensorId;

+		this->intensityCorrection = 0;

 

 		switch(sensorId)

 		{

@@ -32,12 +34,15 @@ public:
 				break;

 			case IO_SENSOR_IR_100_DEG:

 				this->channel = 3;

+				this->intensityCorrection = 40;

 				break;

 			case IO_SENSOR_IR_180_DEG:

 				this->channel = 4;

+				this->intensityCorrection = 50;

 				break;

 			case IO_SENSOR_IR_260_DEG:

 				this->channel = 5;

+				this->intensityCorrection = 70;

 				break;

 			case IO_SENSOR_IR_300_DEG:

 				this->channel = 6;

@@ -55,6 +60,8 @@ protected:
 	//Hardware

 	uint8 channel;

 

+	uint8 intensityCorrection;

+

 public:

 	uint16 GetIRIntensity();

 };