//------------------------------------------------------------------
// 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