diff --git a/depthai b/depthai new file mode 160000 index 0000000000..65fc1c6ad7 --- /dev/null +++ b/depthai @@ -0,0 +1 @@ +Subproject commit 65fc1c6ad7495929c0f58897067af2d88f62f0f8 diff --git a/src/main/java/VectorAngleCalculator.java b/src/main/java/VectorAngleCalculator.java new file mode 100644 index 0000000000..84501a49c8 --- /dev/null +++ b/src/main/java/VectorAngleCalculator.java @@ -0,0 +1,45 @@ +import java.util.Vector; + +public class VectorAngleCalculator { + public static double calculateAngle(Vector vector1, Vector vector2) { + // Check if the vectors have the same dimension + if (vector1.size() != vector2.size()) { + throw new IllegalArgumentException("Vectors must have the same dimension"); + } + + // Calculate the dot product of the vectors + double dotProduct = 0.0; + double magnitude1 = 0.0; + double magnitude2 = 0.0; + + for (int i = 0; i < vector1.size(); i++) { + dotProduct += vector1.get(i) * vector2.get(i); + magnitude1 += Math.pow(vector1.get(i), 2); + magnitude2 += Math.pow(vector2.get(i), 2); + } + + magnitude1 = Math.sqrt(magnitude1); + magnitude2 = Math.sqrt(magnitude2); + + // Calculate the angle in radians + double radians = Math.acos(dotProduct / (magnitude1 * magnitude2)); + + // Convert radians to degrees + double degrees = Math.toDegrees(radians); + + return degrees; + } + + public static void main(String[] args) { + Vector vector1 = new Vector<>(); + vector1.add(1.0); + vector1.add(0.0); + + Vector vector2 = new Vector<>(); + vector2.add(1.0); + vector2.add(1.0); + + double angleDegrees = calculateAngle(vector1, vector2); + System.out.println("Angle between vectors: " + angleDegrees + " degrees"); + } +} diff --git a/src/main/java/org/myrobotlab/service/InMoov2.java b/src/main/java/org/myrobotlab/service/InMoov2.java index 2e328bdf87..dd57473b24 100644 --- a/src/main/java/org/myrobotlab/service/InMoov2.java +++ b/src/main/java/org/myrobotlab/service/InMoov2.java @@ -109,6 +109,7 @@ public static boolean loadFile(String file) { protected transient ProgramAB chatBot; protected List configList; + /** * Configuration from runtime has started. This is when runtime starts * processing a configuration set for the first time since inmoov was started @@ -633,6 +634,10 @@ public InMoov2Hand getRightHand() { } public String getState() { + FiniteStateMachine fsm = (FiniteStateMachine) getPeer("fsm"); + if (fsm == null) { + return null; + } return fsm.getCurrent(); } @@ -1081,7 +1086,7 @@ public void onGestureStatus(Status status) { * A generalized recurring event which can preform checks and various other * methods or tasks. Heartbeats will not start until after boot stage. */ - public void onHeartbeat() { + public void onHeartbeat(String name) { try { // heartbeats can start before config is // done processing - so the following should @@ -1117,10 +1122,10 @@ public void onHeartbeat() { } if (config.pirOnFlash && isPeerStarted("pir") && isPirOn) { - flash("pirOn"); + flash("pir"); } - if (config.batteryLevelCheck) { + if (config.batteryInSystem) { double batteryLevel = Runtime.getBatteryLevel(); invoke("publishBatteryLevel", batteryLevel); // FIXME - thresholding should always have old value or state @@ -1135,7 +1140,7 @@ public void onHeartbeat() { } // flash error until errors are cleared - if (config.healthCheckFlash) { + if (config.flashOnErrors) { if (errors.size() > 0) { invoke("publishFlash", "error"); } else { @@ -1174,6 +1179,7 @@ public void onJointAngles(Map angleMap) { public void onJoystickInput(JoystickData input) throws Exception { // TODO timer ? to test and not send an event // switches to manual control ? + invoke("publishEvent", "joystick"); } /** @@ -1276,7 +1282,7 @@ public void onPirOn() { } /** - * Pir off callback + * Pir off callback - FIXME NEEDS WORK */ public void onPirOff() { isPirOn = false; @@ -1429,6 +1435,7 @@ public void onText(String text) { invoke("publishText", text); } + // TODO FIX/CHECK this, migrate from python land public void powerDown() { // publishFlash(maxInactivityTimeSeconds, maxInactivityTimeSeconds, // maxInactivityTimeSeconds, maxInactivityTimeSeconds, @@ -1462,6 +1469,12 @@ public void publish(String name, String method, Object... data) { invoke("publishMessage", msg); } + public String publishStartConfig(String configName) { + info("config %s started", configName); + invoke("publishEvent", "CONFIG STARTED " + configName); + return configName; + } + public double publishBatteryLevel(double d) { return d; } @@ -1814,11 +1827,6 @@ public void setRightHandSpeed(Double thumb, Double index, Double majeure, Double setHandSpeed("right", thumb, index, majeure, ringFinger, pinky, wrist); } - // ----------------------------------------------------------------------------- - // These are methods added that were in InMoov1 that we no longer had in - // InMoov2. - // From original InMoov1 so we don't loose the - public void setRightHandSpeed(Integer thumb, Integer index, Integer majeure, Integer ringFinger, Integer pinky, Integer wrist) { setHandSpeed("right", (double) thumb, (double) index, (double) majeure, (double) ringFinger, (double) pinky, (double) wrist); } diff --git a/src/main/java/org/myrobotlab/service/config/InMoov2Config.java b/src/main/java/org/myrobotlab/service/config/InMoov2Config.java index a766ae73f8..d90bd6851d 100644 --- a/src/main/java/org/myrobotlab/service/config/InMoov2Config.java +++ b/src/main/java/org/myrobotlab/service/config/InMoov2Config.java @@ -31,12 +31,12 @@ public class InMoov2Config extends ServiceConfig { * When the healthCheck is operating, it will check the battery level. * If the battery level is < 5% it will publishFlash with red at regular interval */ - public boolean batteryLevelCheck = false; + public boolean batteryInSystem = false; /** * enable custom sound map for state changes */ - public boolean customSounds = false; + public boolean customSound = false; public boolean forceMicroOnIfSleeping = true; @@ -46,13 +46,6 @@ public class InMoov2Config extends ServiceConfig { */ public boolean healthCheckFlash = true; - /** - * Single heartbeat to drive InMoov2 .. it can check status, healthbeat, - * and fire events to the FSM. - * Checks battery level and sends a heartbeat flash on publishHeartbeat - * and onHeartbeat at a regular interval - */ - public boolean heartbeat = true; /** * flashes the neopixel every time a health check is preformed. @@ -60,6 +53,14 @@ public class InMoov2Config extends ServiceConfig { * red == battery < 5% */ public boolean heartbeatFlash = false; + + /** + * Single heartbeat to drive InMoov2 .. it can check status, healthbeat, + * and fire events to the FSM. + * Checks battery level and sends a heartbeat flash on publishHeartbeat + * and onHeartbeat at a regular interval + */ + public boolean heartbeat = true; /** * interval heath check processes in milliseconds @@ -233,7 +234,6 @@ public Plan getDefault(Plan plan, String name) { mouthControl.mouth = i01Name + ".mouth"; - ProgramABConfig chatBot = (ProgramABConfig) plan.get(getPeerName("chatBot")); Runtime runtime = Runtime.getInstance(); String[] bots = new String[] { "cn-ZH", "en-US", "fi-FI", "hi-IN", "nl-NL", "ru-RU", "de-DE", "es-ES", "fr-FR", "it-IT", "pt-PT", "tr-TR" }; @@ -276,6 +276,14 @@ public Plan getDefault(Plan plan, String name) { mouth.voice = "Mark"; mouth.speechRecognizers = new String[] { name + ".ear" }; + // == Peer - servoMixer ============================= + // setup name references to different services + ServoMixerConfig servoMixer = (ServoMixerConfig) plan.get(getPeerName("servoMixer")); + servoMixer.listeners = new ArrayList<>(); + servoMixer.listeners.add(new Listener("publishText", name + ".mouth", "onText")); + // servoMixer.listeners.add(new Listener("publishText", name + ".chatBot", + // "onText")); + // == Peer - ear ============================= // setup name references to different services WebkitSpeechRecognitionConfig ear = (WebkitSpeechRecognitionConfig) plan.get(getPeerName("ear")); @@ -285,8 +293,6 @@ public Plan getDefault(Plan plan, String name) { // remove, should only need ServiceConfig.listeners ear.textListeners = new String[] { name + ".chatBot" }; - - JMonkeyEngineConfig simulator = (JMonkeyEngineConfig) plan.get(getPeerName("simulator")); simulator.multiMapped.put(name + ".leftHand.index", new String[] { name + ".leftHand.index", name + ".leftHand.index2", name + ".leftHand.index3" });