-
Notifications
You must be signed in to change notification settings - Fork 1
/
rangefinder.c
102 lines (97 loc) · 2.99 KB
/
rangefinder.c
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
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
/*
* rangefinder.c for the URM37 ultrasonic rangefinder used with
* the Raspberry Pi
* TODO: use a wiringPi interrupt thread to read the range
*/
#include <stdio.h>
#include <wiringPi.h>
#include "serial.h"
#include "time.h"
#define COMPTRIG (4)
int uart;
//char autoModeCmd[] = {0x44, 0x02, 0xaa, 0xf0};
// set the rangefinder mode and gpio pins to read
int initRangefinder()
{
if(wiringPiSetupGpio() == -1)
{
printf("rangefinder: failed to init wiringPi\n");
return -1;
}
// Init the uart at 9600 baud
if((uart = serialOpen("/dev/ttyAMA0", 9600)) <= 0)
{
printf("rangefinder: failed to init uart\n");
return -1;
}
// // start the rangefinder in autonomous mode
// writeSerial(autoModeCmd, 4, uart);
// // init the rangefinder gpios
// pinMode(COMPTRIG, INPUT);
// pullUpDnControl(COMPTRIG, PUD_UP);
return 0;
}
// Use uart commands to get range
int getRange()
{
char buf[] = {0,0,0,0,0,0,0,0,0,0,0,0};
// Note: servo degree goes here at a later time
char readRangeCmd[] = {0x22, 0x00, 0x00, 0x22};
unsigned char sum;
int count, i, range=-1;
writeSerial(readRangeCmd, 4, uart);
// allow some time for rangefinder to respond
for(i=0;i<5;i++)
{
if(serialDataAvail(uart) > 3) break;
delay(5);
}
if((serialDataAvail(uart)%4)>0) return range; // incomplete packet on uart
// handle getting up to three responses back
count = readSerial(buf, 12, uart);
for(i=0;i<count;i+=4)
{
sum = (buf[i]+buf[i+1]+buf[i+2]);
// ignore bad checksum or invalid returned by rangefinder
if(sum != buf[i+3] || (buf[i+1] == 0xff) || ((buf[i+1]<<8) | buf[i+2]) < 3)
continue;
range = ((buf[i+1]<<8) | buf[i+2]);
}
return range;
}
/* Note: No longer using the pwmin pin
// get the range from the pwm out - NOTE: Prefered mode is to read the
// comptrig pin as this function uses a hard loop and is not accurate.
int getRange()
{
int range;
uint64_t start=0, stop=0, pulseWidth;
do
{
while(digitalRead(PWMIN) == LOW); // wait for high signal
while(digitalRead(PWMIN) == HIGH); // wait for falling edge
start = getuSecs(); // start of the low pulse
while(digitalRead(PWMIN) == LOW); // wait for the end of the pulse
stop = getuSecs(); // end of the low pulse
pulseWidth = stop - start; // length of pulse in uSecs
range=(int)((pulseWidth/50.0)+0.5); // every 50us of pulse = 1cm
} while(range < 3 || range > 600); // valid range is 1cm - 500cm
return range;
}
*/
// set the range (cm) threshold for comptrig pin in autonomous mode
// WARNING: Use this sparingly as it writes to the EEPROM
void setRange(int range)
{
char rangeLo[] = {0x44, 0x00, 0x00, 0x00};
char rangeHi[] = {0x44, 0x01, 0x00, 0x00};
// ignore if not a valid range threshold
if(range > 400 || range < 4)
return;
rangeLo[2] = range&0xff;
rangeHi[2] = (range>>8)&0xff;
rangeLo[3] = (rangeLo[0]+rangeLo[1]+rangeLo[2])&0xff;
rangeHi[3] = (rangeHi[0]+rangeHi[1]+rangeHi[2])&0xff;
writeSerial(rangeHi, 4, uart);
writeSerial(rangeLo, 4, uart);
}