import lejos.nxt.*;
/*
 * Mindsensors NXTServo testing program
 * CH, Chen (Taiwan)
 * Aug. 2008
 */
public class NXTServoTest extends NXTServo {
	NXTServo sv;
	static final int DELAY = 500;
	
	private NXTServoTest() {
		super(SensorPort.S1);
		sv = new NXTServo(SensorPort.S1);
	}
	
	public static void main(String[] args) {		
		new NXTServoTest().goStatus();
		new NXTServoTest().goTest();
	}
/*
 * Display Nxtservo status
 */
	private void goStatus() {
		String spid = sv.getSensorType();
		LCD.clear();
		LCD.drawString("Type: "+spid, 0, 0);
			
		int xvol = sv.getBatteryVoltage();
		LCD.drawString("Voltage: "+xvol, 0, 1);
		LCD.drawString("Press Enter ...", 0, 3);
		while (!Button.ENTER.isPressed()) {};

		for (int xii=1; xii<=8; xii++){
			int xpos   = sv.getServoPosition(xii);
			int xspeed = (0xff & sv.getServoSpeed(xii));
			LCD.clear();
			LCD.drawString("Servo "+xii+" setting", 0, 1);
			LCD.drawString("Pos  : "+xpos, 0, 2);
			LCD.drawString("Speed: "+xspeed, 0, 3);
			sleep();
		}
		LCD.drawString("Press Enter ...", 0, 5);
		while (!Button.ENTER.isPressed()) {};
	}
			
/*
 * Testing
 */
	private void goTest() {
		final int [] pos = {250, 150, 50, 250, 50, 150};
		for (int xii=1; xii<=6; xii++) {
			LCD.clear();
			LCD.drawString("Testing ...", 0, 0);
			LCD.drawString("Servo: "+xii, 0, 1);
			for (int xjj = 0; xjj<6; xjj++) {
				LCD.drawString("  Pos:"+pos[xjj]+" ", 0, 2);
				sv.setServoQuickPosition(xii, pos[xjj]);
				sleep();
			}
		}
		
		LCD.clear();
		LCD.drawString("Group testing ...", 0, 1);
		for (int xii=1; xii<=4; xii++) {
			LCD.drawString("Servo "+xii+" next 3", 0, 2);
			sv.setGroupQuickPosition(xii, 3, 50);
			sleep();
			sv.setGroupQuickPosition(xii, 3, 250);
			sleep();
			sv.setGroupQuickPosition(xii, 3, 150);
			sleep();
		}
	}
	 
	private void sleep() {
		try {
			Thread.sleep(DELAY);
		} catch (InterruptedException e) {}
	}
}