From 98145d380e743dad4a767289bf154f1adbae72aa Mon Sep 17 00:00:00 2001 From: kwatters Date: Sat, 13 Mar 2021 16:55:08 -0500 Subject: [PATCH 01/10] minor updates to get the data flowing from the encoder controller through the encoder control to the encoder listeners. --- .../java/org/myrobotlab/service/Arduino.java | 14 ++++++------- .../service/abstracts/AbstractPinEncoder.java | 20 ++++++++----------- 2 files changed, 15 insertions(+), 19 deletions(-) diff --git a/src/main/java/org/myrobotlab/service/Arduino.java b/src/main/java/org/myrobotlab/service/Arduino.java index 36b921f7dd..72f517d0e3 100644 --- a/src/main/java/org/myrobotlab/service/Arduino.java +++ b/src/main/java/org/myrobotlab/service/Arduino.java @@ -349,6 +349,7 @@ public void attach(EncoderControl encoder) throws Exception { // send the attach method with our device id. msg.encoderAttach(m.getId(), type, address); + addListener("publishEncoderData", encoder.getName()); encoder.attach(this); } @@ -1602,27 +1603,26 @@ public EncoderData publishEncoderData(EncoderData data) { // callback for generated method from arduinoMsg.schema public EncoderData publishEncoderData(Integer deviceId, Integer position) { - // Also need to log this - + // This is the raw data coming back from the arduino EncoderControl ec = (EncoderControl) getDevice(deviceId); String pin = null; Double angle = null; if (ec instanceof Amt203Encoder) { // type = 0; pin = ((Amt203Encoder) ec).getPin(); + angle = 360.0 * position / ((As5048AEncoder) ec).resolution; } else if (ec instanceof As5048AEncoder) { // 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: consolidate some of this logic .. some of it is assmebled here.. + // other data is also computed in the AbstractPinEncoder EncoderData data = new EncoderData(ec.getName(), pin, position, angle); - // log.info("Publish Encoder Data Raw {}", data); - - // TODO: all this code needs to move out of here! + //log.info("Publish encoder data {}", data); return data; } diff --git a/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java b/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java index c0ab61b5d3..62773fc1b1 100644 --- a/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java +++ b/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java @@ -41,31 +41,27 @@ public Double publishEncoderAngle(Double angle) { return angle; } - // FIXME - remove this ... + // This is used to relay the data being broadcast from a controller (such as an arduino) public void onEncoderData(EncoderData data) { - // this is getting published from the arduino and updated here when it comes - // in.. - // TODO: shoudl the messaging be setup differently? - // TODO: compare with ultrasonic sensor and see that we're following the - // same pattern - // TODO: maybe use nanoTime? how accurate is this. + // TODO: maybe the raw pin data from the arduino comes in here instead.. + // current timestamp / delta since last update. long now = System.currentTimeMillis(); long delta = now - lastUpdate; - Double angle = 360.0 * data.angle / resolution; - log.info("Angle : {}", angle); if (delta > 0) { // we can compute velocity since the last update // This computes the change in degrees per second that the encoder is // currently moving at. - velocity = (angle - this.lastPosition) / delta * 1000.0; + velocity = (data.angle - this.lastPosition) / delta * 1000.0; } else { // no position update since the last tick. velocity = 0.0; } // update the previous values - this.lastPosition = angle; + this.lastPosition = data.angle; this.lastUpdate = now; - log.info("Encoder Data : {} Angle : {}", data, lastPosition); + // log.info("Encoder Data : {} Angle : {}", data, lastPosition); + // now that we've updated our state.. we can publish along the data. + broadcast("publishEncoderData", data); } public void setZeroPoint() { From 15b2013ca01a52973a6ae1d6467e6f033362a26c Mon Sep 17 00:00:00 2001 From: kwatters Date: Sun, 14 Mar 2021 13:33:52 -0400 Subject: [PATCH 02/10] more stuff for DiyServo2... --- .../org/myrobotlab/service/DiyServo2.java | 496 ++++++++++++++++++ .../service/meta/DiyServo2Meta.java | 28 + 2 files changed, 524 insertions(+) create mode 100755 src/main/java/org/myrobotlab/service/DiyServo2.java create mode 100755 src/main/java/org/myrobotlab/service/meta/DiyServo2Meta.java 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..b28fc72226 --- /dev/null +++ b/src/main/java/org/myrobotlab/service/DiyServo2.java @@ -0,0 +1,496 @@ +/** + * + * @author GroG & Mats (at) myrobotlab.org + * + * 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.interfaces.EncoderControl; +import org.myrobotlab.service.interfaces.MotorControl; +import org.myrobotlab.service.interfaces.ServoControl; +import org.myrobotlab.service.interfaces.ServoController; + +/** + * 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 { + + 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.020; + private double ki = 0.001; // 0.020; + private double kd = 0.0; // 0.020; + public double setPoint = 90.0; // Intial + int sampleTime = 20; + static final public int MODE_AUTOMATIC = 1; + + MotorUpdater motorUpdater; + EncoderControl encoder; + private Double rest = 90.0; + + public DiyServo2(String reservedKey, String inId) { + super(reservedKey, inId); + } + + @Override + synchronized public void startService() { + super.startService(); + initPid(); + } + + void initPid() { + 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; + // Update the pid input value. + pid.setInput(pidKey, data.angle); + } + + + public void attachEncoderControl(EncoderControl publisher) { + // Should i attach just the publisher? or do i care about the whole control? + // for now, minimal.. only publisher. + this.encoder = publisher; + As5048AEncoder enc = (As5048AEncoder)publisher; + enc.addListener("publishEncoderData", getName()); + } + + 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 boolean moveTo(Integer angle) { + return moveTo(Double.valueOf(angle)); + } + + public boolean moveTo(Double angle) { + // This updates the setpoint of the pid control. + pid.setSetpoint(pidKey, angle); + // Why does this return a boolean? + return true; + } + + /** + * 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; + private double lastCurrentPosInput = 0; + // goal is to not use this + + public MotorUpdater(String name) { + super(String.format("%s.motorUpdater", name)); + } + + @Override + public void run() { + + try { + while (true) { + if (enabled) { + if (motorControl != null) { + // Calculate the new value for the motor + // TODO: this probably needs to be synchronized. + if (pid.compute(pidKey)) { + // double setPoint = pid.getSetpoint(pidKey); + double output = pid.getOutput(pidKey); + log.info("Pid output: {}" , output); + // TODO: avoid duplicating the move calls? + if (output != lastOutput) { + log.info("move motor : {}", output); + motorControl.move(output); + lastOutput = output; + // let's see if we've stopped. + // TODO: some debouncing logic here. + // TODO: publish the servo events for started/stopped here. + } + //Test if we've arrived ? + double delta = Math.abs(lastCurrentPosInput - setPoint); + double threshold = 0.5; + if (delta < threshold ) { + log.info("Arrived!"); + } + } + } + // wait for the next update loop. + Thread.sleep(1000 / sampleTime); + } + } + } catch (Exception e) { + if (e instanceof InterruptedException) { + // info("Shutting down MotorUpdater"); + motorControl.stop(); + } else { + log.error("motor updater threw", e); + } + } + } + } + + public static void main(String[] args) throws Exception { + + LoggingFactory.init("info"); + // Make one.. and stuff. + // setup the encoder. + Arduino ard = (Arduino)Runtime.start("ard", "Arduino"); + ard.connect("COM4"); + // 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, 5); + ard.attach(mot); + // TODO: attach both to the diyservo and set the pin. + DiyServo2 diy = (DiyServo2)Runtime.createAndStart("diy", "DiyServo2"); + // attach the encoder and motor + diy.attachEncoderControl(encoder); + diy.attachMotorControl(mot); + // Tell the servo to move somewhere. + diy.moveTo(89.0); + + Thread.sleep(1000); + diy.disable(); + Thread.sleep(1000); + diy.enable(); + Thread.sleep(1000); + diy.moveTo(90.0); + + System.out.println("Press the any key"); + System.in.read(); + + } + + @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 Auto-generated method stub + // TODO: what do to here? + // motorControl.enable(); + enabled = true; + } + + @Override + public double getRest() { + // TODO Auto-generated method stub + // Ok.. not a bad idea.. let's have a rest position for the servo.. default to 90 deg? or something? + return rest; + } + + + // TODO: the following methods are really cruft from the ServoControl interface + // for now most all of these methods are NoOp for the DiyServo2 service. + @Override + public boolean isAutoDisable() { + // TODO: not a bad idea to support this ... + return false; + } + + @Override + public void attach(ServoController listener) { + // TODO: remove from ServoControl interface... NoOp here. + // NoOp : no servo controllers here.. + } + + @Override + public void detach(ServoController listener) { + // TODO maybe remove from interface? this service doesn't give a crapola about servo controllers. + } + + @Override + public String getController() { + // TODO remove from interface.. it has no function here. + 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() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public Mapper getMapper() { + // TODO Auto-generated method stub + // Really? + return null; + } + + @Override + public double getMax() { + // TODO Auto-generated method stub + // 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 0; + } + + @Override + public double getMin() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public String getPin() { + // TODO: This doesn't mean anything here. + // maybe this is the pin from the encoder? but why.. + return null; + } + + @Override + public double getCurrentInputPos() { + // TODO Auto-generated method stub + // TODO: a diy servo always knows where it is.. what is this asking for? + // TODO: return currentAngle? + return 0; + + } + + @Override + public double getCurrentOutputPos() { + // TODO Auto-generated method stub + // TODO: is this the current angle? why do we havge input and output positions?! gah.. this interface has way too much stuff in it. + return currentAngle; + } + + @Override + public Double getSpeed() { + // TODO: ok. nice.. this might be picked up from a base class, if we go there. + return null; + } + + @Override + public double getTargetOutput() { + // the setPoint for the pid control is the target output. + return setPoint; + } + + @Override + public double getTargetPos() { + // TODO the setPoint is the target output.. + return setPoint; + } + + @Override + public boolean isBlocking() { + // TODO Auto-generated method stub + return false; + } + + @Override + public Boolean isEnabled() { + // TODO Auto-generated method stub + return enabled; + } + + @Override + public Boolean isInverted() { + // TODO Auto-generated method stub + return null; + } + + @Override + public boolean isMoving() { + // TODO Auto-generated method stub + 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 setAutoDisable(Boolean autoDisable) { + // TODO ugh. impl me. + } + + @Override + public void setInverted(Boolean invert) { + // TODO: impl me.. this should probably control the pid output * -1 ? + } + + @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. + } + + @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! + } + + @Override + public void setPin(String pin) { + // TODO remove from interface? We don't have any pins. + } + + @Override + public void setPosition(double pos) { + // TODO: maybe deprecate and remove from interface ? + 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. + motorControl.move(0.0); + } + + @Override + public void sync(ServoControl sc) { + // TODO Impl me. + } + + @Override + public void unsync(ServoControl sc) { + // TODO Auto-generated method stub + + } + + @Override + public void waitTargetPos() { + // TODO Auto-generated method stub + // really? ok. + + } + + @Override + public void writeMicroseconds(int uS) { + // NoOp here... should be removed from ServoControl interface + } + + @Override + public void attachServoController(String sc, Integer pin, Double pos, Double speed) { + // NoOp here.. should be removed from ServoControl interface + } + + @Override + public void fullSpeed() { + // TODO: add a velocity control. + // TODO: deprecated, remove from interface? + } + +} \ 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..c7530910e6 --- /dev/null +++ b/src/main/java/org/myrobotlab/service/meta/DiyServo2Meta.java @@ -0,0 +1,28 @@ +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) { + + super(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"); + addPeer("motor", "MotorDualPwm", "MotorControl service"); + addPeer("pid", "Pid", "PID service"); + + } + +} From adc59fca87f3c5da5cf9dad3e0d14103da77cec4 Mon Sep 17 00:00:00 2001 From: kwatters Date: Tue, 16 Mar 2021 11:49:41 -0400 Subject: [PATCH 03/10] worky DiyServo2 ... more testing and cleanup, but.. this seems to be worky. --- .../org/myrobotlab/service/DiyServo2.java | 93 +++++++++++-------- 1 file changed, 52 insertions(+), 41 deletions(-) diff --git a/src/main/java/org/myrobotlab/service/DiyServo2.java b/src/main/java/org/myrobotlab/service/DiyServo2.java index b28fc72226..906ecc78de 100755 --- a/src/main/java/org/myrobotlab/service/DiyServo2.java +++ b/src/main/java/org/myrobotlab/service/DiyServo2.java @@ -1,7 +1,5 @@ /** * - * @author GroG & Mats (at) myrobotlab.org - * * This file is part of MyRobotLab (http://myrobotlab.org). * * MyRobotLab is free software: you can redistribute it and/or modify @@ -57,9 +55,9 @@ public class DiyServo2 extends Service implements EncoderListener, ServoControl //TODO: use the motor name. public String pidKey = "diy2"; - private double kp = 0.020; + private double kp = 0.05; private double ki = 0.001; // 0.020; - private double kd = 0.0; // 0.020; + private double kd = 0.001; // 0.020; public double setPoint = 90.0; // Intial int sampleTime = 20; static final public int MODE_AUTOMATIC = 1; @@ -67,6 +65,7 @@ public class DiyServo2 extends Service implements EncoderListener, ServoControl MotorUpdater motorUpdater; EncoderControl encoder; private Double rest = 90.0; + private long lastActivityTimeMS; public DiyServo2(String reservedKey, String inId) { super(reservedKey, inId); @@ -92,10 +91,8 @@ void initPid() { @Override public void onEncoderData(EncoderData data) { - System.err.println("DIY Servo Encoder Data: " + data); + // System.err.println("DIY Servo Encoder Data: " + data); this.currentAngle = data.angle; - // Update the pid input value. - pid.setInput(pidKey, data.angle); } @@ -104,6 +101,7 @@ public void attachEncoderControl(EncoderControl publisher) { // for now, minimal.. only publisher. this.encoder = publisher; As5048AEncoder enc = (As5048AEncoder)publisher; + enc.addListener("publishEncoderData", getName()); } @@ -127,7 +125,9 @@ public boolean moveTo(Integer angle) { public boolean moveTo(Double 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? return true; } @@ -140,7 +140,7 @@ public boolean moveTo(Double angle) { public class MotorUpdater extends Thread { double lastOutput = 0.0; - private double lastCurrentPosInput = 0; + // goal is to not use this public MotorUpdater(String name) { @@ -152,34 +152,35 @@ public void run() { try { while (true) { - if (enabled) { - if (motorControl != null) { - // Calculate the new value for the motor - // TODO: this probably needs to be synchronized. - if (pid.compute(pidKey)) { - // double setPoint = pid.getSetpoint(pidKey); - double output = pid.getOutput(pidKey); - log.info("Pid output: {}" , output); - // TODO: avoid duplicating the move calls? - if (output != lastOutput) { - log.info("move motor : {}", output); - motorControl.move(output); - lastOutput = output; - // let's see if we've stopped. - // TODO: some debouncing logic here. - // TODO: publish the servo events for started/stopped here. - } - //Test if we've arrived ? - double delta = Math.abs(lastCurrentPosInput - setPoint); - double threshold = 0.5; - if (delta < threshold ) { - log.info("Arrived!"); - } + if (isRunning()) { + // Calculate the new value for the motor + // TODO: this probably needs to be synchronized. + if (pid.compute(pidKey)) { + // double setPoint = pid.getSetpoint(pidKey); + // TODO: maybe set the input here based on the current angle? instead of the onEncoderData? + // Update the pid input value. + pid.setInput(pidKey, currentAngle); + double output = pid.getOutput(pidKey); + // log.info("Pid output: {}" , output); + // TODO: avoid duplicating the move calls? + double delta = Math.abs(currentAngle - setPoint); + if (output != lastOutput) { + log.info("move motor : Power: {} Target: {} Current: {} Delta: {}", output, setPoint, currentAngle, delta); + motorControl.move(output); + lastOutput = output; + // let's see if we've stopped. + // TODO: some debouncing logic here. + // TODO: publish the servo events for started/stopped here. + } + //Test if we've arrived ? + double threshold = 0.5; + if (delta < threshold ) { + log.info("Arrived!"); } } - // wait for the next update loop. - Thread.sleep(1000 / sampleTime); } + // wait for the next update loop. + Thread.sleep(1000 / sampleTime); } } catch (Exception e) { if (e instanceof InterruptedException) { @@ -190,11 +191,22 @@ public void run() { } } } + + private boolean isRunning() { + // if we are enabled, have a motor connected and have received encoder data. + return enabled && motorControl != null && currentAngle != null; + } } public static void main(String[] args) throws Exception { LoggingFactory.init("info"); + + Runtime.start("gui", "SwingGui"); + Thread.sleep(1000); + Runtime.start("python", "Python"); + Thread.sleep(1000); + // Make one.. and stuff. // setup the encoder. Arduino ard = (Arduino)Runtime.start("ard", "Arduino"); @@ -214,14 +226,14 @@ public static void main(String[] args) throws Exception { diy.attachEncoderControl(encoder); diy.attachMotorControl(mot); // Tell the servo to move somewhere. - diy.moveTo(89.0); + diy.moveTo(75.0); + Thread.sleep(2000); +// diy.disable(); +// Thread.sleep(1000); +// diy.enable(); Thread.sleep(1000); - diy.disable(); - Thread.sleep(1000); - diy.enable(); - Thread.sleep(1000); - diy.moveTo(90.0); + diy.moveTo(125.0); System.out.println("Press the any key"); System.in.read(); @@ -247,7 +259,6 @@ public void enable() { @Override public double getRest() { - // TODO Auto-generated method stub // Ok.. not a bad idea.. let's have a rest position for the servo.. default to 90 deg? or something? return rest; } @@ -288,7 +299,7 @@ public EncoderControl getEncoder() { @Override public long getLastActivityTime() { // TODO Auto-generated method stub - return 0; + return lastActivityTimeMS; } @Override From a5ad4f68636eacf5f0ed945fc5ba1c4dc13ac53f Mon Sep 17 00:00:00 2001 From: kwatters Date: Wed, 17 Mar 2021 10:50:57 -0400 Subject: [PATCH 04/10] encoder controls being able to attach encoder listeners. --- .../org/myrobotlab/sensor/TimeEncoder.java | 5 + .../org/myrobotlab/service/DiyServo2.java | 93 +++++++++++++------ .../java/org/myrobotlab/service/RoboClaw.java | 6 ++ .../service/abstracts/AbstractPinEncoder.java | 7 ++ .../service/interfaces/EncoderControl.java | 9 ++ 5 files changed, 94 insertions(+), 26 deletions(-) diff --git a/src/main/java/org/myrobotlab/sensor/TimeEncoder.java b/src/main/java/org/myrobotlab/sensor/TimeEncoder.java index 2fddd40e29..b894f53c30 100644 --- a/src/main/java/org/myrobotlab/sensor/TimeEncoder.java +++ b/src/main/java/org/myrobotlab/sensor/TimeEncoder.java @@ -455,4 +455,9 @@ public void removeListener(String localTopic, String otherService) { // TODO Auto-generated method stub } + + @Override + public void attachEncoderListener(EncoderListener listener) { + // TODO: Probably not supported unless the TimerEncoder is actually a full blown service. + } } \ No newline at end of file diff --git a/src/main/java/org/myrobotlab/service/DiyServo2.java b/src/main/java/org/myrobotlab/service/DiyServo2.java index 906ecc78de..9ddfa24960 100755 --- a/src/main/java/org/myrobotlab/service/DiyServo2.java +++ b/src/main/java/org/myrobotlab/service/DiyServo2.java @@ -31,7 +31,9 @@ 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.ServoStatusPublisher; /** * Simple(ish) DiyServo2. @@ -45,8 +47,9 @@ * The output of the pid control is then written to the motor control */ -public class DiyServo2 extends Service implements EncoderListener, ServoControl { +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; @@ -59,7 +62,7 @@ public class DiyServo2 extends Service implements EncoderListener, ServoControl private double ki = 0.001; // 0.020; private double kd = 0.001; // 0.020; public double setPoint = 90.0; // Intial - int sampleTime = 20; + public int sampleTime = 20; static final public int MODE_AUTOMATIC = 1; MotorUpdater motorUpdater; @@ -74,6 +77,7 @@ public DiyServo2(String reservedKey, String inId) { @Override synchronized public void startService() { super.startService(); + pidKey = getFullName(); initPid(); } @@ -95,14 +99,10 @@ public void onEncoderData(EncoderData data) { this.currentAngle = data.angle; } - - public void attachEncoderControl(EncoderControl publisher) { - // Should i attach just the publisher? or do i care about the whole control? - // for now, minimal.. only publisher. - this.encoder = publisher; - As5048AEncoder enc = (As5048AEncoder)publisher; - - enc.addListener("publishEncoderData", getName()); + 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) { @@ -140,7 +140,8 @@ public boolean moveTo(Double angle) { 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) { @@ -156,35 +157,27 @@ public void run() { // Calculate the new value for the motor // TODO: this probably needs to be synchronized. if (pid.compute(pidKey)) { - // double setPoint = pid.getSetpoint(pidKey); - // TODO: maybe set the input here based on the current angle? instead of the onEncoderData? // Update the pid input value. pid.setInput(pidKey, currentAngle); double output = pid.getOutput(pidKey); - // log.info("Pid output: {}" , output); - // TODO: avoid duplicating the move calls? double delta = Math.abs(currentAngle - setPoint); - if (output != lastOutput) { + if (delta < threshold ) { + log.info("Arrived!"); + // 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; - // let's see if we've stopped. - // TODO: some debouncing logic here. - // TODO: publish the servo events for started/stopped here. - } - //Test if we've arrived ? - double threshold = 0.5; - if (delta < threshold ) { - log.info("Arrived!"); } } } - // wait for the next update loop. + // TODO: maybe a more accurate loop timer? Thread.sleep(1000 / sampleTime); } } catch (Exception e) { if (e instanceof InterruptedException) { - // info("Shutting down MotorUpdater"); motorControl.stop(); } else { log.error("motor updater threw", e); @@ -504,4 +497,52 @@ public void fullSpeed() { // TODO: deprecated, remove from interface? } + @Override + public String publishServoStarted(String name) { + // TODO Auto-generated method stub + return null; + } + + @Override + public String publishServoStopped(String name) { + // TODO Auto-generated method stub + return null; + } + + @Override + public ServoControl publishServoMoveTo(ServoControl sc) { + // TODO Auto-generated method stub + return null; + } + + @Override + public ServoControl publishMoveTo(ServoControl sc) { + // TODO Auto-generated method stub + return null; + } + + @Override + public ServoControl publishServoSetSpeed(ServoControl sc) { + // TODO Auto-generated method stub + return null; + } + + @Override + public ServoControl publishServoEnable(ServoControl sc) { + // TODO Auto-generated method stub + return null; + } + + @Override + public ServoControl publishServoDisable(ServoControl sc) { + // TODO Auto-generated method stub + return null; + } + + @Override + public ServoControl publishServoStop(ServoControl sc) { + // TODO Auto-generated method stub + return null; + } + } \ No newline at end of file diff --git a/src/main/java/org/myrobotlab/service/RoboClaw.java b/src/main/java/org/myrobotlab/service/RoboClaw.java index 540d3c0845..54d27d9ad1 100644 --- a/src/main/java/org/myrobotlab/service/RoboClaw.java +++ b/src/main/java/org/myrobotlab/service/RoboClaw.java @@ -12,6 +12,7 @@ import org.myrobotlab.logging.Logging; import org.myrobotlab.logging.LoggingFactory; import org.myrobotlab.sensor.EncoderData; +import org.myrobotlab.sensor.EncoderListener; import org.myrobotlab.sensor.EncoderPublisher; import org.myrobotlab.serial.CRC; import org.myrobotlab.service.Pid.PidData; @@ -2363,6 +2364,11 @@ public void onBytes(byte[] bytes) { } } + public void attachEncoderListener(EncoderListener enc) { + // subscribe the onEncoderData of the listener. + addListener("publishEncoderData", enc.getName(), "onEncoderData"); + } + @Override public EncoderData publishEncoderData(EncoderData data) { return data; diff --git a/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java b/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java index 62773fc1b1..950a383a75 100644 --- a/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java +++ b/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java @@ -2,6 +2,7 @@ import org.myrobotlab.framework.Service; import org.myrobotlab.sensor.EncoderData; +import org.myrobotlab.sensor.EncoderListener; import org.myrobotlab.service.interfaces.EncoderControl; import org.myrobotlab.service.interfaces.EncoderController; @@ -101,4 +102,10 @@ public EncoderData publishEncoderData(EncoderData data) { public Double getPos() { return lastPosition; } + + @Override + public void attachEncoderListener(EncoderListener listener) { + // TODO: should this be full name? + addListener("publishEncoderData", listener.getName()); + } } diff --git a/src/main/java/org/myrobotlab/service/interfaces/EncoderControl.java b/src/main/java/org/myrobotlab/service/interfaces/EncoderControl.java index 2bf448749b..24ed89929f 100755 --- a/src/main/java/org/myrobotlab/service/interfaces/EncoderControl.java +++ b/src/main/java/org/myrobotlab/service/interfaces/EncoderControl.java @@ -2,6 +2,7 @@ import org.myrobotlab.framework.interfaces.Attachable; import org.myrobotlab.sensor.EncoderData; +import org.myrobotlab.sensor.EncoderListener; public interface EncoderControl extends Attachable { @@ -23,6 +24,14 @@ public interface EncoderControl extends Attachable { */ public EncoderData publishEncoderData(EncoderData data); + /** + * attaches an encoder listener to get the publishEncoderData + * to invoke the onEncoderData method of the listener. + * + * @param listener + */ + public void attachEncoderListener(EncoderListener listener); + /** * return the state of streaming encoder data * From e9fafe97b21f488d7d0adaa225bb981047ba6719 Mon Sep 17 00:00:00 2001 From: kwatters Date: Fri, 19 Mar 2021 13:18:58 -0400 Subject: [PATCH 05/10] filling in some of the blanks.. --- .../org/myrobotlab/service/DiyServo2.java | 173 +++++++++--------- 1 file changed, 85 insertions(+), 88 deletions(-) diff --git a/src/main/java/org/myrobotlab/service/DiyServo2.java b/src/main/java/org/myrobotlab/service/DiyServo2.java index 9ddfa24960..452acbea5e 100755 --- a/src/main/java/org/myrobotlab/service/DiyServo2.java +++ b/src/main/java/org/myrobotlab/service/DiyServo2.java @@ -191,48 +191,6 @@ private boolean isRunning() { } } - public static void main(String[] args) throws Exception { - - LoggingFactory.init("info"); - - Runtime.start("gui", "SwingGui"); - Thread.sleep(1000); - Runtime.start("python", "Python"); - Thread.sleep(1000); - - // Make one.. and stuff. - // setup the encoder. - Arduino ard = (Arduino)Runtime.start("ard", "Arduino"); - ard.connect("COM4"); - // 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, 5); - ard.attach(mot); - // TODO: attach both to the diyservo and set the pin. - DiyServo2 diy = (DiyServo2)Runtime.createAndStart("diy", "DiyServo2"); - // 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(); - Thread.sleep(1000); - diy.moveTo(125.0); - - System.out.println("Press the any key"); - System.in.read(); - - } - @Override public void disable() { // TODO: what do do here? @@ -244,7 +202,6 @@ public void disable() { @Override public void enable() { - // TODO Auto-generated method stub // TODO: what do to here? // motorControl.enable(); enabled = true; @@ -256,12 +213,9 @@ public double getRest() { return rest; } - - // TODO: the following methods are really cruft from the ServoControl interface - // for now most all of these methods are NoOp for the DiyServo2 service. @Override public boolean isAutoDisable() { - // TODO: not a bad idea to support this ... + // TODO: impl this.. safety is good. return false; } @@ -269,16 +223,19 @@ public boolean isAutoDisable() { 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.. it has no function here. + // TODO remove from interface?. we have no controller. + log.warn("Diy Servo doesn't use a controller.. no implemented."); return null; } @@ -291,32 +248,31 @@ public EncoderControl getEncoder() { @Override public long getLastActivityTime() { - // TODO Auto-generated method stub return lastActivityTimeMS; } @Override public Mapper getMapper() { - // TODO Auto-generated method stub - // Really? + // TODO - we have no mapper... return null; } @Override public double getMax() { - // TODO Auto-generated method stub + // 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 0; + return 360; } @Override public double getMin() { - // TODO Auto-generated method stub + // 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; @@ -324,59 +280,57 @@ public String getPin() { @Override public double getCurrentInputPos() { - // TODO Auto-generated method stub - // TODO: a diy servo always knows where it is.. what is this asking for? - // TODO: return currentAngle? - return 0; + // TODO: return currentAngle? we have no mapper. + return currentAngle; } @Override public double getCurrentOutputPos() { - // TODO Auto-generated method stub - // TODO: is this the current angle? why do we havge input and output positions?! gah.. this interface has way too much stuff in it. + // TODO: this interface has way too much stuff in it... + // return the last known encoder angle return currentAngle; } @Override public Double getSpeed() { - // TODO: ok. nice.. this might be picked up from a base class, if we go there. + // 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() { - // TODO the setPoint is the target output.. + // This is the setPoint for the pid control.. the target position. return setPoint; } @Override public boolean isBlocking() { - // TODO Auto-generated method stub + // TODO What does this mean? should we remove this from the interface? return false; } @Override public Boolean isEnabled() { - // TODO Auto-generated method stub return enabled; } @Override public Boolean isInverted() { - // TODO Auto-generated method stub - return null; + // TODO not sure what this means for a diyservo. + return false; } @Override public boolean isMoving() { - // TODO Auto-generated method stub + // TODO we should trigger this off the power output being sent to the motor. return false; } @@ -423,6 +377,8 @@ public void setMapper(Mapper m) { @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 @@ -433,16 +389,20 @@ public void setMinMaxOutput(double minY, double maxY) { @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. + // 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); } @@ -459,7 +419,7 @@ public void setSpeed(Double degreesPerSecond) { @Override public void stop() { - // Stop the motor. + // Stop the motor... anything else? motorControl.move(0.0); } @@ -470,25 +430,27 @@ public void sync(ServoControl sc) { @Override public void unsync(ServoControl sc) { - // TODO Auto-generated method stub - + // 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 + // 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 attachServoController(String sc, Integer pin, Double pos, Double speed) { // NoOp here.. should be removed from ServoControl interface + log.warn("Attach Servo Controller not implemented for DiyServo."); } @Override @@ -499,50 +461,85 @@ public void fullSpeed() { @Override public String publishServoStarted(String name) { - // TODO Auto-generated method stub - return null; + return name; } @Override public String publishServoStopped(String name) { - // TODO Auto-generated method stub - return null; + return name; } @Override public ServoControl publishServoMoveTo(ServoControl sc) { - // TODO Auto-generated method stub - return null; + return sc; } @Override public ServoControl publishMoveTo(ServoControl sc) { - // TODO Auto-generated method stub - return null; + return sc; } @Override public ServoControl publishServoSetSpeed(ServoControl sc) { - // TODO Auto-generated method stub - return null; + return sc; } @Override public ServoControl publishServoEnable(ServoControl sc) { - // TODO Auto-generated method stub - return null; + return sc; } @Override public ServoControl publishServoDisable(ServoControl sc) { - // TODO Auto-generated method stub - return null; + return sc; } @Override public ServoControl publishServoStop(ServoControl sc) { - // TODO Auto-generated method stub - return null; + return sc; + } + + + public static void main(String[] args) throws Exception { + + LoggingFactory.init("info"); + + Runtime.start("gui", "SwingGui"); + Thread.sleep(1000); + Runtime.start("python", "Python"); + Thread.sleep(1000); + + // Make one.. and stuff. + // setup the encoder. + Arduino ard = (Arduino)Runtime.start("ard", "Arduino"); + ard.connect("COM4"); + // 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, 5); + ard.attach(mot); + // TODO: attach both to the diyservo and set the pin. + DiyServo2 diy = (DiyServo2)Runtime.createAndStart("diy", "DiyServo2"); + // 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(); + Thread.sleep(1000); + diy.moveTo(125.0); + + System.out.println("Press the any key"); + System.in.read(); + } } \ No newline at end of file From 45298fb48a406335e36b99d6c1a93c46579f5eb8 Mon Sep 17 00:00:00 2001 From: kwatters Date: Sun, 1 Jun 2025 11:52:10 -0400 Subject: [PATCH 06/10] reverting some changes --- .../org/myrobotlab/sensor/TimeEncoder.java | 148 ++----- .../java/org/myrobotlab/service/RoboClaw.java | 387 ++++++++++++++++-- 2 files changed, 393 insertions(+), 142 deletions(-) diff --git a/src/main/java/org/myrobotlab/sensor/TimeEncoder.java b/src/main/java/org/myrobotlab/sensor/TimeEncoder.java index b894f53c30..43233f46d2 100644 --- a/src/main/java/org/myrobotlab/sensor/TimeEncoder.java +++ b/src/main/java/org/myrobotlab/sensor/TimeEncoder.java @@ -1,18 +1,13 @@ package org.myrobotlab.sensor; -import java.io.File; -import java.util.Map; import java.util.Set; -import java.util.concurrent.ConcurrentHashMap; -import org.myrobotlab.codec.CodecUtils; import org.myrobotlab.framework.Service; import org.myrobotlab.framework.interfaces.Attachable; import org.myrobotlab.framework.interfaces.Broadcaster; -import org.myrobotlab.io.FileIO; import org.myrobotlab.logging.LoggerFactory; -import org.myrobotlab.service.Servo; import org.myrobotlab.service.interfaces.EncoderControl; +import org.myrobotlab.service.interfaces.EncoderController; import org.myrobotlab.service.interfaces.ServoControl; import org.slf4j.Logger; @@ -88,108 +83,11 @@ public class TimeEncoder implements Runnable, EncoderControl { boolean enabled = true; - static class Positions implements Runnable { - - static Positions instance = null; - Map positions = null; - transient private Thread worker; - transient boolean running = false; - String filename = null; - // reference counter - when 0 shuts thread down - int refCount; - - public Positions() { - // load previous positions - String positionsDir = Service.getDataDir(Servo.class.getSimpleName()); - filename = positionsDir + File.separator + "positions.json"; - - if (positions == null) { - Map savedPositions = null; - try { - String json = FileIO.toString(filename); - if (json != null) { - savedPositions = CodecUtils.fromJson(json, ConcurrentHashMap.class); - } - } catch (Exception e) { - } - if (savedPositions != null) { - positions = savedPositions; - } else { - positions = new ConcurrentHashMap<>(); - } - } - } - - @Override - public void run() { - - running = true; - - while (running) { - try { - Thread.sleep(2000); - log.debug("saving {} positions of {} servos", filename, positions.size()); - FileIO.toFile(filename, CodecUtils.toJson(positions).getBytes()); - } catch (Exception e) { - log.error("could not save servo positions", e); - } - } // while (running) - - worker = null; - } - - synchronized public void release() { - --refCount; - if (refCount == 0) { - // no one is using time encoders.... - // shutting down - running = false; - } - } - - synchronized public void start() { - ++refCount; - if (worker == null) { - worker = new Thread(this, "TimeEncoder.Positions"); - worker.start(); - } - } - - public void setPosition(String name, double pos) { - positions.put(name, pos); - } - - public static Positions getInstance() { - if (instance == null) { - instance = new Positions(); - } - return instance; - } - - public Double getPosition(String name) { - if (positions.containsKey(name)) { - return positions.get(name); - } - return null; - } - } - - /** - * Static position saver for all servos which use a time encoder. the - * positions are read back in on starting the Servo - */ - Positions positions = null; - - protected boolean enableServoEvents = true; + protected boolean stopMove = false; public TimeEncoder(ServoControl servo) { this.servo = servo; this.name = servo.getName(); - positions = Positions.getInstance(); - Double p = positions.getPosition(servo.getName()); - if (p != null) { - beginPos = targetPos = estimatedPos = p; - } enable(); } @@ -240,9 +138,12 @@ public void run() { isRunning = true; while (isRunning) { + // wait for next move ... synchronized (this) { + stopMove = false; this.wait(); + stopMove = false; } if (speedDegreesPerMs == 0) { @@ -256,6 +157,11 @@ public void run() { while (now < endMoveTs && isRunning) { + if (stopMove) { + endMoveTs = now; + break; + } + now = System.currentTimeMillis(); // speed has +/- direction estimatedPos = beginPos + speedDegreesPerMs * (now - beginMoveTs); @@ -272,11 +178,10 @@ public void run() { // targetPos, estimatedPos) !!! EncoderData d = new EncoderData(name, null, estimatedPos, estimatedPos); - positions.setPosition(name, estimatedPos); - if (enableServoEvents && started) { + if (started) { // ((Broadcaster)servo).broadcast("publishedServoStopped", // ServoStatus.SERVO_STOPPED, estimatedPos); - ((Broadcaster) servo).broadcast("publishServoStarted", servo.getName()); + ((Broadcaster) servo).broadcast("publishServoStarted", servo.getName(), estimatedPos); started = false; } servo.onEncoderData(d);// FIXME !! - broadcast this @@ -287,12 +192,11 @@ public void run() { // log.info("finished moved"); EncoderData d = new EncoderData(name, null, estimatedPos, estimatedPos); servo.onEncoderData(d); - if (enableServoEvents) { - // ((Broadcaster)servo).broadcast("publishedServoStopped", - // ServoStatus.SERVO_STOPPED, estimatedPos); - ((Broadcaster) servo).broadcast("publishServoStopped", servo.getName()); - } - positions.setPosition(name, estimatedPos); + // ((Broadcaster)servo).broadcast("publishedServoStopped", + // ServoStatus.SERVO_STOPPED, estimatedPos); + // FYI - broadcast by-passes queues, but can publish based on notify + // entries + ((Broadcaster) servo).broadcast("publishServoStopped", servo.getName(), estimatedPos); } } catch (InterruptedException e) { log.info("stopping TimeEncoder Timer ..."); @@ -300,6 +204,7 @@ public void run() { myThread = null; } + @Override public String toString() { return String.format("@ ts %d starting at position %.1f %s will travel %.1f degrees to position %.1f in %.1f ms ending at %d ts", beginMoveTs, beginPos, name, distance, targetPos, moveTimeMs, endMoveTs); @@ -369,12 +274,10 @@ public void disable() { if (myThread != null) { myThread.interrupt(); } - positions.release(); } @Override public void enable() { - positions.start(); if (myThread == null) { myThread = new Thread(this, String.format("%s.%s.time-encoder", servo.getName(), getName())); myThread.start(); @@ -400,7 +303,6 @@ public Double getPos() { public void setPos(Double pos) { beginPos = targetPos = estimatedPos = pos; - positions.setPosition(name, estimatedPos); } @Override @@ -457,7 +359,17 @@ public void removeListener(String localTopic, String otherService) { } @Override - public void attachEncoderListener(EncoderListener listener) { - // TODO: Probably not supported unless the TimerEncoder is actually a full blown service. + public Set getAttached(String publishingPoint) { + // TODO Auto-generated method stub + return null; + } + + public void stopMove() { + stopMove = true; + } + + @Override + public void attachEncoderController(EncoderController controller) { + // NoOp, the TimeEncoder doesn't need a controller. } } \ No newline at end of file diff --git a/src/main/java/org/myrobotlab/service/RoboClaw.java b/src/main/java/org/myrobotlab/service/RoboClaw.java index 805ed99d8d..2d3cfe705d 100644 --- a/src/main/java/org/myrobotlab/service/RoboClaw.java +++ b/src/main/java/org/myrobotlab/service/RoboClaw.java @@ -12,11 +12,11 @@ import org.myrobotlab.logging.Logging; import org.myrobotlab.logging.LoggingFactory; import org.myrobotlab.sensor.EncoderData; -import org.myrobotlab.sensor.EncoderListener; import org.myrobotlab.sensor.EncoderPublisher; import org.myrobotlab.serial.CRC; import org.myrobotlab.service.Pid.PidData; import org.myrobotlab.service.abstracts.AbstractMotorController; +import org.myrobotlab.service.config.AbstractMotorControllerConfig; import org.myrobotlab.service.interfaces.MotorControl; import org.myrobotlab.service.interfaces.MotorController; import org.myrobotlab.service.interfaces.PortConnector; @@ -55,7 +55,7 @@ * this value IS correct * */ -public class RoboClaw extends AbstractMotorController implements EncoderPublisher, PortConnector, MotorController, SerialDataListener { +public class RoboClaw extends AbstractMotorController implements EncoderPublisher, PortConnector, MotorController, SerialDataListener { private static final long serialVersionUID = 1L; @@ -109,28 +109,30 @@ public static class MotorData implements Serializable { } // FIXME separate the methods into mrl & roboclaw native + @Override public void connect(String port) throws Exception { connect(port, 38400, 8, 1, 0); } + @Override public void disconnect() throws IOException { if (serial != null) { serial.close(); } } + @Override public boolean motorDetach(String name) { - if (motors.containsKey(name)) { - motors.remove(name); - return true; - } - return false; + motors.remove(name); + return true; } + @Override public void motorMove(String name) { motorMove((MotorControl) Runtime.getService(name)); } + @Override public void motorMoveTo(String name, Integer position) { error("not implemented"); } @@ -247,12 +249,12 @@ void setBaudRate(int baudRate) { @Override public void motorMove(MotorControl mc) { - if (!motors.containsKey(mc.getName())) { + if (!motors.contains(mc.getName())) { error("%s not attached to %s", mc.getName(), getName()); return; } - MotorPort motor = (MotorPort) motors.get(mc.getName()); + MotorPort motor = (MotorPort) Runtime.getService(mc.getName()); String port = motor.getPort(); /// double pwr = motor.getPowerLevel(); @@ -394,18 +396,20 @@ public void connect(String port, int rate, int databits, int stopbits, int parit sleep(3000); } + @Override public void detach(MotorControl device) { motors.remove(device); } // FIXME - become interface for motor port shields & controllers + @Override public List getPorts() { return ports; } @Override public Set getAttached() { - return motors.keySet(); + return motors; } /** @@ -428,7 +432,7 @@ public void attach(Attachable service) throws Exception { throw new IOException("port number in motor must be set to m1 or m2"); } - motors.put(motor.getName(), motor); + motors.add(motor.getName()); // give opportunity for motor to attach motor.attach(this); @@ -473,7 +477,7 @@ public void attach(Attachable service) throws Exception { @Override public boolean isAttached(Attachable service) { - return motors.containsKey(service.getName()) || (serial != null && serial.getName().equals(service.getName())); + return motors.contains(service.getName()) || (serial != null && serial.getName().equals(service.getName())); } @Override @@ -483,7 +487,7 @@ public void connect(String port, int rate) throws Exception { @Override public void detach() { - for (String name : motors.keySet()) { + for (String name : motors) { Motor m = (Motor) Runtime.getService(name); if (m != null) { m.detach(this); @@ -523,6 +527,9 @@ void driveForwardM1(int value) { Send: [Address, 1, Value, CRC(2 bytes)] Receive: [0xFF] * + * + * @param value + * v */ public void driveBackwardM1(int value) { sendPacket(address, 1, value); @@ -542,6 +549,9 @@ Sets main battery (B- / B+) minimum voltage level. If the battery voltages drops Brushed DC Motor Controllers RoboClaw Series User Manual 65 * + * + * @param value + * v */ public void setMiniMainVoltage(int value) { sendPacket(address, 2, value); @@ -559,6 +569,9 @@ Sets main battery (B- / B+) maximum voltage level. The valid data range is 30 - Send: [Address, 3, Value, CRC(2 bytes)] Receive: [0xFF] * + * + * @param value + * v */ public void setMaxMainVoltage(int value) { sendPacket(address, 3, value); @@ -572,6 +585,9 @@ public void setMaxMainVoltage(int value) { Send: [Address, 4, Value, CRC(2 bytes)] Receive: [0xFF] * + * + * @param value + * v */ public void driveForwardM2(int value) { sendPacket(address, 4, value); @@ -585,6 +601,9 @@ public void driveForwardM2(int value) { Send: [Address, 5, Value, CRC(2 bytes)] Receive: [0xFF] * + * + * @param value + * v */ public void driveBackwardM2(int value) { sendPacket(address, 5, value); @@ -598,6 +617,9 @@ public void driveBackwardM2(int value) { Send: [Address, 6, Value, CRC(2 bytes)] Receive: [0xFF] * + * + * @param value + * v */ public void driveM1(int value) { sendPacket(address, 6, value); @@ -618,6 +640,9 @@ public void driveM1(int value) { steering. Before a command is executed, both valid drive and turn data packets are required Once RoboClaw begins to operate the motors turn and speed can be updated independently. * + * + * @param value + * v */ public void driveM2(int value) { sendPacket(address, 7, value); @@ -631,6 +656,9 @@ public void driveM2(int value) { Send: [Address, 8, Value, CRC(2 bytes)] Receive: [0xFF] * + * + * @param value + * v */ public void driveForward(int value) { sendPacket(address, 8, value); @@ -644,6 +672,9 @@ public void driveForward(int value) { Send: [Address, 9, Value, CRC(2 bytes)] Receive: [0xFF] * + * + * @param value + * v */ public void driveBackward(int value) { sendPacket(address, 9, value); @@ -657,6 +688,9 @@ public void driveBackward(int value) { Send: [Address, 10, Value, CRC(2 bytes)] Receive: [0xFF] * + * + * @param value + * v */ public void turnRight(int value) { sendPacket(address, 10, value); @@ -670,6 +704,9 @@ public void turnRight(int value) { Send: [Address, 11, Value, CRC(2 bytes)] Receive: [0xFF] * + * + * @param value + * v */ public void turnLeft(int value) { sendPacket(address, 11, value); @@ -683,6 +720,9 @@ public void turnLeft(int value) { Send: [Address, 12, Value, CRC(2 bytes)] Receive: [0xFF] * + * + * @param value + * v */ public void driveForwardOrBackward(int value) { sendPacket(address, 12, value); @@ -696,6 +736,9 @@ public void driveForwardOrBackward(int value) { Send: [Address, 13, Value, CRC(2 bytes)] Receive: [0xFF * + * + * @param value + * v */ public void turnLeftOrRight(int value) { sendPacket(address, 13, value); @@ -711,6 +754,8 @@ public void turnLeftOrRight(int value) { The command will return up to 48 bytes. The return string includes the product name and firmware version. The return string is terminated with a line feed (10) and null (0) character. * + * + * @return the firmware version */ public String readFirmwareVersion() { sendPacket(address, 21); @@ -727,6 +772,8 @@ public String readFirmwareVersion() { Send: [Address, 24] Receive: [Value(2 bytes), CRC(2 bytes)] * + * + * @return battery voltage */ public Integer readMainBatteryVoltage() { @@ -746,6 +793,8 @@ public Integer readMainBatteryVoltage() { Send: [Address, 25] Receive: [Value.Byte1, Value.Byte0, CRC(2 bytes)] * + * + * @return voltage */ public Integer readLogicbatteryVoltage() { logicBatteryVoltageLevel = null; @@ -771,6 +820,9 @@ Sets logic input (LB- / LB+) minimum voltage level. RoboClaw will shut down with Send: [Address, 26, Value, CRC(2 bytes)] Receive: [0xFF] * + * + * @param value + * v */ public void setMinLogicVoltage(int value) { sendPacket(address, 26, value); @@ -788,6 +840,9 @@ Sets logic input (LB- / LB+) maximum voltage level. The valid data range is 30 - Send: [Address, 27, Value, CRC(2 bytes)] Receive: [0xFF] * + * + * @param value + * v */ public void setMaxLogicVoltage(int value) { @@ -823,6 +878,9 @@ public void readMotorPwmValues() { Send: [Address, 49] Receive: [M1 Current(2 bytes), M2 Currrent(2 bytes), CRC(2 bytes)] * + * + * @param value + * v */ public void readMotorCurrents(int value) { sendPacket(address, 49); @@ -837,6 +895,11 @@ public void readMotorCurrents(int value) { Send: [Address, 57, Min(2 bytes), Max(2bytes, CRC(2 bytes)] Receive: [0xFF] * + * + * @param min + * min voltage + * @param max + * max voltage */ public void setMainBatteryVoltages(int min, int max) { sendPacket(address, 57, byte1(min), byte0(min), byte1(max), byte0(max)); @@ -866,6 +929,11 @@ private int byte0(int value) { Send: [Address, 58, Min(2 bytes), Max(2bytes, CRC(2 bytes)] Receive: [0xFF] * + * + * @param min + * voltage + * @param max + * voltage */ public void setLogicBatteryVoltages(int min, int max) { sendPacket(address, 58, byte1(min), byte0(min), byte1(max), byte0(max)); @@ -903,6 +971,9 @@ Set the default acceleration for M1 when using duty cycle commands(Cmds 32,33 an Send: [Address, 68, Accel(4 bytes), CRC(2 bytes)] Receive: [0xFF] * + * + * @param accel + * acc */ public void setDefaultDutyAccelM1(int accel) { sendPacket(address, 68, byte3(accel), byte2(accel), byte1(accel), byte0(accel)); @@ -916,6 +987,9 @@ Set the default acceleration for M2 when using duty cycle commands(Cmds 32,33 an Send: [Address, 69, Accel(4 bytes), CRC(2 bytes)] Receive: [0xFF] * + * + * @param accel + * acc */ public void setDefaultDutyAccelM2(int accel) { sendPacket(address, 69, byte3(accel), byte2(accel), byte1(accel), byte0(accel)); @@ -942,6 +1016,13 @@ public void setDefaultDutyAccelM2(int accel) { Voltage Clamp: Sets the signal pin as an output to drive an external voltage clamp circuit Home(M1 and M2): will trigger the specific motor to stop and the encoder count to reset to 0. * + * + * @param s3 + * a + * @param s4 + * a + * @param s5 + * a */ public void setModes(int s3, int s4, int s5) { sendPacket(address, 74, s3, s4, s5); @@ -1010,6 +1091,8 @@ public void readDefaultDutyAccelSettings() { Send: [Address, 82] Receive: [Temperature(2 bytes), CRC(2 bytes)] * + * + * @return temp */ public int readTemp() { sendPacket(address, 82); @@ -1094,6 +1177,9 @@ public void readEncoderModes() { Send: [Address, 92, Mode, CRC(2 bytes)] Receive: [0xFF] * + * + * @param mode + * mode */ public void setEncoderModeM1(int mode) { sendPacket(address, 92, mode); @@ -1110,6 +1196,9 @@ public void setEncoderModeM1(int mode) { Brushed DC Motor Controllers RoboClaw Series User Manual 75 * + * + * @param mode + * m */ public void setEncoderModeM2(int mode) { sendPacket(address, 93, mode); @@ -1192,6 +1281,9 @@ public void readSettingsFromEeprom() { Brushed DC Motor Controllers RoboClaw Series User Manual 77 * + * + * @param config + * config */ public void setStandardConfig(int config) { sendPacket(address, 98, byte1(config), byte0(config)); @@ -1261,6 +1353,9 @@ Set CTRL1 output value(available on select models) Receive: [0xFF] Set the output state value of CTRL1. See 100 - Set CTRL Modes for valid values. * + * + * @param value + * v */ public void setCtrl1(int value) { sendPacket(address, 102, byte1(value), byte0(value)); @@ -1274,6 +1369,9 @@ Set CTRL2 output value(available on select models) Receive: [0xFF] Set the output state value of CTRL2. See 100 - Set CTRL Modes for valid values. * + * + * @param value + * v */ public void setCtrl2(int value) { sendPacket(address, 103, byte1(value), byte0(value)); @@ -1301,6 +1399,9 @@ public void readCtrls() { Send: [Address, 134, MaxCurrent(4 bytes), 0, 0, 0, 0, CRC(2 bytes)] Receive: [0xFF] * + * + * @param maxCurrent + * current */ public void setMaxCurrentM1(int maxCurrent) { sendPacket(address, 133, byte3(maxCurrent), byte2(maxCurrent), byte1(maxCurrent), byte0(maxCurrent), 0, 0, 0, 0); @@ -1315,6 +1416,9 @@ public void setMaxCurrentM1(int maxCurrent) { Send: [Address, 134, MaxCurrent(4 bytes), 0, 0, 0, 0, CRC(2 bytes)] Receive: [0xFF] * + * + * @param maxCurrent + * current */ public void setMaxCurrentM2(int maxCurrent) { sendPacket(address, 134, byte3(maxCurrent), byte2(maxCurrent), byte1(maxCurrent), byte0(maxCurrent), 0, 0, 0, 0); @@ -1356,6 +1460,9 @@ Set PWM Drive mode. Locked Antiphase(0) or Sign Magnitude(1). Send: [Address, 148, Mode, CRC(2 bytes)] Receive: [0xFF] * + * + * @param mode + * pwm mode */ public void setPwmMode(int mode) { sendPacket(address, 148, mode); @@ -1368,6 +1475,8 @@ public void setPwmMode(int mode) { Send: [Address, 149] Receive: [PWMMode, CRC(2 bytes)] * + * + * @return pwm mode */ public Integer readPwmMode() { @@ -1399,6 +1508,8 @@ public Integer readPwmMode() { Bit6 - Reserved Bit7 - Reserved * + * + * @return encoder data from m1 */ public EncoderData readEncoderM1() { m1.encoder = null; @@ -1425,7 +1536,7 @@ public static int bytes2ToInt(byte[] data) { } public static int bytes2ToInt(byte[] data, int start) { - return data[start] << 8 | data[start + 1]; + return (data[start] & 0xFF) << 8 | (data[start + 1] & 0xFF); } /** @@ -1446,6 +1557,8 @@ public static int bytes2ToInt(byte[] data, int start) { Bit6 - Reserved Bit7 - Reserved * + * + * @return encoder data from m2 */ public EncoderData readEncoderM2() { @@ -1502,6 +1615,8 @@ synchronized public byte[] sendReadPacket(int bytesRequested, int... sendData) { Receive: [Speed(4 bytes), Status, CRC(2 bytes)] Status indicates the direction (0 – forward, 1 - backward). * + * + * @return encoder speed m1 */ public long readEncoderSpeedM1() { byte data[] = sendReadPacket(7, address, 18); @@ -1528,6 +1643,8 @@ public long readEncoderSpeedM1() { Receive: [Speed(4 bytes), Status, CRC(2 bytes)] Status indicates the direction (0 – forward, 1 - backward). * + * + * @return encoder speed m2 */ public long readEncoderSpeedM2() { byte data[] = sendReadPacket(7, address, 17); @@ -1568,6 +1685,9 @@ public void resetEncoders() { Send: [Address, 22, Value(4 bytes), CRC(2 bytes)] Receive: [0xFF] * + * + * @param value + * v */ public void setEncoderM1(int value) { sendPacket(address, 22, byte3(value), byte2(value), byte1(value), byte0(value)); @@ -1581,6 +1701,9 @@ public void setEncoderM1(int value) { Send: [Address, 23, Value(4 bytes), CRC(2 bytes)] Receive: [0xFF] * + * + * @param value + * v */ public void setEncoderM2(int value) { sendPacket(address, 23, byte3(value), byte2(value), byte1(value), byte0(value)); @@ -1597,6 +1720,8 @@ public void setEncoderM2(int value) { Receive: [Speed(4 bytes), Status, CRC(2 bytes)] The Status byte is direction (0 – forward, 1 - backward). * + * + * @return speed of m1 */ public Long readSpeedM1() { byte[] data = sendReadPacket(7, address, 30); @@ -1624,6 +1749,8 @@ public Long readSpeedM1() { Receive: [Speed(4 bytes), Status, CRC(2 bytes)] The Status byte is direction (0 – forward, 1 - backward). * + * + * @return speed m2 */ public Long readSpeedM2() { @@ -1688,6 +1815,15 @@ public void readISpeedCounters() { Send: [Address, 28, D(4 bytes), P(4 bytes), I(4 bytes), QPPS(4 byte), CRC(2 bytes)] Receive: [0xFF] * + * + * @param D + * pid + * @param P + * pid + * @param I + * pid + * @param QPPS + * see comments */ public void setPidQppsM1(int D, int P, int I, int QPPS) { sendPacket(address, 28, byte3(D), byte2(D), byte1(D), byte0(D), byte3(P), byte2(P), byte1(P), byte0(P), byte3(I), byte2(I), byte1(I), byte0(I), byte3(QPPS), byte2(QPPS), @@ -1711,6 +1847,15 @@ public void setPidQppsM1(int D, int P, int I, int QPPS) { Send: [Address, 29, D(4 bytes), P(4 bytes), I(4 bytes), QPPS(4 byte), CRC(2 bytes)] Receive: [0xFF] * + * + * @param D + * pid + * @param P + * pid + * @param I + * pid + * @param QPPS + * pid */ public void setPidQppsM2(int D, int P, int I, int QPPS) { sendPacket(address, 29, byte3(D), byte2(D), byte1(D), byte0(D), byte3(P), byte2(P), byte1(P), byte0(P), byte3(I), byte2(I), byte1(I), byte0(I), byte3(QPPS), byte2(QPPS), @@ -1736,6 +1881,9 @@ public void setPidQppsDeadzoneMinMaxM2(int D, int P, int I, int QPPS, int deadzo Receive: [0xFF] The duty value is signed and the range is -32767 to +32767 (eg. +-100% duty). * + * + * @param duty + * duty m1 */ public void driveDutyM1(int duty) { sendPacket(address, 32, byte1(duty), byte0(duty)); @@ -1750,6 +1898,9 @@ public void driveDutyM1(int duty) { Receive: [0xFF] The duty value is signed and the range is -32768 to +32767 (eg. +-100% duty). * + * + * @param duty + * m2 duty */ public void driveDutyM2(int duty) { sendPacket(address, 32, byte1(duty), byte0(duty)); @@ -1764,6 +1915,11 @@ public void driveDutyM2(int duty) { Receive: [0xFF] The duty value is signed and the range is -32768 to +32767 (eg. +-100% duty). * + * + * @param duty1 + * m1 duty + * @param duty2 + * m2 duty */ public void driveDutyM1M2(int duty1, int duty2) { sendPacket(address, 34, byte1(duty1), byte0(duty1), byte1(duty2), byte0(duty2)); @@ -1780,6 +1936,9 @@ public void driveDutyM1M2(int duty1, int duty2) { Send: [Address, 35, Speed(4 Bytes), CRC(2 bytes)] Receive: [0xFF] * + * + * @param speed + * s */ public void driveSpeedM1(int speed) { sendPacket(address, 35, byte3(speed), byte2(speed), byte1(speed), byte0(speed)); @@ -1796,6 +1955,9 @@ public void driveSpeedM1(int speed) { Send: [Address, 36, Speed(4 Bytes), CRC(2 bytes)] Receive: [0xFF] * + * + * @param speed + * s m2 */ public void driveSpeedM2(int speed) { sendPacket(address, 36, byte3(speed), byte2(speed), byte1(speed), byte0(speed)); @@ -1812,6 +1974,11 @@ public void driveSpeedM2(int speed) { Send: [Address, 37, SpeedM1(4 Bytes), SpeedM2(4 Bytes), CRC(2 bytes)] Receive: [0xFF] * + * + * @param speedM1 + * m1 + * @param speedM2 + * m2 */ public void driveSpeedM1M2(int speedM1, int speedM2) { sendPacket(address, 37, byte3(speedM1), byte2(speedM1), byte1(speedM1), byte0(speedM1), byte3(speedM2), byte2(speedM2), byte1(speedM2), byte0(speedM2)); @@ -1836,6 +2003,11 @@ public void driveSpeedM1M2(int speedM1, int speedM2) { Brushed DC Motor Controllers RoboClaw Series User Manual 92 * + * + * @param accel + * a + * @param speed + * s */ public void driveSpeedAccelM1(int accel, int speed) { sendPacket(address, 38, accel, speed); @@ -1858,6 +2030,11 @@ public void driveSpeedAccelM1(int accel, int speed) { Another example would be an acceleration value of 24,000 QPPS and a speed value of 12,000 QPPS would accelerate the motor to 12,000 QPPS in 0.5 seconds. * + * + * @param accel + * a + * @param speed + * s */ public void driveSpeedAccelM2(int accel, int speed) { sendPacket(address, 39, accel, speed); @@ -1881,6 +2058,13 @@ public void driveSpeedAccelM2(int accel, int speed) { Another example would be an acceleration value of 24,000 QPPS and a speed value of 12,000 QPPS would accelerate the motor to 12,000 QPPS in 0.5 seconds. * + * + * @param accel + * a + * @param speedM1 + * m1 + * @param speedM2 + * m2 */ public void driveSpeedAccelM1M2(int accel, int speedM1, int speedM2) { @@ -1904,6 +2088,11 @@ public void driveSpeedAccelM1M2(int accel, int speedM1, int speedM2) { and executed in the order sent. If a value of 1 is used the current running command is stopped, any other commands in the buffer are deleted and the new command is executed. * + * + * @param speed + * s + * @param distance + * d */ public void driveSpeedDistM1(int speed, int distance) { sendPacket(address, 41, byte3(speed), byte2(speed), byte1(speed), byte0(speed), byte3(distance), byte2(distance), byte1(distance), byte0(distance), buffer); @@ -1924,6 +2113,11 @@ public void driveSpeedDistM1(int speed, int distance) { and executed in the order sent. If a value of 1 is used the current running command is stopped, any other commands in the buffer are deleted and the new command is executed. * + * + * @param speed + * s + * @param distance + * d */ public void driveSpeedDistM2(int speed, int distance) { sendPacket(address, 42, byte3(speed), byte2(speed), byte1(speed), byte0(speed), byte3(distance), byte2(distance), byte1(distance), byte0(distance), buffer); @@ -1945,6 +2139,15 @@ public void driveSpeedDistM2(int speed, int distance) { and executed in the order sent. If a value of 1 is used the current running command is stopped, any other commands in the buffer are deleted and the new command is executed. * + * + * @param speedM1 + * s + * @param distanceM1 + * d + * @param speedM2 + * s + * @param distanceM2 + * d */ public void driveSpeedDistM1M2(int speedM1, int distanceM1, int speedM2, int distanceM2) { sendPacket(address, 43, byte3(speedM1), byte2(speedM1), byte1(speedM1), byte0(speedM1), byte3(distanceM1), byte2(distanceM1), byte1(distanceM1), byte0(distanceM1), @@ -1975,6 +2178,13 @@ Buffer, CRC(2 bytes)] Brushed DC Motor Controllers RoboClaw Series User Manual 94 * + * + * @param accel + * a + * @param speed + * s + * @param distance + * d */ public void driveSpeedAccelDistM1(int accel, int speed, int distance) { sendPacket(address, 44, byte3(accel), byte2(accel), byte1(accel), byte0(accel), byte3(speed), byte2(speed), byte1(speed), byte0(speed), byte3(distance), byte2(distance), @@ -1999,6 +2209,13 @@ Buffer, CRC(2 bytes)] and executed in the order sent. If a value of 1 is used the current running command is stopped, any other commands in the buffer are deleted and the new command is executed. * + * + * @param speed + * s + * @param accel + * a + * @param distance + * d */ public void driveSpeedAccelDistM2(int speed, int accel, int distance) { sendPacket(address, 45, byte3(speed), byte2(speed), byte1(speed), byte0(speed), byte3(accel), byte2(accel), byte1(accel), byte0(accel), byte3(distance), byte2(distance), @@ -2024,6 +2241,17 @@ public void driveSpeedAccelDistM2(int speed, int accel, int distance) { and executed in the order sent. If a value of 1 is used the current running command is stopped, any other commands in the buffer are deleted and the new command is executed. * + * + * @param accel + * a + * @param speedM1 + * s + * @param distanceM1 + * d + * @param speedM2 + * s + * @param distanceM2 + * d */ public void driveSpeedAccelDistM1M2(int accel, int speedM1, int distanceM1, int speedM2, int distanceM2) { sendPacket(address, 46, byte3(accel), byte2(accel), byte1(accel), byte0(accel), byte3(speedM1), byte2(speedM1), byte1(speedM1), byte0(speedM1), byte3(distanceM1), @@ -2068,6 +2296,15 @@ public void readBufferLength() { Another example would be an acceleration value of 24,000 QPPS and a speed value of 12,000 QPPS would accelerate the motor to 12,000 QPPS in 0.5 seconds. * + * + * @param accelM1 + * a + * @param speedM1 + * s + * @param accelM2 + * a + * @param speedM2 + * s */ public void driveSpeedAndIndividualAccelM1M2(int accelM1, int speedM1, int accelM2, int speedM2) { sendPacket(address, 50, byte3(accelM1), byte2(accelM1), byte1(accelM1), byte0(accelM1), byte3(speedM1), byte2(speedM1), byte1(speedM1), byte0(speedM1), byte3(accelM2), @@ -2093,6 +2330,19 @@ public void driveSpeedAndIndividualAccelM1M2(int accelM1, int speedM1, int accel and executed in the order sent. If a value of 1 is used the current running command is stopped, any other commands in the buffer are deleted and the new command is executed. * + * + * @param accelM1 + * a + * @param speedM1 + * s + * @param distanceM1 + * d + * @param accelM2 + * a + * @param speedM2 + * s + * @param distanceM2 + * d */ public void driveSpeedAccelDistM1M2(int accelM1, int speedM1, int distanceM1, int accelM2, int speedM2, int distanceM2) { sendPacket(address, 51, byte3(accelM1), byte2(accelM1), byte1(accelM1), byte0(accelM1), byte3(speedM1), byte2(speedM1), byte1(speedM1), byte0(speedM1), byte3(distanceM1), @@ -2112,6 +2362,11 @@ public void driveSpeedAccelDistM1M2(int accelM1, int speedM1, int distanceM1, in The duty value is signed and the range is -32768 to +32767(eg. +-100% duty). The accel value range is 0 to 655359(eg maximum acceleration rate is -100% to 100% in 100ms). * + * + * @param duty + * d + * @param accel + * a */ public void driveDutyAccelM1(int duty, int accel) { sendPacket(address, 52, byte1(duty), byte0(duty), byte1(accel), byte0(accel)); @@ -2129,6 +2384,11 @@ public void driveDutyAccelM1(int duty, int accel) { The duty value is signed and the range is -32768 to +32767 (eg. +-100% duty). The accel value range is 0 to 655359 (eg maximum acceleration rate is -100% to 100% in 100ms). * + * + * @param duty + * d + * @param accel + * a */ public void driveDutyAccelM2(int duty, int accel) { sendPacket(address, 53, byte1(duty), byte0(duty), byte1(accel), byte0(accel)); @@ -2147,6 +2407,17 @@ public void driveDutyAccelM2(int duty, int accel) { The duty value is signed and the range is -32768 to +32767 (eg. +-100% duty). The accel value range is 0 to 655359 (eg maximum acceleration rate is -100% to 100% in 100ms). * + * + * @param cmd + * c + * @param dutyM1 + * d + * @param accelM1 + * a + * @param dutyM2 + * d + * @param accelM2 + * a */ public void driveDutyAccelM1M2(int cmd, int dutyM1, int accelM1, int dutyM2, int accelM2) { sendPacket(address, 54, cmd, byte1(dutyM1), byte0(dutyM1), byte1(accelM1), byte0(accelM1), byte1(dutyM2), byte0(dutyM2), byte1(accelM2), byte0(accelM2)); @@ -2159,6 +2430,8 @@ public void driveDutyAccelM1M2(int cmd, int dutyM1, int accelM1, int dutyM2, int Send: [Address, 55] Receive: [P(4 bytes), I(4 bytes), D(4 bytes), QPPS(4 byte), CRC(2 bytes)] * + * + * @return the pid data */ // FIXME - contain both inside a pair<> public PidData readPidQppsM1() { @@ -2181,6 +2454,8 @@ public PidData readPidQppsM1() { Send: [Address, 56] Receive: [P(4 bytes), I(4 bytes), D(4 bytes), QPPS(4 byte), CRC(2 bytes)] * + * + * @return pid data */ public PidData readPidQppsM2() { sendPacket(address, 56); @@ -2209,6 +2484,21 @@ public PidData readPidQppsM2() { Position constants are used only with the Position commands, 65,66 and 67 or when encoders are enabled in RC/Analog modes. * + * + * @param D + * pid + * @param P + * pid + * @param I + * pid + * @param maxI + * i + * @param deadzone + * dz + * @param minPos + * min + * @param maxPos + * max */ public void setPidM1(int D, int P, int I, int maxI, int deadzone, int minPos, int maxPos) { sendPacket(address, 61, byte3(D), byte2(D), byte1(D), byte0(D), byte3(P), byte2(P), byte1(P), byte0(P), byte3(I), byte2(I), byte1(I), byte0(I), byte3(maxI), byte2(maxI), @@ -2230,6 +2520,21 @@ public void setPidM1(int D, int P, int I, int maxI, int deadzone, int minPos, in Position constants are used only with the Position commands, 65,66 and 67 or when encoders are enabled in RC/Analog modes. * + * + * @param D + * pid + * @param P + * pid + * @param I + * pid + * @param maxI + * max i + * @param deadzone + * dz + * @param minPos + * min + * @param maxPos + * max */ public void setPidM2(int D, int P, int I, int maxI, int deadzone, int minPos, int maxPos) { sendPacket(address, 62, byte3(D), byte2(D), byte1(D), byte0(D), byte3(P), byte2(P), byte1(P), byte0(P), byte3(I), byte2(I), byte1(I), byte0(I), byte3(maxI), byte2(maxI), @@ -2246,6 +2551,8 @@ public void setPidM2(int D, int P, int I, int maxI, int deadzone, int minPos, in Receive: [P(4 bytes), I(4 bytes), D(4 bytes), MaxI(4 byte), Deadzone(4 byte), MinPos(4 byte), MaxPos(4 byte), CRC(2 bytes)] * + * + * @return pid data */ public PidData readPidM1() { @@ -2257,8 +2564,8 @@ public PidData readPidM1() { m1.pid.kd = bytes4ToLong(data, 8); // MaxI ??? m1.pid.kd = bytes4ToLong(data, 12); m1.pid.deadband = bytes4ToLong(data, 16); - m1.pid.outMin = bytes4ToLong(data, 20); - m1.pid.outMax = bytes4ToLong(data, 24); + m1.pid.outMin = (double) bytes4ToLong(data, 20); + m1.pid.outMax = (double) bytes4ToLong(data, 24); log.info("m1.pid {}", m1.pid.toString()); } @@ -2273,6 +2580,8 @@ public PidData readPidM1() { Receive: [P(4 bytes), I(4 bytes), D(4 bytes), MaxI(4 byte), Deadzone(4 byte), MinPos(4 byte), MaxPos(4 byte), CRC(2 bytes)] * + * + * @return pid data */ public PidData readPidM2() { byte[] data = sendReadPacket(30, address, 64); @@ -2283,8 +2592,8 @@ public PidData readPidM2() { m2.pid.kd = bytes4ToLong(data, 8); // MaxI ??? m1.pid.kd = bytes4ToLong(data, 12); m2.pid.deadband = bytes4ToLong(data, 16); - m2.pid.outMin = bytes4ToLong(data, 20); - m2.pid.outMax = bytes4ToLong(data, 24); + m2.pid.outMin = (double) bytes4ToLong(data, 20); + m2.pid.outMax = (double) bytes4ToLong(data, 24); log.info("m2.pid {}", m2.pid.toString()); } @@ -2301,6 +2610,15 @@ public PidData readPidM2() { Position(4 Bytes), Buffer, CRC(2 bytes)] Receive: [0xFF] * + * + * @param accel + * a + * @param speed + * s + * @param deccel + * d + * @param pos + * p */ public void driveSpeedAccelDeccelPosM1(int accel, int speed, int deccel, int pos) { sendPacket(address, 65, byte3(accel), byte2(accel), byte1(accel), byte0(accel), byte3(speed), byte2(speed), byte1(speed), byte0(speed), byte3(deccel), byte2(deccel), @@ -2318,6 +2636,15 @@ public void driveSpeedAccelDeccelPosM1(int accel, int speed, int deccel, int pos Position(4 Bytes), Buffer, CRC(2 bytes)] Receive: [0xFF] * + * + * @param accel + * a + * @param speed + * s + * @param deccel + * d + * @param pos + * p */ public void driveSpeedAccelDeccelPosM2(int accel, int speed, int deccel, int pos) { sendPacket(address, 66, byte3(accel), byte2(accel), byte1(accel), byte0(accel), byte3(speed), byte2(speed), byte1(speed), byte0(speed), byte3(deccel), byte2(deccel), @@ -2335,6 +2662,23 @@ public void driveSpeedAccelDeccelPosM2(int accel, int speed, int deccel, int pos PositionM2(4 Bytes), Buffer, CRC(2 bytes)] Receive: [0xFF] * + * + * @param accelM1 + * a + * @param speedM1 + * s + * @param deccelM1 + * d + * @param posM1 + * p + * @param accelM2 + * a + * @param speedM2 + * s + * @param deccelM2 + * d + * @param posM2 + * p */ public void driveSpeedAccelDeccelPosM1M2(int accelM1, int speedM1, int deccelM1, int posM1, int accelM2, int speedM2, int deccelM2, int posM2) { @@ -2364,11 +2708,6 @@ public void onBytes(byte[] bytes) { } } - public void attachEncoderListener(EncoderListener enc) { - // subscribe the onEncoderData of the listener. - addListener("publishEncoderData", enc.getName(), "onEncoderData"); - } - @Override public EncoderData publishEncoderData(EncoderData data) { return data; @@ -2623,4 +2962,4 @@ public static void main(String[] args) { } -} +} \ No newline at end of file From 894da040ded05a3c7d8bb08571a83b13dc12c3de Mon Sep 17 00:00:00 2001 From: kwatters Date: Sun, 1 Jun 2025 11:55:13 -0400 Subject: [PATCH 07/10] reverting more changes --- .../service/abstracts/AbstractPinEncoder.java | 38 +++++++++---------- 1 file changed, 18 insertions(+), 20 deletions(-) diff --git a/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java b/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java index 2983098291..d9d1bd821e 100644 --- a/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java +++ b/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java @@ -2,11 +2,11 @@ import org.myrobotlab.framework.Service; import org.myrobotlab.sensor.EncoderData; -import org.myrobotlab.sensor.EncoderListener; +import org.myrobotlab.service.config.ServiceConfig; import org.myrobotlab.service.interfaces.EncoderControl; import org.myrobotlab.service.interfaces.EncoderController; -public class AbstractPinEncoder extends Service implements EncoderControl { +public abstract class AbstractPinEncoder extends Service implements EncoderControl { private static final long serialVersionUID = 1L; public String pin; @@ -24,12 +24,13 @@ public AbstractPinEncoder(String n, String id) { super(n, id); } - public void attach(EncoderController controller) throws Exception { + @Override + public void attachEncoderController(EncoderController controller) { if (this.controller == controller) { log.info("{} already attached to controller {}", getName(), controller.getName()); } this.controller = controller; - controller.attach(this); + controller.attachEncoderControl(this); lastUpdate = System.currentTimeMillis(); } @@ -42,28 +43,31 @@ public Double publishEncoderAngle(Double angle) { return angle; } - // This is used to relay the data being broadcast from a controller (such as - // an arduino) + // FIXME - remove this ... public void onEncoderData(EncoderData data) { - // TODO: maybe the raw pin data from the arduino comes in here instead.. - // current timestamp / delta since last update. + // this is getting published from the arduino and updated here when it comes + // in.. + // TODO: shoudl the messaging be setup differently? + // TODO: compare with ultrasonic sensor and see that we're following the + // same pattern + // TODO: maybe use nanoTime? how accurate is this. long now = System.currentTimeMillis(); long delta = now - lastUpdate; + Double angle = 360.0 * data.angle / resolution; + log.info("Angle : {}", angle); if (delta > 0) { // we can compute velocity since the last update // This computes the change in degrees per second that the encoder is // currently moving at. - velocity = (data.angle - this.lastPosition) / delta * 1000.0; + velocity = (angle - this.lastPosition) / delta * 1000.0; } else { // no position update since the last tick. velocity = 0.0; } // update the previous values - this.lastPosition = data.angle; + this.lastPosition = angle; this.lastUpdate = now; - // log.info("Encoder Data : {} Angle : {}", data, lastPosition); - // now that we've updated our state.. we can publish along the data. - broadcast("publishEncoderData", data); + log.info("Encoder Data : {} Angle : {}", data, lastPosition); } public void setZeroPoint() { @@ -103,10 +107,4 @@ public EncoderData publishEncoderData(EncoderData data) { public Double getPos() { return lastPosition; } - - @Override - public void attachEncoderListener(EncoderListener listener) { - // TODO: should this be full name? - addListener("publishEncoderData", listener.getName()); - } -} +} \ No newline at end of file From d5716c096989ab002400c362906bb3f0c1dfeb78 Mon Sep 17 00:00:00 2001 From: kwatters Date: Sun, 1 Jun 2025 11:56:10 -0400 Subject: [PATCH 08/10] foo.. --- src/main/java/org/myrobotlab/service/RoboClaw.java | 2 +- .../org/myrobotlab/service/abstracts/AbstractPinEncoder.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/myrobotlab/service/RoboClaw.java b/src/main/java/org/myrobotlab/service/RoboClaw.java index 2d3cfe705d..f9d8e3f745 100644 --- a/src/main/java/org/myrobotlab/service/RoboClaw.java +++ b/src/main/java/org/myrobotlab/service/RoboClaw.java @@ -2962,4 +2962,4 @@ public static void main(String[] args) { } -} \ No newline at end of file +} diff --git a/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java b/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java index d9d1bd821e..6aa5720aeb 100644 --- a/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java +++ b/src/main/java/org/myrobotlab/service/abstracts/AbstractPinEncoder.java @@ -107,4 +107,4 @@ public EncoderData publishEncoderData(EncoderData data) { public Double getPos() { return lastPosition; } -} \ No newline at end of file +} From 77b520288974e0aa855df2a3568b1e2f0df513c7 Mon Sep 17 00:00:00 2001 From: kwatters Date: Sun, 1 Jun 2025 11:57:21 -0400 Subject: [PATCH 09/10] whitespace --- src/main/java/org/myrobotlab/service/Arduino.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/org/myrobotlab/service/Arduino.java b/src/main/java/org/myrobotlab/service/Arduino.java index a422b0b683..8385273fe6 100644 --- a/src/main/java/org/myrobotlab/service/Arduino.java +++ b/src/main/java/org/myrobotlab/service/Arduino.java @@ -2371,4 +2371,3 @@ public static void main(String[] args) { } } - From bd426bd1f8780338abacd2eb989ee3864c2d4896 Mon Sep 17 00:00:00 2001 From: kwatters Date: Sun, 1 Jun 2025 12:02:54 -0400 Subject: [PATCH 10/10] revert --- .../service/interfaces/EncoderControl.java | 21 +++++++------------ 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/src/main/java/org/myrobotlab/service/interfaces/EncoderControl.java b/src/main/java/org/myrobotlab/service/interfaces/EncoderControl.java index 264234f8bf..895fc7f7f3 100755 --- a/src/main/java/org/myrobotlab/service/interfaces/EncoderControl.java +++ b/src/main/java/org/myrobotlab/service/interfaces/EncoderControl.java @@ -2,7 +2,6 @@ import org.myrobotlab.framework.interfaces.Attachable; import org.myrobotlab.sensor.EncoderData; -import org.myrobotlab.sensor.EncoderListener; public interface EncoderControl extends Attachable { @@ -20,29 +19,25 @@ public interface EncoderControl extends Attachable { * publishes the EncoderData from the encoder * * @param data - * @return data + * the data to publish + * @return encoder data */ public EncoderData publishEncoderData(EncoderData data); /** - * attaches an encoder listener to get the publishEncoderData - * to invoke the onEncoderData method of the listener. + * Attach a controller to an encoder control. * - * @param listener + * @param controller */ - public void attachEncoderListener(EncoderListener listener); - + public void attachEncoderController(EncoderController controller); + /** - * return the state of streaming encoder data - * - * @return + * @return the state of streaming encoder data */ public Boolean isEnabled(); /** - * the position of the encoder in degrees or cm for linear encoder ? - * - * @return + * @return the position of the encoder in degrees or cm for linear encoder ? */ public Double getPos();