<commit_before>package org.myrobotlab.service;

import org.myrobotlab.framework.Peers;
import org.myrobotlab.framework.Service;
import org.myrobotlab.framework.Status;
import org.myrobotlab.logging.LoggerFactory;
import org.slf4j.Logger;

/**
 * InMoovHead - This is the inmoov head service.
 * This service controls the servos for the following:
 * jaw, eyeX, eyeY, rothead and neck.
 * 
 */
public class InMoovHead extends Service {

	private static final long serialVersionUID = 1L;

	public final static Logger log = LoggerFactory.getLogger(InMoovHead.class);

	transient public Servo jaw;
	transient public Servo eyeX;
	transient public Servo eyeY;
	transient public Servo rothead;
	transient public Servo neck;
	transient public Arduino arduino;

	// static in Java are not overloaded but overwritten - there is no
	// polymorphism for statics
	public static Peers getPeers(String name) {
		Peers peers = new Peers(name);

		peers.put("jaw", "Servo", "Jaw servo");
		peers.put("eyeX", "Servo", "Eyes pan servo");
		peers.put("eyeY", "Servo", "Eyes tilt servo");
		peers.put("rothead", "Servo", "Head pan servo");
		peers.put("neck", "Servo", "Head tilt servo");
		peers.put("arduino", "Arduino", "Arduino controller for this arm");

		return peers;
	}

	public InMoovHead(String n) {
		super(n);
		jaw = (Servo) createPeer("jaw");
		eyeX = (Servo) createPeer("eyeX");
		eyeY = (Servo) createPeer("eyeY");
		rothead = (Servo) createPeer("rothead");
		neck = (Servo) createPeer("neck");
		arduino = (Arduino) createPeer("arduino");

		// connection details
		neck.setPin(12);
		rothead.setPin(13);
		jaw.setPin(26);
		eyeX.setPin(22);
		eyeY.setPin(24);

		neck.setController(arduino);
		rothead.setController(arduino);
		jaw.setController(arduino);
		eyeX.setController(arduino);
		eyeY.setController(arduino);

		neck.setMinMax(20, 160);
		rothead.setMinMax(30, 150);
		// reset by mouth control
		jaw.setMinMax(10, 25);
		eyeX.setMinMax(60, 100);
		eyeY.setMinMax(50, 100);

		neck.setRest(90);
		rothead.setRest(90);
		jaw.setRest(10);
		eyeX.setRest(80);
		eyeY.setRest(90);

	}

	/**
	 * attach all the servos - this must be re-entrant and accomplish the
	 * re-attachment when servos are detached
	 * 
	 * @return
	 */
	public boolean attach() {
		sleep(InMoov.attachPauseMs);
		eyeX.attach();
		sleep(InMoov.attachPauseMs);
		eyeY.attach();
		sleep(InMoov.attachPauseMs);
		jaw.attach();
		sleep(InMoov.attachPauseMs);
		rothead.attach();
		sleep(InMoov.attachPauseMs);
		neck.attach();

		return true;
	}

	// FIXME - should be broadcastServoState
	@Override
	public void broadcastState() {
		// notify the gui
		rothead.broadcastState();
		neck.broadcastState();
		eyeX.broadcastState();
		eyeY.broadcastState();
		jaw.broadcastState();
	}

	// FIXME - make interface for Arduino / Servos !!!
	public boolean connect(String port) throws Exception {
		arduino.connect(port);
		
		attach();
		setSpeed(0.5f, 0.5f, 0.5f, 0.5f, 0.5f);
		rest();
		sleep(1000);
		setSpeed(1.0f, 1.0f, 1.0f, 1.0f, 1.0f);
		broadcastState();
		return true;
	}

	public void detach() {
		sleep(InMoov.attachPauseMs);
		rothead.detach();
		sleep(InMoov.attachPauseMs);
		neck.detach();
		sleep(InMoov.attachPauseMs);
		eyeX.detach();
		sleep(InMoov.attachPauseMs);
		eyeY.detach();
		sleep(InMoov.attachPauseMs);
		jaw.detach();
	}

	@Override
	public String[] getCategories() {
		return new String[] { "robot" };
	}

