107 lines
2 KiB
C
107 lines
2 KiB
C
|
//------------------------------------------------------------------
|
||
|
|
// qfixSonar.h
|
||
|
|
//
|
||
|
|
// This file contains the class Sonar which represents a
|
||
|
|
// sonar modul SFH04/08.
|
||
|
|
//
|
||
|
|
// Copyright 2005 by KTB mechatronics GmbH
|
||
|
|
// Author: Florian Schrapp, Stefan Enderle
|
||
|
|
// Version 1.0
|
||
|
|
//------------------------------------------------------------------
|
||
|
|
|
||
|
|
|
||
|
|
#ifndef qfixSonar_h
|
||
|
|
#define qfixSonar_h
|
||
|
|
|
||
|
|
#include "qfix.h"
|
||
|
|
#include "qfixI2C.h"
|
||
|
|
|
||
|
|
|
||
|
|
class Sonar
|
||
|
|
{
|
||
|
|
private:
|
||
|
|
uint8_t id;
|
||
|
|
I2C i2c;
|
||
|
|
|
||
|
|
public:
|
||
|
|
|
||
|
|
/** Constructor with given ID
|
||
|
|
*/
|
||
|
|
Sonar(uint8_t ID);
|
||
|
|
|
||
|
|
/** Change logical device ID
|
||
|
|
*/
|
||
|
|
void changeID(uint8_t newID);
|
||
|
|
|
||
|
|
/** Returns the distance to the next object in millimeter
|
||
|
|
*/
|
||
|
|
int distance();
|
||
|
|
};
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
Sonar::Sonar(uint8_t ID) : i2c()
|
||
|
|
{
|
||
|
|
id = 112+ID;
|
||
|
|
|
||
|
|
uint8_t buf[10];
|
||
|
|
buf[0]=2; // register 2 = select range register
|
||
|
|
buf[1]=48; // set maximum range to 2107 mm (formula: range=(register*43)+43 mm)
|
||
|
|
i2c.send(id, buf, 2);
|
||
|
|
|
||
|
|
buf[0]=1; // register 1 = amplification
|
||
|
|
buf[1]=4; // improve detection along center axis
|
||
|
|
i2c.send(id, buf, 2);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void Sonar::changeID(uint8_t newID)
|
||
|
|
{
|
||
|
|
if(newID>=0 && newID<=15){ // only 16 IDs allowed
|
||
|
|
uint8_t buf[10];
|
||
|
|
|
||
|
|
buf[0]=0;
|
||
|
|
buf[1]=160;
|
||
|
|
i2c.send(id, buf, 2);
|
||
|
|
|
||
|
|
buf[0]=0;
|
||
|
|
buf[1]=170;
|
||
|
|
i2c.send(id, buf, 2);
|
||
|
|
|
||
|
|
buf[0]=0;
|
||
|
|
buf[1]=165;
|
||
|
|
i2c.send(id, buf, 2);
|
||
|
|
|
||
|
|
buf[0]=0;
|
||
|
|
buf[1]=(newID*2)+224;
|
||
|
|
i2c.send(id, buf, 2);
|
||
|
|
|
||
|
|
id=112+newID;
|
||
|
|
} //else error!
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
int Sonar::distance()
|
||
|
|
{
|
||
|
|
uint8_t buf[10];
|
||
|
|
|
||
|
|
buf[0]=0; // start measurement
|
||
|
|
buf[1]=81;
|
||
|
|
i2c.send(id, buf, 2);
|
||
|
|
msleep(70); // wait for echo!!! (about 65ms)
|
||
|
|
|
||
|
|
i2c.send(id, 3); // select byte to be read
|
||
|
|
|
||
|
|
i2c.get(id, buf, 1); // read first byte
|
||
|
|
uint8_t r1=buf[0];
|
||
|
|
|
||
|
|
i2c.send(id, 2); // select byte to be read
|
||
|
|
|
||
|
|
i2c.get(id, buf, 1); // read second byte
|
||
|
|
uint8_t r2=buf[0];
|
||
|
|
|
||
|
|
return ((r1+(256*r2))*10); // compute mm
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
#endif
|