import lejos.nxt.*;
/*
 * Mindsensors NXTServo sensor class
 * CH,Chen 
 * Aug. 2008
 */
public class NXTServo extends I2CSensor {
	private byte[] buf = new byte[8];
	
	public NXTServo (I2CPort port ) {
		super(port);
		setAddress(0x58); //I2C address 0xb0 convert to 7 bits
	}
	
	public int getBatteryVoltage() {
		int ret = getData(0x41, buf, 1);
		if(ret != 0) return -1;
		//return (buf[0] & 0xff);
		return ((buf[0] & 0xff)*37); // Convert to voltage
	}
/*
 * Get/Set position registers: (0x42,0x43)~(0x50,0x51) of servo id: 1~8
 */	
	public int getServoPosition(int id) {
		int ret = getData(0x42+2*(id-1), buf, 2);
		if(ret != 0) return -1;
		return (buf[0] & 0xff)+((buf[1] & 0xff)*255);
	}
	
	public void setServoPosition(int id, int pos) {
		buf[0] = (byte)(pos / 255);
		buf[1] = (byte)(pos % 255);
		sendData(0x42 + 2*(id-1), buf, 2);
	}
/*
 * Setting quick position registers: 0x5a~0x61 of servo id: 1~8
 * 10th the resolution of Servo position registers(50~255,neutral:150)
 */	
	public void setServoQuickPosition(int id, int pos) {
		sendData(0x5a+id-1, (byte)pos);
	}
/*
 * Setting quick position from fmid th servo and next cnt servos
 */	
	public void setGroupQuickPosition(int fmid, int cnt, int pos) {
		for (int xii=0; xii<cnt; xii++) {
			buf[xii] = (byte)pos;
		}
		sendData(0x5a+fmid-1, buf, cnt);
	}
/* 
 * Get/Set speed registers: 0x52~0x59 of servo id: 1~8
 */
	public byte getServoSpeed(int id) {
		int ret = getData(0x52+id-1, buf, 1);
		if(ret != 0) return -1;
		return (buf[0]);
	}
	
	public void setServoSpeed(int id, int speed) {
		sendData(0x52+id-1, (byte)speed);
	}
/*
 * Store IDth servo current position and speed to permanent memory
 * Command: In (n: servo id 1~8)
 */	
	public void storeServoProperty(int id) {
		buf[0] = 0x49;
		buf[1] = (byte)id;
		sendData(0x41, buf, 2);
	}
/*
 * Reset all servo to factory setting: position 1500, speed 0
 * Command: S
 */	
	public void resetServoDefault() {
		sendData(0x41, (byte)0x53);
	}
}