diff --git a/src/main/java/org/myrobotlab/sensor/EncoderData.java b/src/main/java/org/myrobotlab/sensor/EncoderData.java index a016c9f2a5..c45ec857b4 100644 --- a/src/main/java/org/myrobotlab/sensor/EncoderData.java +++ b/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 */ diff --git a/src/main/java/org/myrobotlab/sensor/EncoderListener.java b/src/main/java/org/myrobotlab/sensor/EncoderListener.java index 33bc18558a..90e8b3abc4 100644 --- a/src/main/java/org/myrobotlab/sensor/EncoderListener.java +++ b/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); } diff --git a/src/main/java/org/myrobotlab/sensor/EncoderPublisher.java b/src/main/java/org/myrobotlab/sensor/EncoderPublisher.java index 59b029b165..e91fb564ca 100644 --- a/src/main/java/org/myrobotlab/sensor/EncoderPublisher.java +++ b/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); + } diff --git a/src/main/java/org/myrobotlab/sensor/TimeEncoder.java b/src/main/java/org/myrobotlab/sensor/TimeEncoder.java index 43233f46d2..fb006cf4c9 100644 --- a/src/main/java/org/myrobotlab/sensor/TimeEncoder.java +++ b/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); + + } } \ No newline at end of file diff --git a/src/main/java/org/myrobotlab/service/Amt203Encoder.java b/src/main/java/org/myrobotlab/service/Amt203Encoder.java index 870f01e0a7..8549ef9051 100755 --- a/src/main/java/org/myrobotlab/service/Amt203Encoder.java +++ b/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 implements EncoderControl { +public class Amt203Encoder extends AbstractPinEncoder 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); + } + } diff --git a/src/main/java/org/myrobotlab/service/Arduino.java b/src/main/java/org/myrobotlab/service/Arduino.java index 8385273fe6..796ceb1ccc 100644 --- a/src/main/java/org/myrobotlab/service/Arduino.java +++ b/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; } diff --git a/src/main/java/org/myrobotlab/service/As5048AEncoder.java b/src/main/java/org/myrobotlab/service/As5048AEncoder.java index 1795c0bbc1..b39512133f 100755 --- a/src/main/java/org/myrobotlab/service/As5048AEncoder.java +++ b/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 implements EncoderControl { +public class As5048AEncoder extends AbstractPinEncoder implements EncoderControl, EncoderPublisher { + + private static final int HISTORY_SIZE = 5; private static final long serialVersionUID = 1L; + private LinkedBlockingQueue history = new LinkedBlockingQueue(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); + } } diff --git a/src/main/java/org/myrobotlab/service/DiyServo2.java b/src/main/java/org/myrobotlab/service/DiyServo2.java new file mode 100755 index 0000000000..c6d22ab5a2 --- /dev/null +++ b/src/main/java/org/myrobotlab/service/DiyServo2.java @@ -0,0 +1,638 @@ +/** + * + * This file is part of MyRobotLab (http://myrobotlab.org). + * + * MyRobotLab is free software: you can redistribute it and/or modify + * it under the terms of the Apache License 2.0 as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version (subject to the "Classpath" exception + * as provided in the LICENSE.txt file that accompanied this code). + * + * MyRobotLab is distributed in the hope that it will be useful or fun, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * Apache License 2.0 for more details. + * + * All libraries in thirdParty bundle are subject to their own license + * requirements - please refer to http://myrobotlab.org/libraries for + * details. + * + * Enjoy ! + * + * */ + +package org.myrobotlab.service; + +import org.myrobotlab.framework.Service; +import org.myrobotlab.logging.LoggingFactory; +import org.myrobotlab.math.interfaces.Mapper; +import org.myrobotlab.sensor.EncoderData; +import org.myrobotlab.sensor.EncoderListener; +import org.myrobotlab.service.config.DiyServo2Config; +import org.myrobotlab.service.data.ServoMove; +import org.myrobotlab.service.data.ServoSpeed; +import org.myrobotlab.service.interfaces.EncoderControl; +import org.myrobotlab.service.interfaces.MotorControl; +import org.myrobotlab.service.interfaces.ServoControl; +import org.myrobotlab.service.interfaces.ServoControlPublisher; +import org.myrobotlab.service.interfaces.ServoController; +import org.myrobotlab.service.interfaces.ServoEvent; +import org.myrobotlab.service.interfaces.ServoStatusPublisher; + +/** + * Simple(ish) DiyServo2. + * This service requires an Encoder and a MotorControl. It uses PID control to + * control the position of an actuator measured by the encoder with the motor. + * The standard PID params are supported Kp,Ki,Kd. + * This implements servo control. + * + * Data is published from the encoder to this service, that updates the input to the Pid control. + * The Pid output is computed by default at 20Hz and is controlled by the sampleTime parameter. + * The output of the pid control is then written to the motor control + */ + +public class DiyServo2 extends Service implements EncoderListener, ServoControl, ServoControlPublisher, ServoStatusPublisher { + + private static final long serialVersionUID = 1L; + private volatile boolean enabled = true; + private MotorControl motorControl; + private Double currentAngle; + public Pid pid; + + // TODO: use the motor name. + public String pidKey = "diy2"; + + private double kp = 0.05; + private double ki = 0.001; // 0.020; + private double kd = 0.001; // 0.020; + public double setpoint = 90.0; // Intial + // samples per second. + public int sampleTime = 20; + static final public int MODE_AUTOMATIC = 1; + + transient MotorUpdater motorUpdater; + EncoderControl encoder; + private Double rest = 90.0; + private long lastActivityTimeMS; + + public DiyServo2(String reservedKey, String inId) { + super(reservedKey, inId); + } + + @Override + synchronized public void startService() { + super.startService(); + pidKey = getFullName(); + // TODO:figure out what we should do here? perhaps nothing? + initPid(); + } + + void initPid() { + // are the peers going to be created? + pid = (Pid)Runtime.start("pid", "Pid"); + //pid = (Pid) createPeer("pid"); + pidKey = this.getName(); + pid.setPid(pidKey, kp, ki, kd); // Create a PID with the name of this + // service instance + pid.setMode(pidKey, MODE_AUTOMATIC); // Initial mode is manual + pid.setOutputRange(pidKey, -1.0, 1.0); // Set the Output range to match the Motor input + pid.setSampleTime(pidKey, sampleTime); // Sets the sample time + pid.setSetpoint(pidKey, setpoint); + pid.startService(); + } + + @Override + public void onEncoderData(EncoderData data) { + // System.err.println("DIY Servo Encoder Data: " + data); + this.currentAngle = data.angle; + } + + public void attachEncoderControl(EncoderControl enc) { + encoder = enc; + // Tell the encoder to publish encoder data to this service + encoder.attachEncoderListener(this); + } + + private void attachMotorControl(MotorControl mot) { + // use the motor name as the pid key + this.motorControl = mot; + + // this.pidKey = mot.getName(); + if (motorUpdater == null) { + log.info("Starting MotorUpdater"); + motorUpdater = new MotorUpdater(getName()); + motorUpdater.start(); + log.info("MotorUpdater started"); + } + + } + + public Double moveTo(Double angle) { + log.info("Servo Move to {}", angle); + // This updates the setpoint of the pid control. + this.setpoint = angle; + pid.setSetpoint(pidKey, angle); + lastActivityTimeMS = System.currentTimeMillis(); + // Why does this return a boolean? + + // invoke("publishMoveTo", this); + // invoke("publishServoEvent", angle.doubleValue()); + return angle; + } + + /** + * MotorUpdater The control loop to update the MotorControl with new values + * based on the PID calculations + * + */ + public class MotorUpdater extends Thread { + + double lastOutput = 0.0; + // degree threshold for saying that we've arrived. + double threshold = 0.25; + // goal is to not use this + + public MotorUpdater(String name) { + super(String.format("%s.motorUpdater", name)); + } + + @Override + public void run() { + log.info("Motor updater started"); + try { + while (true) { + if (isRunning()) { + // log.info("Updating control loop"); + // Calculate the new value for the motor + if (pid.data.containsKey(pidKey) && currentAngle != null && pidKey != null) { + // Update the pid input value. + // pass the current angle from the encoder to the pid controller + Double output = pid.compute(pidKey, currentAngle); + if (output == null) { + continue; + } + double delta = Math.abs(currentAngle - setpoint); + if (delta < threshold ) { + log.info("Arrived!"); + motorControl.move(0); + // TODO: some debouncing logic here. + // TODO: publish the servo events for started/stopped here. + } else if (output != lastOutput) { + log.info("move motor : Power: {} Target: {} Current: {} Delta: {}", output, setpoint, currentAngle, delta); + motorControl.move(output); + lastOutput = output; + } else { + log.info("delta {} threshold {} current {} setpoint {} output {} lastOutput {}", delta, threshold, currentAngle, setpoint, output, lastOutput); + } + } + // TODO: how long do we need to sleep + // TODO: maybe a more accurate loop timer? + // This is a samples per second.. we need to only wait for the remainaing amount of time in the period. + Thread.sleep(1000 / sampleTime); + } else { + log.info("Not running?!"); + } + + } + } catch (Exception e) { + if (e instanceof InterruptedException) { + motorControl.stop(); + } else { + log.error("motor updater threw", e); + } + } + } + + private boolean isRunning() { + // if we are enabled, have a motor connected and have received encoder data. + return enabled && motorControl != null && currentAngle != null; + } + } + + @Override + public void disable() { + // TODO: what do do here? + // motorControl.disable(); + motorControl.stop(); + enabled = false; + // TODO: broadcast enabled/disabled messages? + } + + @Override + public void enable() { + // TODO: what do to here? + // motorControl.enable(); + enabled = true; + } + + @Override + public double getRest() { + // Ok.. not a bad idea.. let's have a rest position for the servo.. default to 90 deg? or something? + return rest; + } + + @Override + public boolean isAutoDisable() { + // TODO: impl this.. safety is good. + return false; + } + + @Override + public void attach(ServoController listener) { + // TODO: remove from ServoControl interface... NoOp here. + // NoOp : no servo controllers here.. + log.warn("Diy Servo doesn't use a controller.. no implemented."); + } + + @Override + public void detach(ServoController listener) { + // TODO maybe remove from interface? this service doesn't give a crapola about servo controllers. + log.warn("Diy Servo doesn't use a controller.. no implemented."); + } + + @Override + public String getController() { + // TODO remove from interface?. we have no controller. + log.warn("Diy Servo doesn't use a controller.. no implemented."); + return null; + } + + @Override + public EncoderControl getEncoder() { + // TODO: we just subscribe to the encoder.. we don't have/need a handle to it! + // why are we expected to return it.. remove from interface. + return encoder; + } + + @Override + public long getLastActivityTime() { + return lastActivityTimeMS; + } + + @Override + public Mapper getMapper() { + // TODO - we have no mapper... + return null; + } + + @Override + public double getMax() { + // TODO: should implement.. safety limits are important. + // This might be useful to know what the max/min value that this encoder can get to.. but for us.. it's 360 degrees.. and can rotate as much as we like. + return 360; + } + + @Override + public double getMin() { + // TODO safety limits are good.. + return 0; + } + + @Override + public String getPin() { + log.warn("DiyServo doesn't have pins. No implemented."); + // TODO: This doesn't mean anything here. + // maybe this is the pin from the encoder? but why.. + return null; + } + + @Override + public double getCurrentInputPos() { + // TODO: return currentAngle? we have no mapper. + return currentAngle; + } + + @Override + public double getCurrentOutputPos() { + // TODO: this interface has way too much stuff in it... + // return the last known encoder angle + return currentAngle; + } + + @Override + public Double getSpeed() { + // TODO: implement speed control + return null; + } + + @Override + public double getTargetOutput() { + // the setPoint for the pid control is the target output. + // we have no mapper.. + return setpoint; + } + + @Override + public double getTargetPos() { + // This is the setPoint for the pid control.. the target position. + return setpoint; + } + + @Override + public boolean isBlocking() { + // TODO What does this mean? should we remove this from the interface? + return false; + } + + @Override + public boolean isEnabled() { + return enabled; + } + + @Override + public boolean isInverted() { + // TODO not sure what this means for a diyservo. + return false; + } + + @Override + public boolean isMoving() { + // TODO we should trigger this off the power output being sent to the motor. + return false; + } + + @Override + public void map(double minX, double maxX, double minY, double maxY) { + // TODO No mapper in the diyservo (yet.) + } + + @Override + public Double moveToBlocking(Double pos) { + return moveToBlocking(pos, null); + } + + @Override + public Double moveToBlocking(Double pos, Long timeoutMs) { + // TODO : implement a timed out blocking move. + this.moveTo(pos); + //TODO: block until we get there! + return null; + } + + @Override + public void rest() { + // ok. move to base class. + moveTo(rest); + } + + @Override + public void setMapper(Mapper m) { + // TODO Do we actually need a mapper? + } + + @Override + public void setMinMax(double minXY, double maxXY) { + // TODO: implement this.. for safty limits.. dont support a move call outside the specified range. + // we don't even have a mapper.. we dont' need one..we might want a mapper that gives us a phase shift. + // but reality is. that should be handled by the encoder. + } + + @Override + public void setMinMaxOutput(double minY, double maxY) { + // TODO: implement this.. for safty limits.. dont support a move call outside the specified range. + } + + @Override + public void setPin(Integer pin) { + // TODO: There are no pins! we have no pins! + log.warn("setPin not implemented in DiyServo."); + } + + @Override + public void setPin(String pin) { + // TODO: remove from interface? We don't have any pins. + log.warn("setPin not implemented in DiyServo."); + } + + @Override + public void setPosition(double pos) { + // TODO: maybe deprecate and remove from interface ? + // This method had a strange functionality of setting a position + // even though the servo wasn't attached / enabled? + moveTo(pos); + } + + @Override + public void setRest(double rest) { + // TODO move to base class + this.rest = rest; + } + + @Override + public void setSpeed(Double degreesPerSecond) { + // TODO: velocity control. + } + + @Override + public void stop() { + // Stop the motor... anything else? + motorControl.move(0.0); + } + + @Override + public void sync(ServoControl sc) { + // TODO Impl me. + } + + @Override + public void unsync(ServoControl sc) { + // TODO Impl me + } + + @Override + public void waitTargetPos() { + // TODO Auto-generated method stub + // really? ok. + // here we should wait until we have "arrived" ... + + } + + @Override + public void writeMicroseconds(int uS) { + // NoOp here... should be removed from ServoControl interface.. this is specific to a pwm controlled servo + log.warn("Write Microseconds not implemented for DiyServo."); + } + + + + @Override + public void fullSpeed() { + // TODO: add a velocity control. + // TODO: deprecated, remove from interface? + } + + @Override + public ServoControl publishMoveTo(ServoControl sc) { + return sc; + } + + @Override + public ServoControl publishServoStop(ServoControl sc) { + return sc; + } + + @Override + public Double moveToBlocking(Integer newPos) { + // TODO Auto-generated method stub + return null; + } + + @Override + public Double moveToBlocking(Integer newPos, Long timeoutMs) { + // TODO Auto-generated method stub + return null; + } + + @Override + public ServoEvent publishServoStarted(String name, Double position) { + // TODO Auto-generated method stub + return null; + } + + @Override + public ServoEvent publishServoStopped(String name, Double position) { + // TODO Auto-generated method stub + return null; + } + + @Override + public ServoMove publishServoMoveTo(ServoMove pos) { + // TODO Auto-generated method stub + return null; + } + + @Override + public String publishServoEnable(String name) { + // TODO Auto-generated method stub + return null; + } + + @Override + public void attachServoControlListener(String name) { + // TODO Auto-generated method stub + + } + + @Override + public void setAutoDisable(boolean autoDisable) { + // TODO Auto-generated method stub + + } + + @Override + public void setInverted(boolean invert) { + // TODO Auto-generated method stub + + } + + @Override + public void setSpeed(Integer degreesPerSecond) { + // TODO Auto-generated method stub + + } + + @Override + public void sync(String name) { + // TODO Auto-generated method stub + + } + + @Override + public void unsync(String name) { + // TODO Auto-generated method stub + + } + + @Override + public void attachServoController(String sc) { + // TODO Auto-generated method stub + + } + + @Override + public void setMaxSpeed() { + // TODO Auto-generated method stub + + } + + @Override + public Double moveTo(Integer newPos) { + // TODO Auto-generated method stub + return moveTo(Double.valueOf(newPos)); + } + + @Override + public ServoSpeed publishServoSetSpeed(ServoControl sc) { + // TODO Auto-generated method stub + return null; + } + + @Override + public String publishServoEnable(ServoControl sc) { + // TODO Auto-generated method stub + return null; + } + + @Override + public String publishServoDisable(ServoControl sc) { + // TODO Auto-generated method stub + return null; + } + + + public static void main(String[] args) throws Exception { + + LoggingFactory.init("info"); + + Runtime.start("python", "Python"); + WebGui webgui = (WebGui)Runtime.start("webgui", "WebGui"); + + // Compose the components of the diy servo and attach them. + // Make one.. and stuff. + // setup the encoder. + Arduino ard = (Arduino)Runtime.start("ard", "Arduino"); + ard.connect("COM3"); + // ard.setDebug(true); + As5048AEncoder encoder = (As5048AEncoder) Runtime.start("encoder", "As5048AEncoder"); + encoder.setPin(10); + ard.attach(encoder); + // setup the motor. + // encoder.ttach + MotorDualPwm mot = (MotorDualPwm) Runtime.start("diyServo.motor", "MotorDualPwm"); + mot.setPwmPins(6, 7); + ard.attach(mot); + // TODO: attach both to the diyservo and set the pin. + + + + if (true) { + // What else do we need? + DiyServo2 diy = (DiyServo2)Runtime.start("diy", "DiyServo2"); + // attach the encoder and motor to the diy servo. + diy.attachEncoderControl(encoder); + diy.attachMotorControl(mot); + + // Now we can move it?? + + + + // attach the encoder and motor + // diy.attachEncoderControl(encoder); + // diy.attachMotorControl(mot); + // Tell the servo to move somewhere. + + // diy.moveTo(75.0); + //Thread.sleep(2000); + // diy.disable(); + // Thread.sleep(1000); + // diy.enable(); + diy.moveTo(250.0); + } + System.out.println("Press the any key"); + System.in.read(); + + } + + + +} \ No newline at end of file diff --git a/src/main/java/org/myrobotlab/service/Motor.java b/src/main/java/org/myrobotlab/service/Motor.java index 7ee06fe258..97257e133b 100644 --- a/src/main/java/org/myrobotlab/service/Motor.java +++ b/src/main/java/org/myrobotlab/service/Motor.java @@ -4,7 +4,6 @@ import org.myrobotlab.logging.LoggingFactory; import org.myrobotlab.service.abstracts.AbstractMotor; import org.myrobotlab.service.config.MotorConfig; -import org.myrobotlab.service.config.ServiceConfig; /** * A general motor implementation with a "simple H-bridge" where one control @@ -99,18 +98,18 @@ public static void main(String[] args) { try { - Runtime.start("gui", "SwingGui"); + Runtime.start("python", "Python"); Runtime.start("webgui", "WebGui"); - Runtime.start("motor", "Motor"); + Runtime.start("m1", "MotorDualPwm"); Runtime.start("arduino", "Arduino"); - boolean done = true; + boolean done = false; if (done) { return; } // FIXME - all testing or replacing of main code should be new JUnit // tests - with virtual arduino !!!) - String port = "COM15"; + String port = "COM3"; // Arduino arduino = (Arduino) Runtime.start("arduino", "Arduino"); // Runtime.createAndStart("gui", "SwingGui"); @@ -137,19 +136,27 @@ public static void main(String[] args) { Arduino arduino = (Arduino) Runtime.start("arduino", "Arduino"); arduino.connect(port); - arduino.pinMode(6, Arduino.OUTPUT); - arduino.pinMode(7, Arduino.OUTPUT); + + + - arduino.digitalWrite(7, 1); + // arduino.pinMode(6, Arduino.OUTPUT); + // arduino.pinMode(7, Arduino.OUTPUT); +// +// arduino.digitalWrite(7, 1); // arduino.digitalWrite(6, 1); - arduino.analogWrite(6, 255); - arduino.analogWrite(6, 200); - arduino.analogWrite(6, 100); - arduino.analogWrite(6, 0); + // arduino.analogWrite(6, 255); + // arduino.analogWrite(6, 200); + // arduino.analogWrite(6, 100); + // arduino.analogWrite(6, 0); - Motor m1 = (Motor) Runtime.start("m1", "Motor"); + MotorDualPwm m1 = (MotorDualPwm) Runtime.start("m1", "Motor"); + m1.setPwmPins(6, 7); + + + /* * m1.setType2Pwm(leftPwm, rightPwm); m1.setTypeStepper(); * m1.setTypePulseStep(pwmPin, dirPin); @@ -160,14 +167,30 @@ public static void main(String[] args) { // m1.attach(arduino, Motor.TYPE_SIMPLE, pwmPin, dirPin); m1.attachMotorController(arduino); - m1.move(1.0); - m1.move(-1.0); - - arduino.enableBoardInfo(true); - arduino.enableBoardInfo(false); - m1.stop(); - m1.move(0.5); - m1.stop(); + As5048AEncoder encoder = (As5048AEncoder) Runtime.start("encoder", "As5048AEncoder"); + encoder.setPin(10); + arduino.attachEncoderControl(encoder); + + // attach the motor as an encoder listener? + encoder.attachEncoderListener(m1); +// for (int i = 0; i < 1; i++) { +// m1.move(1.0); +// Thread.sleep(1000); +// m1.move(-1.0); +// Thread.sleep(1000); +// m1.move(1.0); +// Thread.sleep(1000); +// m1.move(-1.0); +// Thread.sleep(1000); +// m1.stop(); +// +// Thread.sleep(1000); +// } + //# arduino.enableBoardInfo(true); + //# arduino.enableBoardInfo(false); + // m1.stop(); + // m1.move(0.5); + // m1.stop(); // Runtime.start("webgui", "WebGui"); diff --git a/src/main/java/org/myrobotlab/service/MotorDualPwm.java b/src/main/java/org/myrobotlab/service/MotorDualPwm.java index 6060be0fb9..56d7429c50 100644 --- a/src/main/java/org/myrobotlab/service/MotorDualPwm.java +++ b/src/main/java/org/myrobotlab/service/MotorDualPwm.java @@ -3,7 +3,6 @@ import java.util.Arrays; import java.util.List; -import org.myrobotlab.framework.Platform; import org.myrobotlab.logging.Level; import org.myrobotlab.logging.LoggingFactory; import org.myrobotlab.service.abstracts.AbstractMotor; diff --git a/src/main/java/org/myrobotlab/service/abstracts/AbstractMotor.java b/src/main/java/org/myrobotlab/service/abstracts/AbstractMotor.java index 6916db6269..f5ae969a6a 100644 --- a/src/main/java/org/myrobotlab/service/abstracts/AbstractMotor.java +++ b/src/main/java/org/myrobotlab/service/abstracts/AbstractMotor.java @@ -290,6 +290,25 @@ public double getTargetPos() { @Override public void onEncoderData(EncoderData data) { // TODO Auto-generated method stub + log.info("Encoder Data (to motor): {}", data); + // TODO: this should probably not be here.. but rather in a DiyServo service instead. + if (false) { + double target = 180.0; + double delta = data.angle - target; + + if (Math.abs(delta) > 0.5) { + // move the motor a bit. + // TODO: this hsould be controlled by a PID algorithm. + if (delta > 0) { + this.move(0.5); + } else { + this.move(0.5); + } + + } else { + this.move(0); + } + } } diff --git a/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java b/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java index 6aa5720aeb..4c5c1fe5be 100644 --- a/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java +++ b/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java @@ -100,6 +100,7 @@ public Boolean isEnabled() { @Override public EncoderData publishEncoderData(EncoderData data) { + // log.info("Abstract pin encoder publish data {}", data); return data; } diff --git a/src/main/java/org/myrobotlab/service/config/DiyServo2Config.java b/src/main/java/org/myrobotlab/service/config/DiyServo2Config.java new file mode 100755 index 0000000000..15def3a605 --- /dev/null +++ b/src/main/java/org/myrobotlab/service/config/DiyServo2Config.java @@ -0,0 +1,37 @@ +package org.myrobotlab.service.config; + +import org.myrobotlab.framework.Plan; +import org.myrobotlab.service.Pid.PidData; + +public class DiyServo2Config extends ServiceConfig { + // TODO: add config here + @Override + public Plan getDefault(Plan plan, String name) { + super.getDefault(plan, name); + + // Ok.. what do we need to add here for the DIYServo? + // we need an encoder... + // we need a motor + // we need a Pid + addDefaultPeerConfig(plan, name, "motor", "MotorDualPwm"); + addDefaultPeerConfig(plan, name, "encoder", "As5048AEncoder"); + addDefaultPeerConfig(plan, name, "pid", "Pid"); + + + MotorDualPwmConfig motor = (MotorDualPwmConfig) plan.get(getPeerName("motor")); + // TODO: controller?! + // motor.controller + motor.leftPwmPin = "6"; + motor.rightPwmPin = "7"; + + // TODO: how do we handle the controller for the encoder? (could be different than the motor) + As5048AEncoderConfig encoder = (As5048AEncoderConfig) plan.get(getPeerName("encoder")); + + PidConfig pid = (PidConfig) plan.get(getPeerName("pid")); + pid.data.put(name, new PidData()); + + // default settings? + return plan; + } +} + diff --git a/src/main/java/org/myrobotlab/service/interfaces/EncoderControl.java b/src/main/java/org/myrobotlab/service/interfaces/EncoderControl.java index 895fc7f7f3..5e6192f581 100755 --- a/src/main/java/org/myrobotlab/service/interfaces/EncoderControl.java +++ b/src/main/java/org/myrobotlab/service/interfaces/EncoderControl.java @@ -2,8 +2,9 @@ import org.myrobotlab.framework.interfaces.Attachable; import org.myrobotlab.sensor.EncoderData; +import org.myrobotlab.sensor.EncoderPublisher; -public interface EncoderControl extends Attachable { +public interface EncoderControl extends EncoderPublisher, Attachable { /** * stop the stream of encoder data @@ -41,4 +42,10 @@ public interface EncoderControl extends Attachable { */ public Double getPos(); -} + /** + * encoder controls can update their copy of the encoder data and then publish it to listeners. + * @param data + */ + public void updateEncoderData(EncoderData data); + +} \ No newline at end of file diff --git a/src/main/java/org/myrobotlab/service/meta/DiyServo2Meta.java b/src/main/java/org/myrobotlab/service/meta/DiyServo2Meta.java new file mode 100755 index 0000000000..51ed2c8d52 --- /dev/null +++ b/src/main/java/org/myrobotlab/service/meta/DiyServo2Meta.java @@ -0,0 +1,27 @@ +package org.myrobotlab.service.meta; + +import org.myrobotlab.framework.Platform; +import org.myrobotlab.logging.LoggerFactory; +import org.myrobotlab.service.meta.abstracts.MetaData; +import org.slf4j.Logger; + +public class DiyServo2Meta extends MetaData { + private static final long serialVersionUID = 1L; + public final static Logger log = LoggerFactory.getLogger(DiyServo2Meta.class); + + /** + * This class is contains all the meta data details of a service. It's peers, + * dependencies, and all other meta data related to the service. + * + */ + public DiyServo2Meta(String name) { + Platform platform = Platform.getLocalInstance(); + addDescription("Controls a motor so that it can be used as a Servo with an encoder feedback"); + addCategory("control", "servo"); + // TODO: add peers here? + //addPeer("motor", "MotorDualPwm", "MotorControl service"); + //addPeer("pid", "Pid", "PID service"); + + } + +} diff --git a/src/main/resources/resource/WebGui/app/service/js/Amt203EncoderGui.js b/src/main/resources/resource/WebGui/app/service/js/Amt203EncoderGui.js new file mode 100755 index 0000000000..0339e3b824 --- /dev/null +++ b/src/main/resources/resource/WebGui/app/service/js/Amt203EncoderGui.js @@ -0,0 +1,39 @@ +angular.module('mrlapp.service.Amt203EncoderGui', []).controller('Amt203EncoderGuiCtrl', ['$scope', 'mrl', function($scope, mrl) { + console.info('Amt203EncoderGuiCtrl') + var _self = this + var msg = this.msg + + $scope.controllers = [] + + // published EncoderData + $scope.data = null + + // GOOD TEMPLATE TO FOLLOW + this.updateState = function(service) { + $scope.service = service + } + + this.onMsg = function(inMsg) { + var data = inMsg.data[0] + switch (inMsg.method) { + case 'onState': + _self.updateState(data) + $scope.$apply() + break + case 'onEncoderData': + $scope.data = data + $scope.$apply() + break + case 'onStatus': + console.info("On status") + default: + console.info("ERROR - unhandled method " + $scope.name + " Method " + inMsg.method) + break + } + + }; + + msg.subscribe('publishEncoderData') + msg.subscribe(this) +} +]) diff --git a/src/main/resources/resource/WebGui/app/service/js/As5048AEncoderGui.js b/src/main/resources/resource/WebGui/app/service/js/As5048AEncoderGui.js new file mode 100755 index 0000000000..d0f11e12cc --- /dev/null +++ b/src/main/resources/resource/WebGui/app/service/js/As5048AEncoderGui.js @@ -0,0 +1,39 @@ +angular.module('mrlapp.service.As5048AEncoderGui', []).controller('As5048AEncoderGuiCtrl', ['$scope', 'mrl', function($scope, mrl) { + console.info('As5048AEncoderGuiCtrl') + var _self = this + var msg = this.msg + + $scope.controllers = [] + + // published EncoderData + $scope.data = null + + // GOOD TEMPLATE TO FOLLOW + this.updateState = function(service) { + $scope.service = service + } + + this.onMsg = function(inMsg) { + var data = inMsg.data[0] + switch (inMsg.method) { + case 'onState': + _self.updateState(data) + $scope.$apply() + break + case 'onEncoderData': + $scope.data = data + $scope.$apply() + break + case 'onStatus': + console.info("On status") + default: + console.info("ERROR - unhandled method " + $scope.name + " Method " + inMsg.method) + break + } + + }; + + msg.subscribe('publishEncoderData') + msg.subscribe(this) +} +]) diff --git a/src/main/resources/resource/WebGui/app/service/js/DiyServo2Gui.js b/src/main/resources/resource/WebGui/app/service/js/DiyServo2Gui.js new file mode 100755 index 0000000000..87dbdfde5a --- /dev/null +++ b/src/main/resources/resource/WebGui/app/service/js/DiyServo2Gui.js @@ -0,0 +1,42 @@ +angular.module('mrlapp.service.DiyServo2Gui', []).controller('DiyServo2GuiCtrl', ['$scope', 'mrl', function($scope, mrl) { + console.info('DiyServo2GuiCtrl') + var _self = this + var msg = this.msg + + // GOOD TEMPLATE TO FOLLOW + this.updateState = function(service) { + $scope.service = service + } + + $scope.moveTo = function(pos) { + msg.send('moveTo', pos) + } + + this.onMsg = function(inMsg) { + var data = inMsg.data[0] + switch (inMsg.method) { + case 'onState': + _self.updateState(data) + $scope.$apply() + break + // TODO: figure out which callbacks we will subscribe to here. + case 'onServoEvent': + $scope.data = data + $scope.$apply() + break + case 'onServoData': + $scope.data = data + $scope.$apply() + break + case 'onStatus': + console.info("On status") + default: + console.info("ERROR - unhandled method " + $scope.name + " Method " + inMsg.method) + break + } + + }; + + msg.subscribe('publishServoEvent') + msg.subscribe(this)} +]) diff --git a/src/main/resources/resource/WebGui/app/service/js/MotorDualPwmGui.js b/src/main/resources/resource/WebGui/app/service/js/MotorDualPwmGui.js index c9c86d4041..5a0d02719f 100644 --- a/src/main/resources/resource/WebGui/app/service/js/MotorDualPwmGui.js +++ b/src/main/resources/resource/WebGui/app/service/js/MotorDualPwmGui.js @@ -85,7 +85,7 @@ angular.module('mrlapp.service.MotorDualPwmGui', []).controller('MotorDualPwmGui $scope.update = function() { console.info('update') - msg.send('map', $scope.service.config.mapper.minIn, $scope.service.config.mapper.minIn, $scope.service.config.mapper.minOut, $scope.service.config.mapper.maxOut) + msg.send('map', $scope.service.config.mapper.minIn, $scope.service.config.mapper.maxIn, $scope.service.config.mapper.minOut, $scope.service.config.mapper.maxOut) } $scope.setController = function(c) { @@ -110,7 +110,7 @@ angular.module('mrlapp.service.MotorDualPwmGui', []).controller('MotorDualPwmGui $scope.setSpeed = function() { - msg.send('setSpeed', $scope.requestedPower) + msg.send('move', $scope.requestedPower) } diff --git a/src/main/resources/resource/WebGui/app/service/views/Amt203EncoderGui.html b/src/main/resources/resource/WebGui/app/service/views/Amt203EncoderGui.html new file mode 100755 index 0000000000..a3bda34fa5 --- /dev/null +++ b/src/main/resources/resource/WebGui/app/service/views/Amt203EncoderGui.html @@ -0,0 +1,18 @@ +
+
 
+
+ AS5048A Encoder + + + + + + + + + + +
Source{{ data.source }}
Angle{{ data.angle.toFixed(3) }}
Raw Value{{ data.value }}
+
+
+ diff --git a/src/main/resources/resource/WebGui/app/service/views/As5048AEncoderGui.html b/src/main/resources/resource/WebGui/app/service/views/As5048AEncoderGui.html new file mode 100755 index 0000000000..a3bda34fa5 --- /dev/null +++ b/src/main/resources/resource/WebGui/app/service/views/As5048AEncoderGui.html @@ -0,0 +1,18 @@ +
+
 
+
+ AS5048A Encoder + + + + + + + + + + +
Source{{ data.source }}
Angle{{ data.angle.toFixed(3) }}
Raw Value{{ data.value }}
+
+
+ diff --git a/src/main/resources/resource/WebGui/app/service/views/DiyServo2Gui.html b/src/main/resources/resource/WebGui/app/service/views/DiyServo2Gui.html new file mode 100755 index 0000000000..4461bfaa70 --- /dev/null +++ b/src/main/resources/resource/WebGui/app/service/views/DiyServo2Gui.html @@ -0,0 +1,11 @@ + +
+ Diy Servo 2 ! {{ serivce.targetPos }} + Diy Servo a..! {{ targetPos }} + Data {{ data }} + +
+ +