	@Override
	public String getDescription() {
		return "InMoov Head Service";
	}

	public long getLastActivityTime() {

		long lastActivityTime = Math.max(rothead.getLastActivityTime(), neck.getLastActivityTime());
		lastActivityTime = Math.max(lastActivityTime, eyeX.getLastActivityTime());
		lastActivityTime = Math.max(lastActivityTime, eyeY.getLastActivityTime());
		lastActivityTime = Math.max(lastActivityTime, jaw.getLastActivityTime());
		return lastActivityTime;
	}

	public String getScript(String inMoovServiceName) {
		return String.format("%s.moveHead(%d,%d,%d,%d,%d)\n", inMoovServiceName, neck.getPos(), rothead.getPos(), eyeX.getPos(), eyeY.getPos(), jaw.getPos());
	}

	public boolean isAttached() {
		boolean attached = false;

		attached |= rothead.isAttached();
		attached |= neck.isAttached();
		attached |= eyeX.isAttached();
		attached |= eyeY.isAttached();
		attached |= jaw.isAttached();

		return attached;
	}

	public boolean isValid() {
		rothead.moveTo(rothead.getRest() + 2);
		neck.moveTo(neck.getRest() + 2);
		eyeX.moveTo(eyeX.getRest() + 2);
		eyeY.moveTo(eyeY.getRest() + 2);
		jaw.moveTo(jaw.getRest() + 2);
		return true;
	}

	public void moveTo(Integer neck, Integer rothead) {
		moveTo(neck, rothead, null, null, null);
	}

