/*
 *  distancesensor.h
 *  serialtest
 *
 *  Created by sicrash on 5/1/07.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#ifndef DISTANCESENSOR
#define DISTANCESENSOR

#include "dynamixel.h"

#define P_DISTANCE     1

class DistanceSensor : public Dynamixel
{  
  public:
    DistanceSensor(int id, Connection *conn) : Dynamixel(id, conn)
    {
    }
        
    int getDistance(int *xdist, int *ydist)
    {
      byte dist[4];
      int r = readByte(P_DISTANCE, dist, 4);
      //int r = this->read(P_DISTANCE, dist, 4);      
      *xdist = byte2word_s(dist[1], dist[0]);
      *ydist = (signed char) dist[2];
      *ydist = -*ydist;
      
      return r;
    }
  
    int reset()
    {      
      Packet pkt(id, INST_RESET, NULL, 0);
      return ( send(&pkt, &pkt) == NO_ERROR );
    }
};

#endif
