// phidgetAdvancedServo - object to manipulate a phidget advanced servo motor controller

// phidgetAdvancedServo
//
// Constructors
//
//  phidgetAdvancedServo() - open with first available phidget IK
//
//  phidgetAdvancedServo( int serial ) - open phidget with given serial number
//
//
// Methods
//
//  void close() - close the phidget Advanced Servo
//  
//  boolean isAttached() - returns true if phidget is attached
//
//  void waitForAttachment( int timeout ) - wait for phidget to attach - with time out
//  
//  void waitForAttachment( ) - wait for phidget to attach - no timeout
//
//  int serial() - returnt serial number of attached phidget, return -1 if not attached
//  
//  int version() - return device version
//  
//  int getMotorCount() - return number of motors available
//  
//  boolean getEngaged( int index ) - returns true if indexed motor is engaged
//  
//  void setEngaged( int index, boolean state ) - engage/disengage indexed motor
// 
//  float getPosition( int index ) - returns position of indexed motor in range [-1 ... 1]
//  
//  void setPosition( int index, float position ) - set position of indexed motor in range [-1 ... 1]
//


import com.phidgets.*;
import com.phidgets.event.*;


class phidgetAdvancedServo {
  AdvancedServoPhidget advServo;
  
  
  // constructors
  // open first available advanced servo phidget
  phidgetAdvancedServo() {
    try {
      advServo = new AdvancedServoPhidget();
      advServo.openAny();
    }
    catch(Exception e) {}
  }
  
  // open andvanced servo for given serial number
  phidgetAdvancedServo( int serial ) {
    try {
      advServo = new AdvancedServoPhidget();
      advServo.open( serial );
    }
    catch(Exception e) {}
  }
  
  // close the phidget
  void close() {
    try {
      advServo.close();
    }
    catch(Exception e) {}
  }
  
  // check if interface kit is attached - returns true if attached
  boolean isAttached()
  {
    boolean result;
    
    try {
      result = advServo.isAttached();
    }
    catch(Exception e) {
      result = false;
    }
    return result;
  }
  
  // wait for phidget to attach - with time out
  void waitForAttachment( int timeout )
  {
    try {
      advServo.waitForAttachment(timeout);
    }
    catch( Exception e) {
    }
  }
  
  
  // wait for phidget to attach - no timeout
  void waitForAttachment( )
  {
    try {
      advServo.waitForAttachment();
    }
    catch( Exception e) {
    }
  }
  

  // return serial number of attached phidget, return -1 if not attached
  int serial()
  {
    int s;
    
    try {
      s = advServo.getSerialNumber();
    }
    catch( Exception e ) {
      s = -1;
    }
    return s;
  }
  
  // return device version
  int version() {
    int v;
    
    try {
      v = advServo.getDeviceVersion();
    }
    catch( Exception e ) {
      v = -1;
    }
    return v;
  }
  
  // return the number of motors available
  int getMotorCount() {
    int c;
    
    try {
      c = advServo.getMotorCount();
    }
    catch( Exception e ) {
      c = 0;
    }
    return c;
  }
  
  
  // return true if inedexed motor is engaged
  boolean getEngaged( int index ) {
    boolean engaged;
    
    try {
      engaged = advServo.getEngaged( index );
    }
    catch( Exception e ) {
      engaged = false;
    }
    return engaged;
  }
  
  
  // engage/disengage indexed motor
  void setEngaged( int index, boolean state ) {
    try {
      advServo.setEngaged( index, state );
    }
    catch( Exception e ) {
    }
  }
  
 
  // returns position of indexed motor
  float getPosition( int index ) {
    double p;
    
    try {
      p = advServo.getPosition( index );
    }
    catch( Exception e ) {
      p = 0;
    }
    float t = 10.6 * ( (float)p + 23.0 );
    return (t - 1500.0)/500.0;
  }
  
  
  // set position of indexed motor
  void setPosition( int index, float position ) {
    float t = map( position, -1.0, 1.0, 1000.0, 2000.0 );   // pulse length from normalized position [-1 ... 1]
    t = max( min( t, 2000.0 ), 1000.0 );                    // clamp the width of the timing pulse
    double angle = t / 10.6 - 23.0;
    
    try {
      advServo.setPosition( index, angle );
    }
    catch( Exception e ) {
    }
  }
  
  
}