diff options
Diffstat (limited to 'source/Concept/Framework/modules/input/distance_sensor.c')
| -rwxr-xr-x | source/Concept/Framework/modules/input/distance_sensor.c | 19 |
1 files changed, 17 insertions, 2 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);
|