blob: 508eeed9b3b7783d8ba7aea49e593153c415cff3 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
|
#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
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1);
//Wait for response
for(int i=0;(!(PINC & pin))&&(i < 1000);i++) {asm volatile("nop");}
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1);
//Calculate duration of response
while((*hardwarePin & pin)&&(result < 300000))
{
result++;
asm volatile("nop");
}
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1);
return (float(result) * DISTANCE_PER_VALUE);
}
|
