Merge branch 'develop' of github.com-myrobotlab:MyRobotLab/myrobotlab…

… into inmoov-and-statemachine-1

src/main/java/VectorAngleCalculator.java

@@ -0,0 +1,45 @@
+import java.util.Vector;
+
+public class VectorAngleCalculator {
+    public static double calculateAngle(Vector<Double> vector1, Vector<Double> 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<Double> vector1 = new Vector<>();
+        vector1.add(1.0);
+        vector1.add(0.0);
+
+        Vector<Double> vector2 = new Vector<>();
+        vector2.add(1.0);
+        vector2.add(1.0);
+
+        double angleDegrees = calculateAngle(vector1, vector2);
+        System.out.println("Angle between vectors: " + angleDegrees + " degrees");
+    }
+}

src/main/java/org/myrobotlab/service/InMoov2.java

@@ -109,6 +109,7 @@ public static boolean loadFile(String file) {
   protected transient ProgramAB chatBot;
 
   protected List<String> 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<String, Double> 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);
   }

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,20 +46,21 @@ 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.
    * green == good
    * 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" });