	public void moveTo(Integer neck, Integer rothead, Integer eyeX, Integer eyeY) {<commit_msg>Added the method lookAt(x,y,z) to translate a point (x,y,z) to a spherical system of coordinates (distance, rothead, neck)<commit_after>package org.myrobotlab.service;

import org.myrobotlab.framework.Peers;
import org.myrobotlab.framework.Service;
import org.myrobotlab.framework.Status;
import org.myrobotlab.logging.LoggerFactory;
import org.slf4j.Logger;

/**
 * InMoovHead - This is the inmoov head service.
 * This service controls the servos for the following:
 * jaw, eyeX, eyeY, rothead and neck.
 * 
 */
public class InMoovHead extends Service {

	private static final long serialVersionUID = 1L;

	public final static Logger log = LoggerFactory.getLogger(InMoovHead.class);

	transient public Servo jaw;
	transient public Servo eyeX;
	transient public Servo eyeY;
	transient public Servo rothead;
	transient public Servo neck;
	transient public Arduino arduino;

	// static in Java are not overloaded but overwritten - there is no
	// polymorphism for statics
	public static Peers getPeers(String name) {
		Peers peers = new Peers(name);

		peers.put("jaw", "Servo", "Jaw servo");
		peers.put("eyeX", "Servo", "Eyes pan servo");
		peers.put("eyeY", "Servo", "Eyes tilt servo");
		peers.put("rothead", "Servo", "Head pan servo");
		peers.put("neck", "Servo", "Head tilt servo");
		peers.put("arduino", "Arduino", "Arduino controller for this arm");

		return peers;
	}

	public InMoovHead(String n) {
		super(n);
		jaw = (Servo) createPeer("jaw");
		eyeX = (Servo) createPeer("eyeX");
		eyeY = (Servo) createPeer("eyeY");
		rothead = (Servo) createPeer("rothead");
		neck = (Servo) createPeer("neck");
		arduino = (Arduino) createPeer("arduino");

		// connection details
		neck.setPin(12);
		rothead.setPin(13);
		jaw.setPin(26);
		eyeX.setPin(22);
		eyeY.setPin(24);

		neck.setController(arduino);
		rothead.setController(arduino);
		jaw.setController(arduino);
		eyeX.setController(arduino);
		eyeY.setController(arduino);

		neck.setMinMax(20, 160);
		rothead.setMinMax(30, 150);
		// reset by mouth control
		jaw.setMinMax(10, 25);
		eyeX.setMinMax(60, 100);
		eyeY.setMinMax(50, 100);

		neck.setRest(90);
		rothead.setRest(90);
		jaw.setRest(10);
		eyeX.setRest(80);
		eyeY.setRest(90);

	}

	/**
	 * attach all the servos - this must be re-entrant and accomplish the
	 * re-attachment when servos are detached
	 * 
	 * @return
	 */
	public boolean attach() {
		sleep(InMoov.attachPauseMs);
		eyeX.attach();
		sleep(InMoov.attachPauseMs);
		eyeY.attach();
		sleep(InMoov.attachPauseMs);
		jaw.attach();
		sleep(InMoov.attachPauseMs);
		rothead.attach();
		sleep(InMoov.attachPauseMs);
		neck.attach();

		return true;
	}

	// FIXME - should be broadcastServoState
	@Override
	public void broadcastState() {
		// notify the gui
		rothead.broadcastState();
		neck.broadcastState();
		eyeX.broadcastState();
		eyeY.broadcastState();
		jaw.broadcastState();
	}

	// FIXME - make interface for Arduino / Servos !!!
	public boolean connect(String port) throws Exception {
		arduino.connect(port);
		
		attach();
		setSpeed(0.5f, 0.5f, 0.5f, 0.5f, 0.5f);
		rest();
		sleep(1000);
		setSpeed(1.0f, 1.0f, 1.0f, 1.0f, 1.0f);
		broadcastState();
		return true;
	}

	public void detach() {
		sleep(InMoov.attachPauseMs);
		rothead.detach();
		sleep(InMoov.attachPauseMs);
		neck.detach();
		sleep(InMoov.attachPauseMs);
		eyeX.detach();
		sleep(InMoov.attachPauseMs);
		eyeY.detach();
		sleep(InMoov.attachPauseMs);
		jaw.detach();
	}

	@Override
	public String[] getCategories() {
		return new String[] { "robot" };
	}

	@Override
	public String getDescription() {
		return "InMoov Head Service";
	}

	public long getLastActivityTime() {

		long lastActivityTime = Math.max(rothead.getLastActivityTime(), neck.getLastActivityTime());
		lastActivityTime = Math.max(lastActivityTime, eyeX.getLastActivityTime());
		lastActivityTime = Math.max(lastActivityTime, eyeY.getLastActivityTime());
		lastActivityTime = Math.max(lastActivityTime, jaw.getLastActivityTime());
		return lastActivityTime;
	}

	public String getScript(String inMoovServiceName) {
		return String.format("%s.moveHead(%d,%d,%d,%d,%d)\n", inMoovServiceName, neck.getPos(), rothead.getPos(), eyeX.getPos(), eyeY.getPos(), jaw.getPos());
	}

	public boolean isAttached() {
		boolean attached = false;

		attached |= rothead.isAttached();
		attached |= neck.isAttached();
		attached |= eyeX.isAttached();
		attached |= eyeY.isAttached();
		attached |= jaw.isAttached();

		return attached;
	}

	public boolean isValid() {
		rothead.moveTo(rothead.getRest() + 2);
		neck.moveTo(neck.getRest() + 2);
		eyeX.moveTo(eyeX.getRest() + 2);
		eyeY.moveTo(eyeY.getRest() + 2);
		jaw.moveTo(jaw.getRest() + 2);
		return true;
	}
	
	public void lookAt(Double x, Double y,Double z) {
		Double distance = Math.sqrt(Math.pow(x, 2.0)+ Math.pow(y, 2.0) + Math.pow(z, 2.0));
		Double rotation = Math.toDegrees(Math.atan(y/x));
		Double colatitude = Math.toDegrees(Math.acos(z/distance));
		System.out.println(distance);
		System.out.println(rotation);
		System.out.println(colatitude);
		log.info(String.format("object distance is %f,rothead servo %f,neck servo %f ", distance, rotation, colatitude));
	}

	public void moveTo(Integer neck, Integer rothead) {
		moveTo(neck, rothead, null, null, null);
	}

	public void moveTo(Integer neck, Integer rothead, Integer eyeX, Integer eyeY) {
{'fraction_non_alphanumeric': 0.1437908496732026, 'fraction_numerical': 0.015448603683897801, 'mean_word_length': 4.4737127371273715, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5617823', 'n_tokens_mistral': 4145, 'n_tokens_neox': 3615, 'n_words': 798}