sync local changes to get as5048 data flowing to a motor.

src/main/java/org/myrobotlab/sensor/EncoderData.java

@@ -30,6 +30,54 @@ public class EncoderData {
    */
   public double value;
 
+  public String getPin() {
+    return pin;
+  }
+
+  public void setPin(String pin) {
+    this.pin = pin;
+  }
+
+  public String getType() {
+    return type;
+  }
+
+  public void setType(String type) {
+    this.type = type;
+  }
+
+  public String getSource() {
+    return source;
+  }
+
+  public void setSource(String source) {
+    this.source = source;
+  }
+
+  public Double getAngle() {
+    return angle;
+  }
+
+  public void setAngle(Double angle) {
+    this.angle = angle;
+  }
+
+  public double getValue() {
+    return value;
+  }
+
+  public void setValue(double value) {
+    this.value = value;
+  }
+
+  public long getTimestamp() {
+    return timestamp;
+  }
+
+  public void setTimestamp(long timestamp) {
+    this.timestamp = timestamp;
+  }
+
   /**
    * time data was generated
    */

src/main/java/org/myrobotlab/sensor/EncoderListener.java

@@ -1,9 +1,31 @@
 package org.myrobotlab.sensor;
 
-import org.myrobotlab.framework.interfaces.Attachable;
+/**
+ * Any device/service that wants to handle the onEncoderData method
+ * and encoder publisher will publish the encoder data to listeners.
+ */
+public interface EncoderListener {  
+
+  public String getName();
 
-public interface EncoderListener extends Attachable {
+  public void onEncoderData(EncoderData encoderData);
+
+  default public void attachEncoderPublisher(EncoderPublisher publisher) {
+    attachEncoderPublisher(publisher.getName());
+  }
 
-  void onEncoderData(EncoderData data);
+  default public void attachEncoderPublisher(String name) {
+    send(name, "attachEncoderListener", getName());
+  }
+
+  default public void detachEncoderPublisher(EncoderPublisher publisher) {
+    detachEncoderPublisher(publisher.getName());
+  }
+
+  default public void detachEncoderPublisher(String name) {
+    send(name, "detachEncoderListener", getName());
+  }
+
+  public void send(String name, String method, Object... data);
 
 }

src/main/java/org/myrobotlab/sensor/EncoderPublisher.java

@@ -1,5 +1,49 @@
 package org.myrobotlab.sensor;
 
-public interface EncoderPublisher {
-  public EncoderData publishEncoderData(EncoderData Data);
+import org.myrobotlab.framework.interfaces.NameProvider;
+
+/**
+ * The EncoderPublisher interface is used with any device that publishes encoder data
+ * such as the AS5048A and the AMT203 encoders.  Analog pins (potentiometers) can also 
+ * emulate and publish encoder data.
+ */
+public interface EncoderPublisher extends NameProvider {
+
+  // These are all the methods that the Encoder publisher should produce.
+  public static String[] publishMethods = new String[] { "publishEncoderData" };
+
+  // Define the methods that an Encoder publisher should have for publishing data.
+  default public EncoderData publishEncoderData(EncoderData data) {
+    return data;
+  };
+
+  // helper default methods to attach and detach a listener.
+  default public void attachEncoderListener(String name) {
+    for (String publishMethod : EncoderPublisher.publishMethods) {
+      addListener(publishMethod, name);
+    }
+  }
+
+  default public void attachEncoderListener(EncoderListener listener) {
+    for (String publishMethod : EncoderPublisher.publishMethods) {
+      addListener(publishMethod, listener.getName());
+    }
+  }
+
+  // detach methods
+  default public void detachEncoderListener(String name) {
+    for (String publishMethod : EncoderPublisher.publishMethods) {
+      removeListener(publishMethod, name);
+    }
+  }
+
+  default public void detachEncoderListener(EncoderListener listener) {
+    detachEncoderListener(listener.getName());
+  }
+
+  // Add the addListener method to the interface all services implement this.
+  public void addListener(String topicMethod, String callbackName);
+
+  public void removeListener(String topicMethod, String callbackName);
+
 }

src/main/java/org/myrobotlab/sensor/TimeEncoder.java

@@ -372,4 +372,12 @@ public void stopMove() {
   public void attachEncoderController(EncoderController controller) {
     // NoOp, the TimeEncoder doesn't need a controller.
   }
+
+  @Override
+  public void updateEncoderData(EncoderData data) {
+    // NoOp, this encoder updates itself.
+    // TODO: should we publish an invoke here?
+    // invoke("publishEncoderData", data);
+
+  }
 }

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

@@ -1,6 +1,8 @@
 package org.myrobotlab.service;
 
 import org.myrobotlab.logging.LoggingFactory;
+import org.myrobotlab.sensor.EncoderData;
+import org.myrobotlab.sensor.EncoderPublisher;
 import org.myrobotlab.service.abstracts.AbstractPinEncoder;
 import org.myrobotlab.service.config.ServiceConfig;
 import org.myrobotlab.service.interfaces.EncoderControl;
@@ -23,7 +25,7 @@
  * @author kwatters
  *
  */
-public class Amt203Encoder extends AbstractPinEncoder<ServiceConfig> implements EncoderControl {
+public class Amt203Encoder extends AbstractPinEncoder<ServiceConfig> implements EncoderControl, EncoderPublisher {
 
   private static final long serialVersionUID = 1L;
 
@@ -50,4 +52,10 @@ public static void main(String[] args) throws Exception {
     log.info("Here we are..");
   }
 
+  @Override
+  public void updateEncoderData(EncoderData data) {
+    // publish the updated encoder data (this is updated from the arduino..)
+    invoke("publishEncoderData", data); 
+  }
+
 }

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

@@ -503,6 +503,20 @@ public void reattach(DeviceMapping dm) {
       if (servo.isEnabled()) {
         msg.servoAttachPin(dm.getId(), pin);
       }
+    } else if (attachable instanceof As5048AEncoder) {
+      // TODO: reattach it!
+      As5048AEncoder enc = (As5048AEncoder)attachable;
+      log.info("================ re-attaching {} {} {} ================", enc.getName(), dm.getId(), enc.getPin());
+      msg.encoderAttach(dm.getId(), 1, Integer.valueOf(enc.getPin()));
+    } else if (attachable instanceof MotorDualPwm) {
+      // TODO: reattach it!
+      MotorDualPwm motor = (MotorDualPwm)attachable;
+      int[] pins = new int[] {Integer.valueOf(motor.getLeftPwmPin()), Integer.valueOf(motor.getRightPwmPin())};
+      // TODO: what's the type?!
+      int type = 0;
+      log.info("================ re-attaching {} {} {} ================", motor.getName(), dm.getId(), pins);
+      msg.motorAttach(dm.getId(),type, pins );
+
     } else if (attachable instanceof UltrasonicSensorControl) {
       log.warn("UltrasonicSensorControl not implemented");
       // reattach logic
@@ -1685,7 +1699,6 @@ public EncoderData publishEncoderData(EncoderData data) {
   @Override
   public EncoderData publishEncoderData(Integer deviceId, Integer position) {
     // Also need to log this
-
     EncoderControl ec = (EncoderControl) getDevice(deviceId);
     String pin = null;
     Double angle = null;
@@ -1696,14 +1709,19 @@ public EncoderData publishEncoderData(Integer deviceId, Integer position) {
       // type = 1;
       pin = ((As5048AEncoder) ec).getPin();
       angle = 360.0 * position / ((As5048AEncoder) ec).resolution;
-      log.info("Angle : {}", angle);
+      // log.info("Angle : {}", angle);
     } else {
       error("unknown encoder type {}", ec.getClass().getName());
     }
-
+    // TODO: figure out how to handle analog pin data as encoder data.. 
     EncoderData data = new EncoderData(ec.getName(), pin, position, angle);
     // log.info("Publish Encoder Data Raw {}", data);
-
+    // TODO: how do i publish the data from the encoder?
+   // ec.publishEncoderData(data);
+    // This will pass the encoder data to the encoder and the encoder will publish it to listeners.
+    //log.info("Encoder data! {}", data);
+    ((EncoderControl)ec).updateEncoderData(data);
+    //     invoke("publishEncoderData", data);
     // TODO: all this code needs to move out of here!
     return data;
   }

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

@@ -1,6 +1,12 @@
 package org.myrobotlab.service;
 
+import java.util.concurrent.LinkedBlockingQueue;
+
+import org.apache.commons.math3.util.Precision;
 import org.myrobotlab.logging.LoggingFactory;
+import org.myrobotlab.sensor.EncoderData;
+import org.myrobotlab.sensor.EncoderListener;
+import org.myrobotlab.sensor.EncoderPublisher;
 import org.myrobotlab.service.abstracts.AbstractPinEncoder;
 import org.myrobotlab.service.config.ServiceConfig;
 import org.myrobotlab.service.interfaces.EncoderControl;
@@ -11,10 +17,14 @@
  * @author kwatters
  *
  */
-public class As5048AEncoder extends AbstractPinEncoder<ServiceConfig> implements EncoderControl {
+public class As5048AEncoder extends AbstractPinEncoder<ServiceConfig> implements EncoderControl, EncoderPublisher {
+
+  private static final int HISTORY_SIZE = 5;
 
   private static final long serialVersionUID = 1L;
 
+  private LinkedBlockingQueue<EncoderData> history = new LinkedBlockingQueue<EncoderData>(HISTORY_SIZE);
+
   public As5048AEncoder(String n, String id) {
     super(n, id);
     // 14 bit encoder is 2^16 steps of resolution
@@ -41,5 +51,30 @@ public static void main(String[] args) throws Exception {
     encoder.setZeroPoint();
     log.info("Here we are..");
   }
+
+  public void updateEncoderData(EncoderData data) {
+    // publish the updated encoder data (this is updated from the arduino..)
+    // log.info("Encoder data: {}", data);
+    // pop the first value
+    if (history.remainingCapacity() == 0) {
+      history.poll();
+    }
+    history.offer(data);
+    // This is computing a moving average for the encoder value to smooth it out a bit.
+    double avgVal = history.stream().mapToDouble(EncoderData::getValue).average().orElse(0.0);
+    double avgAngle = Precision.round(history.stream().mapToDouble(EncoderData::getAngle).average().orElse(0.0), 1);    
+
+   // Precision.round(avgAngle, 2);
+   // double avgVal = (previousData.value + data.value)/2;
+   // double avgAngle = (previousData.angle + data.angle)/2;
+    // 0.1 degree resolution... truncate and filter the value for stability..
+    EncoderData filteredData = new EncoderData( data.source, data.pin, avgVal, avgAngle);
+   //  log.info("Original Angle: {}  Filtered Angle: {}", data.angle, filteredData.angle);
+
+    // previousData = data;
+    //invoke("publishEncoderData", data); 
+
+    invoke("publishEncoderData", filteredData); 
+  }
 
 }