From 0be25ff8b7cfe26e5952000c1b27788eb953ab49 Mon Sep 17 00:00:00 2001 From: Christian Date: Sun, 22 Jan 2017 13:46:42 -0500 Subject: [PATCH 1/8] moving dhlink in jm3 --- .../control/IntegratedMovementGUI.java | 4 + .../myrobotlab/kinematics/CollisionItem.java | 10 ++ src/org/myrobotlab/kinematics/Map3D.java | 2 +- .../kinematics/TestJmeIntegratedMovement.java | 94 ++++++++++++++++--- .../service/IntegratedMovement.java | 61 ++---------- 5 files changed, 106 insertions(+), 65 deletions(-) diff --git a/src/org/myrobotlab/control/IntegratedMovementGUI.java b/src/org/myrobotlab/control/IntegratedMovementGUI.java index a6ad769263..c92d4ae0ef 100644 --- a/src/org/myrobotlab/control/IntegratedMovementGUI.java +++ b/src/org/myrobotlab/control/IntegratedMovementGUI.java @@ -128,6 +128,10 @@ public void getState(IntegratedMovement im) { moveLocation[i].setVisible(true); move[i].setVisible(true); } + else { + moveLocation[i].setVisible(false); + move[i].setVisible(false); + } i++; } } diff --git a/src/org/myrobotlab/kinematics/CollisionItem.java b/src/org/myrobotlab/kinematics/CollisionItem.java index 2853faf30d..bf90439510 100644 --- a/src/org/myrobotlab/kinematics/CollisionItem.java +++ b/src/org/myrobotlab/kinematics/CollisionItem.java @@ -9,6 +9,12 @@ import org.myrobotlab.openni.PVector; import org.python.jline.internal.Log; +import com.jme3.math.Vector2f; +import com.jme3.math.Vector3f; +import com.jme3.scene.Mesh; +import com.jme3.scene.VertexBuffer.Type; +import com.jme3.util.BufferUtils; + public class CollisionItem { Point origin = null; Point end = null; @@ -17,6 +23,8 @@ public class CollisionItem { ArrayList ignore = new ArrayList(); ArrayList done = new ArrayList(); boolean fromKinect = false; + HashMap cloudMap; + /** * @param origin * @param end @@ -41,6 +49,7 @@ public CollisionItem(Point origin, Point end, String name) { public CollisionItem(HashMap cloudMap) { name = UUID.randomUUID().toString(); + this.cloudMap = cloudMap; buildCollisionItem(cloudMap); } @@ -258,4 +267,5 @@ public boolean isFromKinect() { return fromKinect; } + } \ No newline at end of file diff --git a/src/org/myrobotlab/kinematics/Map3D.java b/src/org/myrobotlab/kinematics/Map3D.java index 1b0fd00f88..7346299d51 100644 --- a/src/org/myrobotlab/kinematics/Map3D.java +++ b/src/org/myrobotlab/kinematics/Map3D.java @@ -35,7 +35,7 @@ public enum CoordStateValue { public int closestDistance = 450; public int fartestDistance = 1000; - public int skip = 10; + public int skip = 6; HashMap>> coordValue = new HashMap>>(); private Point kinectPosition; //ArrayList cloudMap = new ArrayList(); diff --git a/src/org/myrobotlab/kinematics/TestJmeIntegratedMovement.java b/src/org/myrobotlab/kinematics/TestJmeIntegratedMovement.java index caff607694..2b997e35a2 100644 --- a/src/org/myrobotlab/kinematics/TestJmeIntegratedMovement.java +++ b/src/org/myrobotlab/kinematics/TestJmeIntegratedMovement.java @@ -10,9 +10,9 @@ import com.jme3.math.ColorRGBA; import com.jme3.math.FastMath; import com.jme3.math.Vector3f; -import com.jme3.renderer.Camera; import com.jme3.scene.Geometry; import com.jme3.scene.Node; +import com.jme3.scene.shape.Box; import com.jme3.scene.shape.Cylinder; /** @@ -21,6 +21,11 @@ */ public class TestJmeIntegratedMovement extends SimpleApplication { private HashMap objects; + private Vector3f camLocation; + private int camXDir = 1; + private int camZDir = -1; + private HashMap shapes = new HashMap(); + private boolean updateShape = false; @Override public void simpleInitApp() { @@ -40,22 +45,46 @@ public void simpleInitApp() { // // } for (CollisionItem ci : objects.values()) { - Cylinder c= new Cylinder(8,50,(float)ci.getRadius()/2,(float)ci.getLength()/2,true,false); - Geometry geom = new Geometry("Cylinder",c); Vector3f ori = new Vector3f((float)ci.getOrigin().getX()/2, (float)ci.getOrigin().getZ()/2, (float)ci.getOrigin().getY()/2); Vector3f end = new Vector3f((float)ci.getEnd().getX()/2, (float)ci.getEnd().getZ()/2, (float)ci.getEnd().getY()/2); - geom.setLocalTranslation(FastMath.interpolateLinear(0.5f, ori, end)); - geom.lookAt(end, Vector3f.UNIT_Y); - //geom.scale(0.5f); - Material mat = new Material(assetManager,"Common/MatDefs/Misc/Unshaded.j3md"); - mat.setColor("Color", ColorRGBA.Blue); - geom.setMaterial(mat); - Node pivot = new Node("pivot"); - rootNode.attachChild(pivot); - pivot.attachChild(geom); + if (!ci.fromKinect) { + Cylinder c= new Cylinder(8,50,(float)ci.getRadius()/2,(float)ci.getLength()/2,true,false); + Geometry geom = new Geometry("Cylinder",c); + shapes.put(ci.name, geom); + geom.setLocalTranslation(FastMath.interpolateLinear(0.5f, ori, end)); + geom.lookAt(end, Vector3f.UNIT_Y); + //geom.scale(0.5f); + Material mat = new Material(assetManager,"Common/MatDefs/Misc/Unshaded.j3md"); + if (ci.fromKinect) { + mat.setColor("Color", ColorRGBA.Red); + } + else { + mat.setColor("Color", ColorRGBA.Blue); + } + geom.setMaterial(mat); + Node pivot = new Node("pivot"); + rootNode.attachChild(pivot); + pivot.attachChild(geom); // pivot.rotate(0.4f, 0.4f, 0.0f); + } + else { + Node item = new Node("item"); + for(Map3DPoint p : ci.cloudMap.values()) { + Box b = new Box(1.5f, 1.5f, 1.5f); + Geometry geo = new Geometry("Box",b); + Vector3f pos = new Vector3f((float)p.point.getX()/2, (float)p.point.getZ()/2, (float)p.point.getY()/2); + geo.setLocalTranslation(pos); + Material mat = new Material(assetManager,"Common/MatDefs/Misc/Unshaded.j3md"); + mat.setColor("Color", ColorRGBA.Red); + geo.setMaterial(mat); + item.attachChild(geo); + + } + rootNode.attachChild(item); + } } this.cam.setLocation(new Vector3f(0f,0f,1000f)); + //this.cam.lookAtDirection(new Vector3f(0,0,0), new Vector3f(0,0,0)); } public static void main(String[] args) { @@ -67,5 +96,46 @@ public void setObjects(HashMap collisionObject) { objects = collisionObject; } + + @Override + public void simpleUpdate(float tpf) { + Vector3f camLoc = cam.getLocation(); + if (camLoc.x >= 1000){ + camXDir = -1; + } + if (camLoc.x <= -1000) { + camXDir = 1; + } + if (camLoc.z >= 1000){ + camZDir = -1; + } + if (camLoc.z <= -1000) { + camZDir = 1; + } + //camLoc.add(2*tpf*camXDir, 0, 2*tpf*camZDir); + camLoc.x += 50*tpf*camXDir; + camLoc.z += 50*tpf*camZDir; + //this.cam.setLocation(camLoc); + //cam.lookAtDirection(new Vector3f(camLoc.x*-1,0,camLoc.z*-1), cam.getUp()); + if (updateShape ) { + for (CollisionItem ci : objects.values()) { + if (!ci.isFromKinect()) { + Vector3f ori = new Vector3f((float)ci.getOrigin().getX()/2, (float)ci.getOrigin().getZ()/2, (float)ci.getOrigin().getY()/2); + Vector3f end = new Vector3f((float)ci.getEnd().getX()/2, (float)ci.getEnd().getZ()/2, (float)ci.getEnd().getY()/2); + Geometry geom = shapes.get(ci.name); + geom.setLocalTranslation(FastMath.interpolateLinear(0.5f, ori, end)); + geom.lookAt(end, Vector3f.UNIT_Y); + //geom.scale(0.5f); + shapes.put(ci.name, geom); + } + } + updateShape = false; + } + } + + public void updateObjects(HashMap items) { + objects = items; + updateShape = true; + } } diff --git a/src/org/myrobotlab/service/IntegratedMovement.java b/src/org/myrobotlab/service/IntegratedMovement.java index 0ab518eaa7..ce5fdbf818 100644 --- a/src/org/myrobotlab/service/IntegratedMovement.java +++ b/src/org/myrobotlab/service/IntegratedMovement.java @@ -102,6 +102,8 @@ private class MoveInfo { public Map3D map3d = new Map3D(); private String kinectName = "kinect"; private boolean ProcessKinectData = false; + + TestJmeIntegratedMovement jmeApp = null; public IntegratedMovement(String n) { super(n); @@ -279,57 +281,9 @@ else if (computeMethod == IK_COMPUTE_METHOD_GENETIC_ALGORYTHM) { newPos.setY(newPos.getY() + vector[1]); newPos.setZ(newPos.getZ() + vector[2]); } -// double oc = Math.sqrt(((ori.getX() - colPoint.getX()) * (ori.getX() - colPoint.getX())) + ((ori.getY() - colPoint.getY()) * (ori.getY() - colPoint.getY())) + ((ori.getZ() - colPoint.getZ()) * (ori.getZ() - colPoint.getZ()))); -// double ec = Math.sqrt(((end.getX() - colPoint.getX()) * (end.getX() - colPoint.getX())) + ((end.getY() - colPoint.getY()) * (end.getY() - colPoint.getY())) + ((end.getZ() - colPoint.getZ()) * (end.getZ() - colPoint.getZ()))); -// if (oc > ec) { -// newPos.setX(newPos.getX()+colPoint.getX()-end.getX()); -// newPos.setY(newPos.getX()+colPoint.getY()-end.getY()); -// newPos.setZ(newPos.getZ()+colPoint.getZ()-end.getZ()); -// } -// else { -// newPos.setX(newPos.getX()+colPoint.getX()-ori.getX()); -// newPos.setY(newPos.getX()+colPoint.getY()-ori.getY()); -// newPos.setZ(newPos.getZ()+colPoint.getZ()-ori.getZ()); -// } Point oldGoTo = goTo; if(!stopMoving) moveTo(newPos); goTo = oldGoTo; -// int dmove = 0; -// int deltaMove = 5; -// if (collisionItems.getCollisionPoint()[itemIndex].getX() >= collisionItems.getCollisionPoint()[1-itemIndex].getX()) { -// dmove+=deltaMove; -// } -// else dmove-=deltaMove; -// if (collisionItems.getCollisionPoint()[itemIndex].getY() >= collisionItems.getCollisionPoint()[1-itemIndex].getY()) { -// dmove+=deltaMove; -// } -// else dmove-=deltaMove; -// if (collisionItems.getCollisionPoint()[itemIndex].getZ() >= collisionItems.getCollisionPoint()[1-itemIndex].getZ()) { -// dmove+=deltaMove; -// } -// else dmove-=deltaMove; -// ArrayList tempPos = new ArrayList(); -// for (DHLink l : currentArm.getLinks()) { -// Point actPoint = currentArm.getJointPosition(linkIndex); -// Double distAct = actPoint.distanceTo(collisionItems.getCollisionPoint()[1-itemIndex]); -// l.incrRotate(MathUtils.degToRad(dmove)); -// Point newPoint = currentArm.getJointPosition(linkIndex); -// Double distNew = newPoint.distanceTo(collisionItems.getCollisionPoint()[1-itemIndex]); -// if (distAct < distNew) { -// l.incrRotate(MathUtils.degToRad(dmove * -2)); -// } -// tempPos.add(l.getPositionValueDeg()); -// } -// currentArm = simulateMove(tempPos); -// for (int k = 0; k < currentArm.getNumLinks(); k++){ -// Servo servo = currentServos.get(currentArm.getLink(k).getName()); -// while (timeToWait + servo.lastActivityTime > System.currentTimeMillis()) { -// sleep(1); -// } -// servo.moveTo(currentArm.getLink(k).getPositionValueDeg().intValue()); -// } -// log.info("moving to {}", currentPosition()); -// timeToWait = (long) (time*1000); if (moveInfo != null) { goTo = moveToObject(); } @@ -534,7 +488,7 @@ public static void main(String[] args) throws Exception { ik.setGeneticComputeSimulation(false); //#move to a position - ik.moveTo("leftArm",350,400,700); + ik.moveTo("leftArm",260,410,-120); //ik.moveTo(280,190,-345); //#ik.moveTo("cymbal",ik.ObjectPointLocation.ORIGIN_SIDE, 0,0,5) //#mtorso.moveTo(45) @@ -835,6 +789,9 @@ public void onIKServoEvent(IKData data) { if (openni != null) { map3d.updateKinectPosition(currentPosition(kinectName)); } + if (jmeApp != null) { + jmeApp.updateObjects(collisionItems.getItems()); + } } public void moveTo(String name, ObjectPointLocation location, int xoffset, int yoffset, int zoffset) { @@ -976,9 +933,9 @@ public Collection getArms() { } public void visualize() { - TestJmeIntegratedMovement app = new TestJmeIntegratedMovement(); - app.setObjects(getCollisionObject()); - app.start(); + jmeApp = new TestJmeIntegratedMovement(); + jmeApp.setObjects(getCollisionObject()); + jmeApp.start(); } } From 7cedcf1b8c8f04838ad87aabbfc4124a87fd65b6 Mon Sep 17 00:00:00 2001 From: Christian Date: Fri, 27 Jan 2017 18:21:33 -0500 Subject: [PATCH 2/8] integratedMovement --- src/org/myrobotlab/service/IntegratedMovement.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/org/myrobotlab/service/IntegratedMovement.java b/src/org/myrobotlab/service/IntegratedMovement.java index ce5fdbf818..2db980a14a 100644 --- a/src/org/myrobotlab/service/IntegratedMovement.java +++ b/src/org/myrobotlab/service/IntegratedMovement.java @@ -281,6 +281,7 @@ else if (computeMethod == IK_COMPUTE_METHOD_GENETIC_ALGORYTHM) { newPos.setY(newPos.getY() + vector[1]); newPos.setZ(newPos.getZ() + vector[2]); } + this.outbox.notify(); Point oldGoTo = goTo; if(!stopMoving) moveTo(newPos); goTo = oldGoTo; From 0ac7d52c13d73453a664fa3269b3642a6cbf8660 Mon Sep 17 00:00:00 2001 From: Christian Date: Sun, 29 Jan 2017 00:07:39 -0500 Subject: [PATCH 3/8] some clean out in mrlcomm --- .../arduino/ArduinoMsgGenerator.java | 2 +- src/org/myrobotlab/arduino/Msg.java | 240 ++++++++---------- src/org/myrobotlab/arduino/VirtualMsg.java | 161 ++++++------ .../myrobotlab/arduino/virtual/MrlComm.java | 12 - .../myrobotlab/arduino/virtual/MrlServo.java | 9 +- .../service/Adafruit16CServoDriver.java | 1 - src/org/myrobotlab/service/Arduino.java | 16 +- src/org/myrobotlab/service/Servo.java | 123 ++++----- .../service/interfaces/ServoController.java | 2 - .../Arduino/MRLComm/ArduinoMsgCodec.h | 78 +++--- src/resource/Arduino/MRLComm/Device.h | 3 +- src/resource/Arduino/MRLComm/MrlComm.cpp | 31 +-- src/resource/Arduino/MRLComm/MrlComm.h | 9 +- src/resource/Arduino/MRLComm/MrlI2cBus.cpp | 2 +- src/resource/Arduino/MRLComm/MrlNeopixel.cpp | 25 +- src/resource/Arduino/MRLComm/MrlNeopixel.h | 5 +- src/resource/Arduino/MRLComm/MrlServo.cpp | 32 ++- src/resource/Arduino/MRLComm/MrlServo.h | 4 +- src/resource/Arduino/MRLComm/Msg.cpp | 42 ++- src/resource/Arduino/MRLComm/Msg.h | 1 + .../Arduino/generate/arduinoMsgs.schema | 6 +- 21 files changed, 378 insertions(+), 426 deletions(-) diff --git a/src/org/myrobotlab/arduino/ArduinoMsgGenerator.java b/src/org/myrobotlab/arduino/ArduinoMsgGenerator.java index 775319bf80..5cb57eb3d4 100644 --- a/src/org/myrobotlab/arduino/ArduinoMsgGenerator.java +++ b/src/org/myrobotlab/arduino/ArduinoMsgGenerator.java @@ -25,7 +25,7 @@ public class ArduinoMsgGenerator { public transient final static Logger log = LoggerFactory.getLogger(ArduinoMsgGenerator.class); - static final Integer MRLCOMM_VERSION = 53; + static final Integer MRLCOMM_VERSION = 54; public void generateDefinitions() throws IOException { generateDefinitions(new File("src/resource/Arduino/generate/arduinoMsgs.schema")); diff --git a/src/org/myrobotlab/arduino/Msg.java b/src/org/myrobotlab/arduino/Msg.java index 5e2f2a0a47..2d66d347c2 100644 --- a/src/org/myrobotlab/arduino/Msg.java +++ b/src/org/myrobotlab/arduino/Msg.java @@ -63,7 +63,7 @@ public class Msg { public static final int MAX_MSG_SIZE = 64; public static final int MAGIC_NUMBER = 170; // 10101010 - public static final int MRLCOMM_VERSION = 53; + public static final int MRLCOMM_VERSION = 54; // send buffer int sendBufferSize = 0; @@ -127,80 +127,78 @@ public static class AckLock { public final static int ECHO = 10; // < publishEcho/f32 myFloat/myByte/f32 secondFloat public final static int PUBLISH_ECHO = 11; - // > controllerAttach/serialPort - public final static int CONTROLLER_ATTACH = 12; // > customMsg/[] msg - public final static int CUSTOM_MSG = 13; + public final static int CUSTOM_MSG = 12; // < publishCustomMsg/[] msg - public final static int PUBLISH_CUSTOM_MSG = 14; + public final static int PUBLISH_CUSTOM_MSG = 13; // > deviceDetach/deviceId - public final static int DEVICE_DETACH = 15; + public final static int DEVICE_DETACH = 14; // > i2cBusAttach/deviceId/i2cBus - public final static int I2C_BUS_ATTACH = 16; + public final static int I2C_BUS_ATTACH = 15; // > i2cRead/deviceId/deviceAddress/size - public final static int I2C_READ = 17; + public final static int I2C_READ = 16; // > i2cWrite/deviceId/deviceAddress/[] data - public final static int I2C_WRITE = 18; + public final static int I2C_WRITE = 17; // > i2cWriteRead/deviceId/deviceAddress/readSize/writeValue - public final static int I2C_WRITE_READ = 19; + public final static int I2C_WRITE_READ = 18; // < publishI2cData/deviceId/[] data - public final static int PUBLISH_I2C_DATA = 20; + public final static int PUBLISH_I2C_DATA = 19; // > neoPixelAttach/deviceId/pin/b32 numPixels - public final static int NEO_PIXEL_ATTACH = 21; + public final static int NEO_PIXEL_ATTACH = 20; // > neoPixelSetAnimation/deviceId/animation/red/green/blue/b16 speed - public final static int NEO_PIXEL_SET_ANIMATION = 22; + public final static int NEO_PIXEL_SET_ANIMATION = 21; // > neoPixelWriteMatrix/deviceId/[] buffer - public final static int NEO_PIXEL_WRITE_MATRIX = 23; + public final static int NEO_PIXEL_WRITE_MATRIX = 22; // > analogWrite/pin/value - public final static int ANALOG_WRITE = 24; + public final static int ANALOG_WRITE = 23; // > digitalWrite/pin/value - public final static int DIGITAL_WRITE = 25; + public final static int DIGITAL_WRITE = 24; // > disablePin/pin - public final static int DISABLE_PIN = 26; + public final static int DISABLE_PIN = 25; // > disablePins - public final static int DISABLE_PINS = 27; + public final static int DISABLE_PINS = 26; // > pinMode/pin/mode - public final static int PIN_MODE = 28; + public final static int PIN_MODE = 27; // < publishDebug/str debugMsg - public final static int PUBLISH_DEBUG = 29; + public final static int PUBLISH_DEBUG = 28; // < publishPinArray/[] data - public final static int PUBLISH_PIN_ARRAY = 30; + public final static int PUBLISH_PIN_ARRAY = 29; // > setTrigger/pin/triggerValue - public final static int SET_TRIGGER = 31; + public final static int SET_TRIGGER = 30; // > setDebounce/pin/delay - public final static int SET_DEBOUNCE = 32; + public final static int SET_DEBOUNCE = 31; // > servoAttach/deviceId/pin/b16 initPos/b16 initVelocity/str name - public final static int SERVO_ATTACH = 33; + public final static int SERVO_ATTACH = 32; // > servoAttachPin/deviceId/pin - public final static int SERVO_ATTACH_PIN = 34; + public final static int SERVO_ATTACH_PIN = 33; // > servoDetachPin/deviceId - public final static int SERVO_DETACH_PIN = 35; - // > servoSetMaxVelocity/deviceId/b16 maxVelocity - public final static int SERVO_SET_MAX_VELOCITY = 36; + public final static int SERVO_DETACH_PIN = 34; // > servoSetVelocity/deviceId/b16 velocity - public final static int SERVO_SET_VELOCITY = 37; + public final static int SERVO_SET_VELOCITY = 35; // > servoSweepStart/deviceId/min/max/step - public final static int SERVO_SWEEP_START = 38; + public final static int SERVO_SWEEP_START = 36; // > servoSweepStop/deviceId - public final static int SERVO_SWEEP_STOP = 39; + public final static int SERVO_SWEEP_STOP = 37; // > servoMoveToMicroseconds/deviceId/b16 target - public final static int SERVO_MOVE_TO_MICROSECONDS = 40; + public final static int SERVO_MOVE_TO_MICROSECONDS = 38; // > servoSetAcceleration/deviceId/b16 acceleration - public final static int SERVO_SET_ACCELERATION = 41; + public final static int SERVO_SET_ACCELERATION = 39; + // < publishServoEvent/deviceId/eventType/currentPos/targetPos + public final static int PUBLISH_SERVO_EVENT = 40; // > serialAttach/deviceId/relayPin - public final static int SERIAL_ATTACH = 42; + public final static int SERIAL_ATTACH = 41; // > serialRelay/deviceId/[] data - public final static int SERIAL_RELAY = 43; + public final static int SERIAL_RELAY = 42; // < publishSerialData/deviceId/[] data - public final static int PUBLISH_SERIAL_DATA = 44; + public final static int PUBLISH_SERIAL_DATA = 43; // > ultrasonicSensorAttach/deviceId/triggerPin/echoPin - public final static int ULTRASONIC_SENSOR_ATTACH = 45; + public final static int ULTRASONIC_SENSOR_ATTACH = 44; // > ultrasonicSensorStartRanging/deviceId - public final static int ULTRASONIC_SENSOR_START_RANGING = 46; + public final static int ULTRASONIC_SENSOR_START_RANGING = 45; // > ultrasonicSensorStopRanging/deviceId - public final static int ULTRASONIC_SENSOR_STOP_RANGING = 47; + public final static int ULTRASONIC_SENSOR_STOP_RANGING = 46; // < publishUltrasonicSensorData/deviceId/b16 echoTime - public final static int PUBLISH_ULTRASONIC_SENSOR_DATA = 48; + public final static int PUBLISH_ULTRASONIC_SENSOR_DATA = 47; /** @@ -215,6 +213,7 @@ public static class AckLock { // public void publishI2cData(Integer deviceId/*byte*/, int[] data/*[]*/){} // public void publishDebug(String debugMsg/*str*/){} // public void publishPinArray(int[] data/*[]*/){} + // public void publishServoEvent(Integer deviceId/*byte*/, Integer eventType/*byte*/, Integer currentPos/*byte*/, Integer targetPos/*byte*/){} // public void publishSerialData(Integer deviceId/*byte*/, int[] data/*[]*/){} // public void publishUltrasonicSensorData(Integer deviceId/*byte*/, Integer echoTime/*b16*/){} @@ -469,6 +468,39 @@ public void processCommand(int[] ioCmd) { break; } + case PUBLISH_SERVO_EVENT: { + Integer deviceId = ioCmd[startPos+1]; // bu8 + startPos += 1; + Integer eventType = ioCmd[startPos+1]; // bu8 + startPos += 1; + Integer currentPos = ioCmd[startPos+1]; // bu8 + startPos += 1; + Integer targetPos = ioCmd[startPos+1]; // bu8 + startPos += 1; + if(invoke){ + arduino.invoke("publishServoEvent", deviceId, eventType, currentPos, targetPos); + } else { + arduino.publishServoEvent( deviceId, eventType, currentPos, targetPos); + } + if(record != null){ + rxBuffer.append("< publishServoEvent"); + rxBuffer.append("/"); + rxBuffer.append(deviceId); + rxBuffer.append("/"); + rxBuffer.append(eventType); + rxBuffer.append("/"); + rxBuffer.append(currentPos); + rxBuffer.append("/"); + rxBuffer.append(targetPos); + rxBuffer.append("\n"); + try{ + record.write(rxBuffer.toString().getBytes()); + rxBuffer.setLength(0); + }catch(IOException e){} + } + + break; + } case PUBLISH_SERIAL_DATA: { Integer deviceId = ioCmd[startPos+1]; // bu8 startPos += 1; @@ -735,35 +767,6 @@ public void echo(Float myFloat/*f32*/, Integer myByte/*byte*/, Float secondFloat } } - public void controllerAttach(Integer serialPort/*byte*/) { - try { - if (ackEnabled){ - waitForAck(); - } - write(MAGIC_NUMBER); - write(1 + 1); // size - write(CONTROLLER_ATTACH); // msgType = 12 - write(serialPort); - - if (ackEnabled){ - // we just wrote - block threads sending - // until they get an ack - ackRecievedLock.acknowledged = false; - } - if(record != null){ - txBuffer.append("> controllerAttach"); - txBuffer.append("/"); - txBuffer.append(serialPort); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } - - } catch (Exception e) { - log.error("controllerAttach threw",e); - } - } - public void customMsg(int[] msg/*[]*/) { try { if (ackEnabled){ @@ -771,7 +774,7 @@ public void customMsg(int[] msg/*[]*/) { } write(MAGIC_NUMBER); write(1 + (1 + msg.length)); // size - write(CUSTOM_MSG); // msgType = 13 + write(CUSTOM_MSG); // msgType = 12 write(msg); if (ackEnabled){ @@ -800,7 +803,7 @@ public void deviceDetach(Integer deviceId/*byte*/) { } write(MAGIC_NUMBER); write(1 + 1); // size - write(DEVICE_DETACH); // msgType = 15 + write(DEVICE_DETACH); // msgType = 14 write(deviceId); if (ackEnabled){ @@ -829,7 +832,7 @@ public void i2cBusAttach(Integer deviceId/*byte*/, Integer i2cBus/*byte*/) { } write(MAGIC_NUMBER); write(1 + 1 + 1); // size - write(I2C_BUS_ATTACH); // msgType = 16 + write(I2C_BUS_ATTACH); // msgType = 15 write(deviceId); write(i2cBus); @@ -861,7 +864,7 @@ public void i2cRead(Integer deviceId/*byte*/, Integer deviceAddress/*byte*/, Int } write(MAGIC_NUMBER); write(1 + 1 + 1 + 1); // size - write(I2C_READ); // msgType = 17 + write(I2C_READ); // msgType = 16 write(deviceId); write(deviceAddress); write(size); @@ -896,7 +899,7 @@ public void i2cWrite(Integer deviceId/*byte*/, Integer deviceAddress/*byte*/, in } write(MAGIC_NUMBER); write(1 + 1 + 1 + (1 + data.length)); // size - write(I2C_WRITE); // msgType = 18 + write(I2C_WRITE); // msgType = 17 write(deviceId); write(deviceAddress); write(data); @@ -931,7 +934,7 @@ public void i2cWriteRead(Integer deviceId/*byte*/, Integer deviceAddress/*byte*/ } write(MAGIC_NUMBER); write(1 + 1 + 1 + 1 + 1); // size - write(I2C_WRITE_READ); // msgType = 19 + write(I2C_WRITE_READ); // msgType = 18 write(deviceId); write(deviceAddress); write(readSize); @@ -969,7 +972,7 @@ public void neoPixelAttach(Integer deviceId/*byte*/, Integer pin/*byte*/, Intege } write(MAGIC_NUMBER); write(1 + 1 + 1 + 4); // size - write(NEO_PIXEL_ATTACH); // msgType = 21 + write(NEO_PIXEL_ATTACH); // msgType = 20 write(deviceId); write(pin); writeb32(numPixels); @@ -1004,7 +1007,7 @@ public void neoPixelSetAnimation(Integer deviceId/*byte*/, Integer animation/*by } write(MAGIC_NUMBER); write(1 + 1 + 1 + 1 + 1 + 1 + 2); // size - write(NEO_PIXEL_SET_ANIMATION); // msgType = 22 + write(NEO_PIXEL_SET_ANIMATION); // msgType = 21 write(deviceId); write(animation); write(red); @@ -1048,7 +1051,7 @@ public void neoPixelWriteMatrix(Integer deviceId/*byte*/, int[] buffer/*[]*/) { } write(MAGIC_NUMBER); write(1 + 1 + (1 + buffer.length)); // size - write(NEO_PIXEL_WRITE_MATRIX); // msgType = 23 + write(NEO_PIXEL_WRITE_MATRIX); // msgType = 22 write(deviceId); write(buffer); @@ -1080,7 +1083,7 @@ public void analogWrite(Integer pin/*byte*/, Integer value/*byte*/) { } write(MAGIC_NUMBER); write(1 + 1 + 1); // size - write(ANALOG_WRITE); // msgType = 24 + write(ANALOG_WRITE); // msgType = 23 write(pin); write(value); @@ -1112,7 +1115,7 @@ public void digitalWrite(Integer pin/*byte*/, Integer value/*byte*/) { } write(MAGIC_NUMBER); write(1 + 1 + 1); // size - write(DIGITAL_WRITE); // msgType = 25 + write(DIGITAL_WRITE); // msgType = 24 write(pin); write(value); @@ -1144,7 +1147,7 @@ public void disablePin(Integer pin/*byte*/) { } write(MAGIC_NUMBER); write(1 + 1); // size - write(DISABLE_PIN); // msgType = 26 + write(DISABLE_PIN); // msgType = 25 write(pin); if (ackEnabled){ @@ -1173,7 +1176,7 @@ public void disablePins() { } write(MAGIC_NUMBER); write(1); // size - write(DISABLE_PINS); // msgType = 27 + write(DISABLE_PINS); // msgType = 26 if (ackEnabled){ // we just wrote - block threads sending @@ -1199,7 +1202,7 @@ public void pinMode(Integer pin/*byte*/, Integer mode/*byte*/) { } write(MAGIC_NUMBER); write(1 + 1 + 1); // size - write(PIN_MODE); // msgType = 28 + write(PIN_MODE); // msgType = 27 write(pin); write(mode); @@ -1231,7 +1234,7 @@ public void setTrigger(Integer pin/*byte*/, Integer triggerValue/*byte*/) { } write(MAGIC_NUMBER); write(1 + 1 + 1); // size - write(SET_TRIGGER); // msgType = 31 + write(SET_TRIGGER); // msgType = 30 write(pin); write(triggerValue); @@ -1263,7 +1266,7 @@ public void setDebounce(Integer pin/*byte*/, Integer delay/*byte*/) { } write(MAGIC_NUMBER); write(1 + 1 + 1); // size - write(SET_DEBOUNCE); // msgType = 32 + write(SET_DEBOUNCE); // msgType = 31 write(pin); write(delay); @@ -1295,7 +1298,7 @@ public void servoAttach(Integer deviceId/*byte*/, Integer pin/*byte*/, Integer i } write(MAGIC_NUMBER); write(1 + 1 + 1 + 2 + 2 + (1 + name.length())); // size - write(SERVO_ATTACH); // msgType = 33 + write(SERVO_ATTACH); // msgType = 32 write(deviceId); write(pin); writeb16(initPos); @@ -1336,7 +1339,7 @@ public void servoAttachPin(Integer deviceId/*byte*/, Integer pin/*byte*/) { } write(MAGIC_NUMBER); write(1 + 1 + 1); // size - write(SERVO_ATTACH_PIN); // msgType = 34 + write(SERVO_ATTACH_PIN); // msgType = 33 write(deviceId); write(pin); @@ -1368,7 +1371,7 @@ public void servoDetachPin(Integer deviceId/*byte*/) { } write(MAGIC_NUMBER); write(1 + 1); // size - write(SERVO_DETACH_PIN); // msgType = 35 + write(SERVO_DETACH_PIN); // msgType = 34 write(deviceId); if (ackEnabled){ @@ -1390,38 +1393,6 @@ public void servoDetachPin(Integer deviceId/*byte*/) { } } - public void servoSetMaxVelocity(Integer deviceId/*byte*/, Integer maxVelocity/*b16*/) { - try { - if (ackEnabled){ - waitForAck(); - } - write(MAGIC_NUMBER); - write(1 + 1 + 2); // size - write(SERVO_SET_MAX_VELOCITY); // msgType = 36 - write(deviceId); - writeb16(maxVelocity); - - if (ackEnabled){ - // we just wrote - block threads sending - // until they get an ack - ackRecievedLock.acknowledged = false; - } - if(record != null){ - txBuffer.append("> servoSetMaxVelocity"); - txBuffer.append("/"); - txBuffer.append(deviceId); - txBuffer.append("/"); - txBuffer.append(maxVelocity); - txBuffer.append("\n"); - record.write(txBuffer.toString().getBytes()); - txBuffer.setLength(0); - } - - } catch (Exception e) { - log.error("servoSetMaxVelocity threw",e); - } - } - public void servoSetVelocity(Integer deviceId/*byte*/, Integer velocity/*b16*/) { try { if (ackEnabled){ @@ -1429,7 +1400,7 @@ public void servoSetVelocity(Integer deviceId/*byte*/, Integer velocity/*b16*/) } write(MAGIC_NUMBER); write(1 + 1 + 2); // size - write(SERVO_SET_VELOCITY); // msgType = 37 + write(SERVO_SET_VELOCITY); // msgType = 35 write(deviceId); writeb16(velocity); @@ -1461,7 +1432,7 @@ public void servoSweepStart(Integer deviceId/*byte*/, Integer min/*byte*/, Integ } write(MAGIC_NUMBER); write(1 + 1 + 1 + 1 + 1); // size - write(SERVO_SWEEP_START); // msgType = 38 + write(SERVO_SWEEP_START); // msgType = 36 write(deviceId); write(min); write(max); @@ -1499,7 +1470,7 @@ public void servoSweepStop(Integer deviceId/*byte*/) { } write(MAGIC_NUMBER); write(1 + 1); // size - write(SERVO_SWEEP_STOP); // msgType = 39 + write(SERVO_SWEEP_STOP); // msgType = 37 write(deviceId); if (ackEnabled){ @@ -1528,7 +1499,7 @@ public void servoMoveToMicroseconds(Integer deviceId/*byte*/, Integer target/*b1 } write(MAGIC_NUMBER); write(1 + 1 + 2); // size - write(SERVO_MOVE_TO_MICROSECONDS); // msgType = 40 + write(SERVO_MOVE_TO_MICROSECONDS); // msgType = 38 write(deviceId); writeb16(target); @@ -1560,7 +1531,7 @@ public void servoSetAcceleration(Integer deviceId/*byte*/, Integer acceleration/ } write(MAGIC_NUMBER); write(1 + 1 + 2); // size - write(SERVO_SET_ACCELERATION); // msgType = 41 + write(SERVO_SET_ACCELERATION); // msgType = 39 write(deviceId); writeb16(acceleration); @@ -1592,7 +1563,7 @@ public void serialAttach(Integer deviceId/*byte*/, Integer relayPin/*byte*/) { } write(MAGIC_NUMBER); write(1 + 1 + 1); // size - write(SERIAL_ATTACH); // msgType = 42 + write(SERIAL_ATTACH); // msgType = 41 write(deviceId); write(relayPin); @@ -1624,7 +1595,7 @@ public void serialRelay(Integer deviceId/*byte*/, int[] data/*[]*/) { } write(MAGIC_NUMBER); write(1 + 1 + (1 + data.length)); // size - write(SERIAL_RELAY); // msgType = 43 + write(SERIAL_RELAY); // msgType = 42 write(deviceId); write(data); @@ -1656,7 +1627,7 @@ public void ultrasonicSensorAttach(Integer deviceId/*byte*/, Integer triggerPin/ } write(MAGIC_NUMBER); write(1 + 1 + 1 + 1); // size - write(ULTRASONIC_SENSOR_ATTACH); // msgType = 45 + write(ULTRASONIC_SENSOR_ATTACH); // msgType = 44 write(deviceId); write(triggerPin); write(echoPin); @@ -1691,7 +1662,7 @@ public void ultrasonicSensorStartRanging(Integer deviceId/*byte*/) { } write(MAGIC_NUMBER); write(1 + 1); // size - write(ULTRASONIC_SENSOR_START_RANGING); // msgType = 46 + write(ULTRASONIC_SENSOR_START_RANGING); // msgType = 45 write(deviceId); if (ackEnabled){ @@ -1720,7 +1691,7 @@ public void ultrasonicSensorStopRanging(Integer deviceId/*byte*/) { } write(MAGIC_NUMBER); write(1 + 1); // size - write(ULTRASONIC_SENSOR_STOP_RANGING); // msgType = 47 + write(ULTRASONIC_SENSOR_STOP_RANGING); // msgType = 46 write(deviceId); if (ackEnabled){ @@ -1778,9 +1749,6 @@ public static String methodToString(int method) { case PUBLISH_ECHO:{ return "publishEcho"; } - case CONTROLLER_ATTACH:{ - return "controllerAttach"; - } case CUSTOM_MSG:{ return "customMsg"; } @@ -1850,9 +1818,6 @@ public static String methodToString(int method) { case SERVO_DETACH_PIN:{ return "servoDetachPin"; } - case SERVO_SET_MAX_VELOCITY:{ - return "servoSetMaxVelocity"; - } case SERVO_SET_VELOCITY:{ return "servoSetVelocity"; } @@ -1868,6 +1833,9 @@ public static String methodToString(int method) { case SERVO_SET_ACCELERATION:{ return "servoSetAcceleration"; } + case PUBLISH_SERVO_EVENT:{ + return "publishServoEvent"; + } case SERIAL_ATTACH:{ return "serialAttach"; } diff --git a/src/org/myrobotlab/arduino/VirtualMsg.java b/src/org/myrobotlab/arduino/VirtualMsg.java index 726c76a1b9..ae152cb613 100644 --- a/src/org/myrobotlab/arduino/VirtualMsg.java +++ b/src/org/myrobotlab/arduino/VirtualMsg.java @@ -63,7 +63,7 @@ public class VirtualMsg { public static final int MAX_MSG_SIZE = 64; public static final int MAGIC_NUMBER = 170; // 10101010 - public static final int MRLCOMM_VERSION = 53; + public static final int MRLCOMM_VERSION = 54; // send buffer int sendBufferSize = 0; @@ -127,80 +127,78 @@ public static class AckLock { public final static int ECHO = 10; // < publishEcho/f32 myFloat/myByte/f32 secondFloat public final static int PUBLISH_ECHO = 11; - // > controllerAttach/serialPort - public final static int CONTROLLER_ATTACH = 12; // > customMsg/[] msg - public final static int CUSTOM_MSG = 13; + public final static int CUSTOM_MSG = 12; // < publishCustomMsg/[] msg - public final static int PUBLISH_CUSTOM_MSG = 14; + public final static int PUBLISH_CUSTOM_MSG = 13; // > deviceDetach/deviceId - public final static int DEVICE_DETACH = 15; + public final static int DEVICE_DETACH = 14; // > i2cBusAttach/deviceId/i2cBus - public final static int I2C_BUS_ATTACH = 16; + public final static int I2C_BUS_ATTACH = 15; // > i2cRead/deviceId/deviceAddress/size - public final static int I2C_READ = 17; + public final static int I2C_READ = 16; // > i2cWrite/deviceId/deviceAddress/[] data - public final static int I2C_WRITE = 18; + public final static int I2C_WRITE = 17; // > i2cWriteRead/deviceId/deviceAddress/readSize/writeValue - public final static int I2C_WRITE_READ = 19; + public final static int I2C_WRITE_READ = 18; // < publishI2cData/deviceId/[] data - public final static int PUBLISH_I2C_DATA = 20; + public final static int PUBLISH_I2C_DATA = 19; // > neoPixelAttach/deviceId/pin/b32 numPixels - public final static int NEO_PIXEL_ATTACH = 21; + public final static int NEO_PIXEL_ATTACH = 20; // > neoPixelSetAnimation/deviceId/animation/red/green/blue/b16 speed - public final static int NEO_PIXEL_SET_ANIMATION = 22; + public final static int NEO_PIXEL_SET_ANIMATION = 21; // > neoPixelWriteMatrix/deviceId/[] buffer - public final static int NEO_PIXEL_WRITE_MATRIX = 23; + public final static int NEO_PIXEL_WRITE_MATRIX = 22; // > analogWrite/pin/value - public final static int ANALOG_WRITE = 24; + public final static int ANALOG_WRITE = 23; // > digitalWrite/pin/value - public final static int DIGITAL_WRITE = 25; + public final static int DIGITAL_WRITE = 24; // > disablePin/pin - public final static int DISABLE_PIN = 26; + public final static int DISABLE_PIN = 25; // > disablePins - public final static int DISABLE_PINS = 27; + public final static int DISABLE_PINS = 26; // > pinMode/pin/mode - public final static int PIN_MODE = 28; + public final static int PIN_MODE = 27; // < publishDebug/str debugMsg - public final static int PUBLISH_DEBUG = 29; + public final static int PUBLISH_DEBUG = 28; // < publishPinArray/[] data - public final static int PUBLISH_PIN_ARRAY = 30; + public final static int PUBLISH_PIN_ARRAY = 29; // > setTrigger/pin/triggerValue - public final static int SET_TRIGGER = 31; + public final static int SET_TRIGGER = 30; // > setDebounce/pin/delay - public final static int SET_DEBOUNCE = 32; + public final static int SET_DEBOUNCE = 31; // > servoAttach/deviceId/pin/b16 initPos/b16 initVelocity/str name - public final static int SERVO_ATTACH = 33; + public final static int SERVO_ATTACH = 32; // > servoAttachPin/deviceId/pin - public final static int SERVO_ATTACH_PIN = 34; + public final static int SERVO_ATTACH_PIN = 33; // > servoDetachPin/deviceId - public final static int SERVO_DETACH_PIN = 35; - // > servoSetMaxVelocity/deviceId/b16 maxVelocity - public final static int SERVO_SET_MAX_VELOCITY = 36; + public final static int SERVO_DETACH_PIN = 34; // > servoSetVelocity/deviceId/b16 velocity - public final static int SERVO_SET_VELOCITY = 37; + public final static int SERVO_SET_VELOCITY = 35; // > servoSweepStart/deviceId/min/max/step - public final static int SERVO_SWEEP_START = 38; + public final static int SERVO_SWEEP_START = 36; // > servoSweepStop/deviceId - public final static int SERVO_SWEEP_STOP = 39; + public final static int SERVO_SWEEP_STOP = 37; // > servoMoveToMicroseconds/deviceId/b16 target - public final static int SERVO_MOVE_TO_MICROSECONDS = 40; + public final static int SERVO_MOVE_TO_MICROSECONDS = 38; // > servoSetAcceleration/deviceId/b16 acceleration - public final static int SERVO_SET_ACCELERATION = 41; + public final static int SERVO_SET_ACCELERATION = 39; + // < publishServoEvent/deviceId/eventType/currentPos/targetPos + public final static int PUBLISH_SERVO_EVENT = 40; // > serialAttach/deviceId/relayPin - public final static int SERIAL_ATTACH = 42; + public final static int SERIAL_ATTACH = 41; // > serialRelay/deviceId/[] data - public final static int SERIAL_RELAY = 43; + public final static int SERIAL_RELAY = 42; // < publishSerialData/deviceId/[] data - public final static int PUBLISH_SERIAL_DATA = 44; + public final static int PUBLISH_SERIAL_DATA = 43; // > ultrasonicSensorAttach/deviceId/triggerPin/echoPin - public final static int ULTRASONIC_SENSOR_ATTACH = 45; + public final static int ULTRASONIC_SENSOR_ATTACH = 44; // > ultrasonicSensorStartRanging/deviceId - public final static int ULTRASONIC_SENSOR_START_RANGING = 46; + public final static int ULTRASONIC_SENSOR_START_RANGING = 45; // > ultrasonicSensorStopRanging/deviceId - public final static int ULTRASONIC_SENSOR_STOP_RANGING = 47; + public final static int ULTRASONIC_SENSOR_STOP_RANGING = 46; // < publishUltrasonicSensorData/deviceId/b16 echoTime - public final static int PUBLISH_ULTRASONIC_SENSOR_DATA = 48; + public final static int PUBLISH_ULTRASONIC_SENSOR_DATA = 47; /** @@ -214,7 +212,6 @@ public static class AckLock { // public void softReset(){} // public void enableAck(Boolean enabled/*bool*/){} // public void echo(Float myFloat/*f32*/, Integer myByte/*byte*/, Float secondFloat/*f32*/){} - // public void controllerAttach(Integer serialPort/*byte*/){} // public void customMsg(int[] msg/*[]*/){} // public void deviceDetach(Integer deviceId/*byte*/){} // public void i2cBusAttach(Integer deviceId/*byte*/, Integer i2cBus/*byte*/){} @@ -234,7 +231,6 @@ public static class AckLock { // public void servoAttach(Integer deviceId/*byte*/, Integer pin/*byte*/, Integer initPos/*b16*/, Integer initVelocity/*b16*/, String name/*str*/){} // public void servoAttachPin(Integer deviceId/*byte*/, Integer pin/*byte*/){} // public void servoDetachPin(Integer deviceId/*byte*/){} - // public void servoSetMaxVelocity(Integer deviceId/*byte*/, Integer maxVelocity/*b16*/){} // public void servoSetVelocity(Integer deviceId/*byte*/, Integer velocity/*b16*/){} // public void servoSweepStart(Integer deviceId/*byte*/, Integer min/*byte*/, Integer max/*byte*/, Integer step/*byte*/){} // public void servoSweepStop(Integer deviceId/*byte*/){} @@ -371,16 +367,6 @@ public void processCommand(int[] ioCmd) { } break; } - case CONTROLLER_ATTACH: { - Integer serialPort = ioCmd[startPos+1]; // bu8 - startPos += 1; - if(invoke){ - arduino.invoke("controllerAttach", serialPort); - } else { - arduino.controllerAttach( serialPort); - } - break; - } case CUSTOM_MSG: { int[] msg = subArray(ioCmd, startPos+2, ioCmd[startPos+1]); startPos += 1 + ioCmd[startPos+1]; @@ -621,18 +607,6 @@ public void processCommand(int[] ioCmd) { } break; } - case SERVO_SET_MAX_VELOCITY: { - Integer deviceId = ioCmd[startPos+1]; // bu8 - startPos += 1; - Integer maxVelocity = b16(ioCmd, startPos+1); - startPos += 2; //b16 - if(invoke){ - arduino.invoke("servoSetMaxVelocity", deviceId, maxVelocity); - } else { - arduino.servoSetMaxVelocity( deviceId, maxVelocity); - } - break; - } case SERVO_SET_VELOCITY: { Integer deviceId = ioCmd[startPos+1]; // bu8 startPos += 1; @@ -904,7 +878,7 @@ public void publishCustomMsg(int[] msg/*[]*/) { } write(MAGIC_NUMBER); write(1 + (1 + msg.length)); // size - write(PUBLISH_CUSTOM_MSG); // msgType = 14 + write(PUBLISH_CUSTOM_MSG); // msgType = 13 write(msg); if (ackEnabled){ @@ -933,7 +907,7 @@ public void publishI2cData(Integer deviceId/*byte*/, int[] data/*[]*/) { } write(MAGIC_NUMBER); write(1 + 1 + (1 + data.length)); // size - write(PUBLISH_I2C_DATA); // msgType = 20 + write(PUBLISH_I2C_DATA); // msgType = 19 write(deviceId); write(data); @@ -965,7 +939,7 @@ public void publishDebug(String debugMsg/*str*/) { } write(MAGIC_NUMBER); write(1 + (1 + debugMsg.length())); // size - write(PUBLISH_DEBUG); // msgType = 29 + write(PUBLISH_DEBUG); // msgType = 28 write(debugMsg); if (ackEnabled){ @@ -994,7 +968,7 @@ public void publishPinArray(int[] data/*[]*/) { } write(MAGIC_NUMBER); write(1 + (1 + data.length)); // size - write(PUBLISH_PIN_ARRAY); // msgType = 30 + write(PUBLISH_PIN_ARRAY); // msgType = 29 write(data); if (ackEnabled){ @@ -1016,6 +990,44 @@ public void publishPinArray(int[] data/*[]*/) { } } + public void publishServoEvent(Integer deviceId/*byte*/, Integer eventType/*byte*/, Integer currentPos/*byte*/, Integer targetPos/*byte*/) { + try { + if (ackEnabled){ + waitForAck(); + } + write(MAGIC_NUMBER); + write(1 + 1 + 1 + 1 + 1); // size + write(PUBLISH_SERVO_EVENT); // msgType = 40 + write(deviceId); + write(eventType); + write(currentPos); + write(targetPos); + + if (ackEnabled){ + // we just wrote - block threads sending + // until they get an ack + ackRecievedLock.acknowledged = false; + } + if(record != null){ + txBuffer.append("> publishServoEvent"); + txBuffer.append("/"); + txBuffer.append(deviceId); + txBuffer.append("/"); + txBuffer.append(eventType); + txBuffer.append("/"); + txBuffer.append(currentPos); + txBuffer.append("/"); + txBuffer.append(targetPos); + txBuffer.append("\n"); + record.write(txBuffer.toString().getBytes()); + txBuffer.setLength(0); + } + + } catch (Exception e) { + log.error("publishServoEvent threw",e); + } + } + public void publishSerialData(Integer deviceId/*byte*/, int[] data/*[]*/) { try { if (ackEnabled){ @@ -1023,7 +1035,7 @@ public void publishSerialData(Integer deviceId/*byte*/, int[] data/*[]*/) { } write(MAGIC_NUMBER); write(1 + 1 + (1 + data.length)); // size - write(PUBLISH_SERIAL_DATA); // msgType = 44 + write(PUBLISH_SERIAL_DATA); // msgType = 43 write(deviceId); write(data); @@ -1055,7 +1067,7 @@ public void publishUltrasonicSensorData(Integer deviceId/*byte*/, Integer echoTi } write(MAGIC_NUMBER); write(1 + 1 + 2); // size - write(PUBLISH_ULTRASONIC_SENSOR_DATA); // msgType = 48 + write(PUBLISH_ULTRASONIC_SENSOR_DATA); // msgType = 47 write(deviceId); writeb16(echoTime); @@ -1116,9 +1128,6 @@ public static String methodToString(int method) { case PUBLISH_ECHO:{ return "publishEcho"; } - case CONTROLLER_ATTACH:{ - return "controllerAttach"; - } case CUSTOM_MSG:{ return "customMsg"; } @@ -1188,9 +1197,6 @@ public static String methodToString(int method) { case SERVO_DETACH_PIN:{ return "servoDetachPin"; } - case SERVO_SET_MAX_VELOCITY:{ - return "servoSetMaxVelocity"; - } case SERVO_SET_VELOCITY:{ return "servoSetVelocity"; } @@ -1206,6 +1212,9 @@ public static String methodToString(int method) { case SERVO_SET_ACCELERATION:{ return "servoSetAcceleration"; } + case PUBLISH_SERVO_EVENT:{ + return "publishServoEvent"; + } case SERIAL_ATTACH:{ return "serialAttach"; } diff --git a/src/org/myrobotlab/arduino/virtual/MrlComm.java b/src/org/myrobotlab/arduino/virtual/MrlComm.java index 11ae145311..04cb0f3313 100644 --- a/src/org/myrobotlab/arduino/virtual/MrlComm.java +++ b/src/org/myrobotlab/arduino/virtual/MrlComm.java @@ -474,12 +474,6 @@ public void echo(float myFloat, int myByte, float mySecondFloat) { msg.publishEcho(myFloat, myByte & 0xFF, mySecondFloat); } - // > controllerAttach/serialPort - // TODO - talk to calamity - public void controllerAttach(int serialPort) { - msg.publishDebug(String("controllerAttach " + String(serialPort))); - } - // > customMsg/[] msg // from PC -. loads customMsg buffer public void customMsg(int[] msg) { @@ -604,12 +598,6 @@ public void servoDetachPin(int deviceId) { servo.detachPin(); } - // > servoSetMaxVelocity/deviceId/b16 maxVelocity - public void servoSetMaxVelocity(int deviceId, int maxVelocity) { - MrlServo servo = (MrlServo) getDevice(deviceId); - servo.setMaxVelocity(maxVelocity); - } - // > servoSetVelocity/deviceId/b16 velocity public void servoSetVelocity(int deviceId, int velocity) { MrlServo servo = (MrlServo) getDevice(deviceId); diff --git a/src/org/myrobotlab/arduino/virtual/MrlServo.java b/src/org/myrobotlab/arduino/virtual/MrlServo.java index b4fce7fde4..158e840202 100644 --- a/src/org/myrobotlab/arduino/virtual/MrlServo.java +++ b/src/org/myrobotlab/arduino/virtual/MrlServo.java @@ -16,6 +16,8 @@ public class MrlServo extends Device { public final static Logger log = LoggerFactory.getLogger(MrlServo.class); + public static final int SERVO_EVENT_STOPPED = 2; + VirtualServo servo; // servo pointer - in case our device is a servo public int pin; public boolean isMoving; @@ -69,10 +71,10 @@ boolean attach(int pin, int initPosUs, int initVelocity, String name){ targetPosUs = initPosUs; velocity = initVelocity; servo.attach(pin); + //publishServoEvent(SERVO_EVENT_STOPPED); return true; } - public void update() { } @@ -123,5 +125,10 @@ public void moveToMicroseconds(int posUs) { } } +// private void publishServoEvent(int type) { +// // TODO Auto-generated method stub +// +//} + }; diff --git a/src/org/myrobotlab/service/Adafruit16CServoDriver.java b/src/org/myrobotlab/service/Adafruit16CServoDriver.java index 63b979226c..2673fbd959 100644 --- a/src/org/myrobotlab/service/Adafruit16CServoDriver.java +++ b/src/org/myrobotlab/service/Adafruit16CServoDriver.java @@ -673,7 +673,6 @@ public void servoDetachPin(ServoControl servo) { } - @Override public void servoSetMaxVelocity(ServoControl servo) { // TODO Auto-generated method stub. // perhaps cannot do this with Adafruit16CServoDriver diff --git a/src/org/myrobotlab/service/Arduino.java b/src/org/myrobotlab/service/Arduino.java index faa8f49289..47b9a85803 100644 --- a/src/org/myrobotlab/service/Arduino.java +++ b/src/org/myrobotlab/service/Arduino.java @@ -460,11 +460,6 @@ public void connect(String port, int rate, int databits, int stopbits, int parit // GAP broadcastState(); } - public void controllerAttach(Arduino controller, int serialPort) { - attachedController.put(serialPort, controller); - msg.controllerAttach(serialPort); - } - public Map createPinList() { pinMap = new ConcurrentHashMap(); pinIndex = new ConcurrentHashMap(); @@ -1583,11 +1578,6 @@ public void servoDetachPin(ServoControl servo) { msg.servoDetachPin(getDeviceId(servo)); } - @Override - // > servoSetMaxVelocity/deviceId/b16 maxVelocity - public void servoSetMaxVelocity(ServoControl servo) { - msg.servoSetMaxVelocity(getDeviceId(servo), (int) servo.getMaxVelocity()); - } @Override // > servoSetVelocity/deviceId/b16 velocity @@ -2038,5 +2028,11 @@ public static void main(String[] args) { } } + public Integer publishServoEvent(Integer deviceId, Integer eventType, Integer currentPos, Integer targetPos) { + // TODO Auto-generated method stub + ((Servo) getDevice(deviceId)).onServoEvent(eventType, currentPos, targetPos); + return currentPos; + } + } diff --git a/src/org/myrobotlab/service/Servo.java b/src/org/myrobotlab/service/Servo.java index dd4bf690c2..ed24a410ba 100644 --- a/src/org/myrobotlab/service/Servo.java +++ b/src/org/myrobotlab/service/Servo.java @@ -214,7 +214,13 @@ static public ServiceType getMetaData() { public int defaultDetachDelay = 10000; private boolean moving; + private double currentPosInput; + public transient static final int SERVO_EVENT_STOPPED = 1; + public transient static final int SERVO_EVENT_POSITION_UPDATE = 2; + + + class IKData { String name; Double pos; @@ -234,6 +240,7 @@ public void onRegistered(ServiceInterface s) { } public void addServoEventListener(NameProvider service) { + isEventsEnabled = true; addListener("publishServoEvent", service.getName(), "onServoEvent"); } @@ -274,7 +281,7 @@ public void attach(int pin) { */ @Override public void detach() { - this.isPinAttached = false; + isPinAttached = false; if (controller != null) { controller.servoDetachPin(this); } @@ -365,26 +372,26 @@ public void moveTo(double pos) { controller.servoMoveTo(this); lastActivityTime = System.currentTimeMillis(); - if (velocity > 0.0) { - moving = true; - if (getTasks().containsKey("EndMoving")){ - purgeTask("EndMoving"); - } - addTask("EndMoving", timeToMove(), "endMoving"); - } - - if (isEventsEnabled) { - // update others of our position change - invoke("publishServoEvent", targetOutput); - broadcastState(); - } - if (isIKEventEnabled) { - IKData data = new IKData(); - data.name = getName(); - data.pos = targetPos; - invoke("publishIKServoEvent", data); - broadcastState(); - } +// if (velocity > 0.0) { +// moving = true; +// if (getTasks().containsKey("EndMoving")){ +// purgeTask("EndMoving"); +// } +// addTask("EndMoving", timeToMove(), "endMoving"); +// } + +// if (isEventsEnabled) { +// // update others of our position change +// invoke("publishServoEvent", targetOutput); //shouldn't be publishing targetPos? +// broadcastState(); +// } +// if (isIKEventEnabled) { +// IKData data = new IKData(); +// data.name = getName(); +// data.pos = targetPos; +// invoke("publishIKServoEvent", data); +// broadcastState(); +// } } /** @@ -696,16 +703,14 @@ public void detach(ServoController controller) { public void setMaxVelocity(int velocity) { this.maxVelocity = velocity; - if (controller != null) { - controller.servoSetMaxVelocity(this); - } } public void setVelocity(double velocity) { if (maxVelocity != -1 && velocity > maxVelocity) { velocity = maxVelocity; + log.info("Trying to set velocity to a value greater than max velocity"); } - this.velocity = velocity; + velocity = velocity; if (controller != null) { controller.servoSetVelocity(this); } @@ -855,48 +860,7 @@ public double getAcceleration() { * @return the current position of the servo */ public Double getCurrentPos() { - Double currentPos = null; - if (velocity == -1) { - return targetPos; - } else { - long currentTime = System.currentTimeMillis(); - double dOutput = velocity * (currentTime - lastActivityTime) / 1000; - if (mapper.getMaxOutput() > 500) { - dOutput *= (2400 - 544) / 180; - } - log.info("dOutput = {}", mapper.calcInput(dOutput)); - if (targetPos > lastPos) { - if (mapper.getMaxY() > mapper.getMinY()) { - currentPos = mapper.calcInput(mapper.calcOutput(lastPos) + dOutput); - if (currentPos > targetPos) { - currentPos = targetPos; - } - } else { - currentPos = mapper.calcInput(mapper.calcOutput(lastPos) - dOutput); - if (currentPos > targetPos) { - currentPos = targetPos; - } - } - } else if (targetPos < lastPos) { - if (mapper.getMaxY() > mapper.getMinY()) { - currentPos = mapper.calcInput(mapper.calcOutput(lastPos) - dOutput); - if (currentPos < targetPos) { - currentPos = targetPos; - } - } else { - currentPos = mapper.calcInput(mapper.calcOutput(lastPos) + dOutput); - if (currentPos < targetPos) { - currentPos = targetPos; - } - } - } else { - currentPos = targetPos; - } - if (currentPos.isNaN()) { - return targetPos; - } - return currentPos; - } + return currentPosInput; } /** @@ -967,7 +931,6 @@ public void endMoving(){ purgeTask("EndMoving"); } if (autoAttach && isPinAttached()){ - if (velocity < 0) sleep(defaultDetachDelay); detach(); } moving = false; @@ -976,5 +939,29 @@ public void endMoving(){ public boolean isMoving() { return moving; } + + public void onServoEvent(Integer eventType, Integer currentPos, Integer targetPos) { + currentPosInput = mapper.calcInput(currentPos.doubleValue()); + if (isEventsEnabled) { + invoke("publishServoEvent", currentPosInput); + } + if (isIKEventEnabled) { + IKData data = new IKData(); + data.name = getName(); + data.pos = currentPosInput; + invoke("publishIKServoEvent", data); + } + if (eventType == SERVO_EVENT_STOPPED && autoAttach && isPinAttached()) { + if (velocity > -1) { + detach(); + } + else { + if (getTasks().containsKey("EndMoving")) { + purgeTask("EndMoving"); + } + addTask("EndMoving",defaultDetachDelay, "endMoving"); + } + } + } } diff --git a/src/org/myrobotlab/service/interfaces/ServoController.java b/src/org/myrobotlab/service/interfaces/ServoController.java index 1c02e5cb5b..0ff8e96c55 100644 --- a/src/org/myrobotlab/service/interfaces/ServoController.java +++ b/src/org/myrobotlab/service/interfaces/ServoController.java @@ -49,8 +49,6 @@ public interface ServoController extends DeviceController { void servoDetachPin(ServoControl servo); - void servoSetMaxVelocity(ServoControl servo); - void servoSetVelocity(ServoControl servo); void servoSetAcceleration(ServoControl servo); diff --git a/src/resource/Arduino/MRLComm/ArduinoMsgCodec.h b/src/resource/Arduino/MRLComm/ArduinoMsgCodec.h index b975e6a707..c5afb03142 100644 --- a/src/resource/Arduino/MRLComm/ArduinoMsgCodec.h +++ b/src/resource/Arduino/MRLComm/ArduinoMsgCodec.h @@ -9,7 +9,7 @@ * src\resource\Arduino\generate\ArduinoMsgCodec.template.h */ -#define MRLCOMM_VERSION 53 +#define MRLCOMM_VERSION 54 #define MAGIC_NUMBER 170 // 10101010 #define MAX_MSG_SIZE 64 @@ -46,80 +46,78 @@ #define ECHO 10 // < publishEcho/f32 myFloat/myByte/f32 secondFloat #define PUBLISH_ECHO 11 -// > controllerAttach/serialPort -#define CONTROLLER_ATTACH 12 // > customMsg/[] msg -#define CUSTOM_MSG 13 +#define CUSTOM_MSG 12 // < publishCustomMsg/[] msg -#define PUBLISH_CUSTOM_MSG 14 +#define PUBLISH_CUSTOM_MSG 13 // > deviceDetach/deviceId -#define DEVICE_DETACH 15 +#define DEVICE_DETACH 14 // > i2cBusAttach/deviceId/i2cBus -#define I2C_BUS_ATTACH 16 +#define I2C_BUS_ATTACH 15 // > i2cRead/deviceId/deviceAddress/size -#define I2C_READ 17 +#define I2C_READ 16 // > i2cWrite/deviceId/deviceAddress/[] data -#define I2C_WRITE 18 +#define I2C_WRITE 17 // > i2cWriteRead/deviceId/deviceAddress/readSize/writeValue -#define I2C_WRITE_READ 19 +#define I2C_WRITE_READ 18 // < publishI2cData/deviceId/[] data -#define PUBLISH_I2C_DATA 20 +#define PUBLISH_I2C_DATA 19 // > neoPixelAttach/deviceId/pin/b32 numPixels -#define NEO_PIXEL_ATTACH 21 +#define NEO_PIXEL_ATTACH 20 // > neoPixelSetAnimation/deviceId/animation/red/green/blue/b16 speed -#define NEO_PIXEL_SET_ANIMATION 22 +#define NEO_PIXEL_SET_ANIMATION 21 // > neoPixelWriteMatrix/deviceId/[] buffer -#define NEO_PIXEL_WRITE_MATRIX 23 +#define NEO_PIXEL_WRITE_MATRIX 22 // > analogWrite/pin/value -#define ANALOG_WRITE 24 +#define ANALOG_WRITE 23 // > digitalWrite/pin/value -#define DIGITAL_WRITE 25 +#define DIGITAL_WRITE 24 // > disablePin/pin -#define DISABLE_PIN 26 +#define DISABLE_PIN 25 // > disablePins -#define DISABLE_PINS 27 +#define DISABLE_PINS 26 // > pinMode/pin/mode -#define PIN_MODE 28 +#define PIN_MODE 27 // < publishDebug/str debugMsg -#define PUBLISH_DEBUG 29 +#define PUBLISH_DEBUG 28 // < publishPinArray/[] data -#define PUBLISH_PIN_ARRAY 30 +#define PUBLISH_PIN_ARRAY 29 // > setTrigger/pin/triggerValue -#define SET_TRIGGER 31 +#define SET_TRIGGER 30 // > setDebounce/pin/delay -#define SET_DEBOUNCE 32 +#define SET_DEBOUNCE 31 // > servoAttach/deviceId/pin/b16 initPos/b16 initVelocity/str name -#define SERVO_ATTACH 33 +#define SERVO_ATTACH 32 // > servoAttachPin/deviceId/pin -#define SERVO_ATTACH_PIN 34 +#define SERVO_ATTACH_PIN 33 // > servoDetachPin/deviceId -#define SERVO_DETACH_PIN 35 -// > servoSetMaxVelocity/deviceId/b16 maxVelocity -#define SERVO_SET_MAX_VELOCITY 36 +#define SERVO_DETACH_PIN 34 // > servoSetVelocity/deviceId/b16 velocity -#define SERVO_SET_VELOCITY 37 +#define SERVO_SET_VELOCITY 35 // > servoSweepStart/deviceId/min/max/step -#define SERVO_SWEEP_START 38 +#define SERVO_SWEEP_START 36 // > servoSweepStop/deviceId -#define SERVO_SWEEP_STOP 39 +#define SERVO_SWEEP_STOP 37 // > servoMoveToMicroseconds/deviceId/b16 target -#define SERVO_MOVE_TO_MICROSECONDS 40 +#define SERVO_MOVE_TO_MICROSECONDS 38 // > servoSetAcceleration/deviceId/b16 acceleration -#define SERVO_SET_ACCELERATION 41 +#define SERVO_SET_ACCELERATION 39 +// < publishServoEvent/deviceId/eventType/currentPos/targetPos +#define PUBLISH_SERVO_EVENT 40 // > serialAttach/deviceId/relayPin -#define SERIAL_ATTACH 42 +#define SERIAL_ATTACH 41 // > serialRelay/deviceId/[] data -#define SERIAL_RELAY 43 +#define SERIAL_RELAY 42 // < publishSerialData/deviceId/[] data -#define PUBLISH_SERIAL_DATA 44 +#define PUBLISH_SERIAL_DATA 43 // > ultrasonicSensorAttach/deviceId/triggerPin/echoPin -#define ULTRASONIC_SENSOR_ATTACH 45 +#define ULTRASONIC_SENSOR_ATTACH 44 // > ultrasonicSensorStartRanging/deviceId -#define ULTRASONIC_SENSOR_START_RANGING 46 +#define ULTRASONIC_SENSOR_START_RANGING 45 // > ultrasonicSensorStopRanging/deviceId -#define ULTRASONIC_SENSOR_STOP_RANGING 47 +#define ULTRASONIC_SENSOR_STOP_RANGING 46 // < publishUltrasonicSensorData/deviceId/b16 echoTime -#define PUBLISH_ULTRASONIC_SENSOR_DATA 48 +#define PUBLISH_ULTRASONIC_SENSOR_DATA 47 diff --git a/src/resource/Arduino/MRLComm/Device.h b/src/resource/Arduino/MRLComm/Device.h index 698b2aac13..da4eaa4ee9 100644 --- a/src/resource/Arduino/MRLComm/Device.h +++ b/src/resource/Arduino/MRLComm/Device.h @@ -31,9 +31,10 @@ class Device { int type; // what type of device is this? int state; // state - single at the moment to handle all the finite states of the sensors (todo maybe this moves into the subclasses?) virtual void update() {}; // all devices must implement this to update their state. + virtual void onDisconnect() {}; //all devices must implement this to react when the communication with MRL is broken Msg* msg; // Msg is the generated interface for all communication }; -#endif +#endif diff --git a/src/resource/Arduino/MRLComm/MrlComm.cpp b/src/resource/Arduino/MRLComm/MrlComm.cpp index 13d9cf8d31..6f9fb6b600 100644 --- a/src/resource/Arduino/MRLComm/MrlComm.cpp +++ b/src/resource/Arduino/MRLComm/MrlComm.cpp @@ -109,9 +109,10 @@ void MrlComm::update() { // until it is reset after sending publishBoardInfo ++loopCount; unsigned long now = millis(); - if ((now - lastHeartbeatUpdate > 1000)) { - // softReset(); - not ready yet to commit to resetting + if ((now - lastHeartbeatUpdate > 1000) && heartbeatEnabled) { + onDisconnect(); lastHeartbeatUpdate = now; + heartbeatEnabled = false; return; } @@ -169,7 +170,7 @@ void MrlComm::enableAck(boolean enabled) { ackEnabled = enabled; } -boolean MrlComm::readMsg() { +bool MrlComm::readMsg() { return msg->readMsg(); } @@ -242,12 +243,6 @@ void MrlComm::echo(float myFloat, byte myByte, float mySecondFloat) { msg->publishEcho(myFloat, myByte, mySecondFloat); } -// > controllerAttach/serialPort -// TODO - talk to calamity -void MrlComm::controllerAttach(byte serialPort) { - msg->publishDebug(String("controllerAttach " + String(serialPort))); -} - // > customMsg/[] msg // from PC --> loads customMsg buffer void MrlComm::customMsg(byte msgSize, const byte*msg) { @@ -383,12 +378,6 @@ void MrlComm::servoDetachPin(byte deviceId) { servo->detachPin(); } -// > servoSetMaxVelocity/deviceId/b16 maxVelocity -void MrlComm::servoSetMaxVelocity(byte deviceId, int maxVelocity) { - MrlServo* servo = (MrlServo*) getDevice(deviceId); - servo->setMaxVelocity(maxVelocity); -} - // > servoSetVelocity/deviceId/b16 velocity void MrlComm::servoSetVelocity(byte deviceId, int velocity) { MrlServo* servo = (MrlServo*) getDevice(deviceId); @@ -471,6 +460,7 @@ void MrlComm::softReset() { customMsgBuffer[i] = 0; } customMsgSize = 0; + heartbeatEnabled = true; } // > ultrasonicSensorAttach/deviceId/triggerPin/echoPin @@ -503,3 +493,14 @@ unsigned int MrlComm::getCustomMsg() { customMsgSize--; return retval; } + + void MrlComm::onDisconnect() { + ListNode* node = deviceList.getRoot(); + // iterate through our device list and call update on them. + while (node != NULL) { + node->data->onDisconnect(); + node = node->next; + } + boardStatusEnabled = false; + } + diff --git a/src/resource/Arduino/MRLComm/MrlComm.h b/src/resource/Arduino/MRLComm/MrlComm.h index 5cf44bfc02..8ac24bb042 100644 --- a/src/resource/Arduino/MRLComm/MrlComm.h +++ b/src/resource/Arduino/MRLComm/MrlComm.h @@ -53,6 +53,8 @@ class MrlComm{ // handles all messages to and from pc Msg* msg; + bool heartbeatEnabled; + public: // utility methods int getFreeRam(); @@ -80,8 +82,6 @@ class MrlComm{ void enableAck( boolean enabled); // > echo/f32 myFloat/myByte/f32 secondFloat void echo( float myFloat, byte myByte, float secondFloat); - // > controllerAttach/serialPort - void controllerAttach( byte serialPort); // > customMsg/[] msg void customMsg( byte msgSize, const byte*msg); // > deviceDetach/deviceId @@ -114,8 +114,6 @@ class MrlComm{ void servoAttachPin( byte deviceId, byte pin); // > servoDetachPin/deviceId void servoDetachPin( byte deviceId); - // > servoSetMaxVelocity/deviceId/b16 maxVelocity - void servoSetMaxVelocity( byte deviceId, int maxVelocity); // > servoSetVelocity/deviceId/b16 velocity void servoSetVelocity( byte deviceId, int velocity); // > servoSweepStart/deviceId/min/max/step @@ -153,6 +151,7 @@ class MrlComm{ int getCustomMsgSize(); void begin(HardwareSerial& serial); bool readMsg(); + void onDisconnect(); }; -#endif +#endif diff --git a/src/resource/Arduino/MRLComm/MrlI2cBus.cpp b/src/resource/Arduino/MRLComm/MrlI2cBus.cpp index f49b3e9d33..6f2dc4f9aa 100644 --- a/src/resource/Arduino/MRLComm/MrlI2cBus.cpp +++ b/src/resource/Arduino/MRLComm/MrlI2cBus.cpp @@ -65,4 +65,4 @@ void MrlI2CBus::i2cWriteRead(byte deviceAddress, byte readSize, byte writeValue) void MrlI2CBus::update() { //Nothing to do } - + diff --git a/src/resource/Arduino/MRLComm/MrlNeopixel.cpp b/src/resource/Arduino/MRLComm/MrlNeopixel.cpp index 8871678122..687174dd4c 100644 --- a/src/resource/Arduino/MRLComm/MrlNeopixel.cpp +++ b/src/resource/Arduino/MRLComm/MrlNeopixel.cpp @@ -588,7 +588,7 @@ void MrlNeopixel::show() { if ((lastShow + (RES / 1000UL)) > millis()) // if ((lastShow + RES) > millis()) return; - for (unsigned int p = 1; p <= numPixel; p++) { + for (int p = 1; p <= numPixel; p++) { sendPixel(pixels[p]); } lastShow = millis(); @@ -667,7 +667,7 @@ void MrlNeopixel::setAnimation ( byte animation, byte red, byte green, byte b } void MrlNeopixel::animationStop() { - for (unsigned int i = 1; i <= numPixel; i++) { + for (int i = 1; i <= numPixel; i++) { pixels[i].clearPixel(); } _animation = NEOPIXEL_ANIMATION_NO_ANIMATION; @@ -693,7 +693,7 @@ void MrlNeopixel::animationColorWipe() { void MrlNeopixel::animationLarsonScanner() { if (!((_count++) % _speed)) { - for (unsigned int i = 1; i <= numPixel; i++) { + for (int i = 1; i <= numPixel; i++) { pixels[i].clearPixel(); } int pos = _pos; @@ -722,7 +722,7 @@ void MrlNeopixel::animationLarsonScanner() { void MrlNeopixel::animationTheaterChase() { if (!((_count++) % _speed)) { - for (unsigned int i = 0; i <= numPixel; i += 3) { + for (int i = 0; i <= numPixel; i += 3) { if (i + _pos <= numPixel) { pixels[i + _pos].clearPixel(); } @@ -730,7 +730,7 @@ void MrlNeopixel::animationTheaterChase() { _pos++; if (_pos >= 4) _pos = 1; - for (unsigned int i = 0; i <= numPixel; i += 3) { + for (int i = 0; i <= numPixel; i += 3) { if (i + _pos <= numPixel) { pixels[i + _pos].setPixel(_baseColorRed, _baseColorGreen, _baseColorBlue); @@ -756,7 +756,7 @@ void MrlNeopixel::animationWheel(unsigned char WheelPos, Pixel& pixel) { void MrlNeopixel::animationTheaterChaseRainbow() { if (!((_count++) % _speed)) { - for (unsigned int i = 0; i <= numPixel; i += 3) { + for (int i = 0; i <= numPixel; i += 3) { if (i + _pos <= numPixel) { pixels[i + _pos].clearPixel(); } @@ -764,7 +764,7 @@ void MrlNeopixel::animationTheaterChaseRainbow() { _pos++; if (_pos >= 4) _pos = 1; - for (unsigned int i = 0; i <= numPixel; i += 3) { + for (int i = 0; i <= numPixel; i += 3) { if (i + _pos <= numPixel) { animationWheel((_baseColorRed + i), pixels[i + _pos]); } @@ -777,7 +777,7 @@ void MrlNeopixel::animationTheaterChaseRainbow() { void MrlNeopixel::animationRainbow() { if (!((_count++) % _speed)) { - for (unsigned int i = 0; i <= numPixel; i++) { + for (int i = 0; i <= numPixel; i++) { animationWheel((_baseColorRed + i), pixels[i]); } _baseColorRed++; @@ -788,7 +788,7 @@ void MrlNeopixel::animationRainbow() { void MrlNeopixel::animationRainbowCycle() { if (!((_count++) % _speed)) { - for (unsigned int i = 0; i <= numPixel; i++) { + for (int i = 0; i <= numPixel; i++) { animationWheel((i * 256 / numPixel) + _baseColorRed, pixels[i]); } _baseColorRed++; @@ -836,7 +836,7 @@ void MrlNeopixel::animationIronman() { _alpha = 100; _dir = -1; } - for (unsigned int i = 1; i <= numPixel; i++) { + for (int i = 1; i <= numPixel; i++) { pixels[i].setPixel((_baseColorRed * _alpha) / 100, (_baseColorGreen * _alpha) / 100, (_baseColorBlue * _alpha) / 100); @@ -846,3 +846,8 @@ void MrlNeopixel::animationIronman() { newData = true; } +void MrlNeopixel::onDisconnect() { + setAnimation(NEOPIXEL_ANIMATION_THEATER_CHASE, 255, 0, 0, 1); + +} + diff --git a/src/resource/Arduino/MRLComm/MrlNeopixel.h b/src/resource/Arduino/MRLComm/MrlNeopixel.h index 5c6e9154fa..7be6a1ff1d 100644 --- a/src/resource/Arduino/MRLComm/MrlNeopixel.h +++ b/src/resource/Arduino/MRLComm/MrlNeopixel.h @@ -69,7 +69,7 @@ struct Pixel{ class MrlNeopixel:public Device{ private: - unsigned int numPixel; + int numPixel; Pixel* pixels; uint8_t bitmask; unsigned long lastShow; @@ -119,8 +119,9 @@ class MrlNeopixel:public Device{ void animationRainbowCycle(); void animationFlashRandom(); void animationIronman(); + void onDisconnect(); }; -#endif +#endif diff --git a/src/resource/Arduino/MRLComm/MrlServo.cpp b/src/resource/Arduino/MRLComm/MrlServo.cpp index c26069270a..d8b5fb123c 100644 --- a/src/resource/Arduino/MRLComm/MrlServo.cpp +++ b/src/resource/Arduino/MRLComm/MrlServo.cpp @@ -32,15 +32,15 @@ bool MrlServo::attach(byte pin, int initPosUs, int initVelocity){ targetPosUs = initPosUs; velocity = initVelocity; servo->attach(pin); + publishServoEvent(SERVO_EVENT_STOPPED); return true; } // This method is equivalent to Arduino's Servo.attach(pin) - (no pos) void MrlServo::attachPin(int pin){ this->pin = pin; - servo->attach(pin); servo->writeMicroseconds(currentPosUs); //return to it's last know state (may be 0 if currentPosUs is not set) - // TODO-KW: we should always have a moveTo for safety, o/w we have no idea what angle we're going to start up at.. maybe + servo->attach(pin); } void MrlServo::detachPin(){ @@ -76,11 +76,20 @@ void MrlServo::update() { else if (currentPosUs > targetPosUs) { step *=-1; } + int previousCurrentPosUs = (int)currentPosUs; currentPosUs += step; if ((step > 0.0 && (int)currentPosUs > targetPosUs) || (step < 0.0 && (int)currentPosUs < targetPosUs)) { currentPosUs = targetPosUs; } - servo->writeMicroseconds((int)currentPosUs); + if (previousCurrentPosUs != (int)currentPosUs) { + servo->writeMicroseconds((int)currentPosUs); + if ((int)currentPosUs == targetPosUs) { + publishServoEvent(SERVO_EVENT_STOPPED); + } + else { + publishServoEvent(SERVO_EVENT_POSITION_UPDATE); + } + } } else { if (isSweeping) { @@ -107,16 +116,9 @@ void MrlServo::moveToMicroseconds(int position) { isMoving = true; lastUpdate = millis(); moveStart = lastUpdate; + publishServoEvent(SERVO_EVENT_STOPPED); } -/* -void MrlServo::servoWriteMicroseconds(int position) { - if (servo) { - servo->writeMicroseconds(position); - } -} -*/ - void MrlServo::startSweep(int minUs, int maxUs, int step) { this->minUs = minUs; this->maxUs = maxUs; @@ -131,9 +133,6 @@ void MrlServo::stopSweep() { isSweeping = false; } -void MrlServo::setMaxVelocity(unsigned int velocity){ - maxVelocity = velocity; -} void MrlServo::setVelocity(int velocity) { this->velocity = velocity; @@ -142,3 +141,8 @@ void MrlServo::setVelocity(int velocity) { void MrlServo::setAcceleration(int acceleration) { this->acceleration = acceleration; } + +void MrlServo::publishServoEvent(int type) { + msg->publishServoEvent(id, type, (int)currentPosUs, targetPosUs); +} + diff --git a/src/resource/Arduino/MRLComm/MrlServo.h b/src/resource/Arduino/MRLComm/MrlServo.h index 0a0776e2bb..63785c23b3 100644 --- a/src/resource/Arduino/MRLComm/MrlServo.h +++ b/src/resource/Arduino/MRLComm/MrlServo.h @@ -24,7 +24,6 @@ class MrlServo : public Device { unsigned long lastUpdate; int velocity; // in deg/sec | velocity < 0 == no speed control int sweepStep; - unsigned int maxVelocity; int acceleration; unsigned long moveStart; @@ -44,6 +43,7 @@ class MrlServo : public Device { void setMaxVelocity(unsigned int velocity); void setVelocity(int velocity); void setAcceleration(int acceleration); + void publishServoEvent(int type); }; -#endif +#endif diff --git a/src/resource/Arduino/MRLComm/Msg.cpp b/src/resource/Arduino/MRLComm/Msg.cpp index 4b02b42bf6..5600acac04 100644 --- a/src/resource/Arduino/MRLComm/Msg.cpp +++ b/src/resource/Arduino/MRLComm/Msg.cpp @@ -72,8 +72,6 @@ Msg* Msg::getInstance() { void enableAck( boolean enabled); // > echo/f32 myFloat/myByte/f32 secondFloat void echo( float myFloat, byte myByte, float secondFloat); - // > controllerAttach/serialPort - void controllerAttach( byte serialPort); // > customMsg/[] msg void customMsg( byte msgSize, const byte*msg); // > deviceDetach/deviceId @@ -106,8 +104,6 @@ Msg* Msg::getInstance() { void servoAttachPin( byte deviceId, byte pin); // > servoDetachPin/deviceId void servoDetachPin( byte deviceId); - // > servoSetMaxVelocity/deviceId/b16 maxVelocity - void servoSetMaxVelocity( byte deviceId, int maxVelocity); // > servoSetVelocity/deviceId/b16 velocity void servoSetVelocity( byte deviceId, int velocity); // > servoSweepStart/deviceId/min/max/step @@ -178,7 +174,7 @@ void Msg::publishEcho( float myFloat, byte myByte, float secondFloat) { void Msg::publishCustomMsg(const byte* msg, byte msgSize) { write(MAGIC_NUMBER); write(1 + (1 + msgSize)); // size - write(PUBLISH_CUSTOM_MSG); // msgType = 14 + write(PUBLISH_CUSTOM_MSG); // msgType = 13 write((byte*)msg, msgSize); flush(); reset(); @@ -187,7 +183,7 @@ void Msg::publishCustomMsg(const byte* msg, byte msgSize) { void Msg::publishI2cData( byte deviceId, const byte* data, byte dataSize) { write(MAGIC_NUMBER); write(1 + 1 + (1 + dataSize)); // size - write(PUBLISH_I2C_DATA); // msgType = 20 + write(PUBLISH_I2C_DATA); // msgType = 19 write(deviceId); write((byte*)data, dataSize); flush(); @@ -197,7 +193,7 @@ void Msg::publishI2cData( byte deviceId, const byte* data, byte dataSize) { void Msg::publishDebug(const char* debugMsg, byte debugMsgSize) { write(MAGIC_NUMBER); write(1 + (1 + debugMsgSize)); // size - write(PUBLISH_DEBUG); // msgType = 29 + write(PUBLISH_DEBUG); // msgType = 28 write((byte*)debugMsg, debugMsgSize); flush(); reset(); @@ -206,16 +202,28 @@ void Msg::publishDebug(const char* debugMsg, byte debugMsgSize) { void Msg::publishPinArray(const byte* data, byte dataSize) { write(MAGIC_NUMBER); write(1 + (1 + dataSize)); // size - write(PUBLISH_PIN_ARRAY); // msgType = 30 + write(PUBLISH_PIN_ARRAY); // msgType = 29 write((byte*)data, dataSize); flush(); reset(); } +void Msg::publishServoEvent( byte deviceId, byte eventType, byte currentPos, byte targetPos) { + write(MAGIC_NUMBER); + write(1 + 1 + 1 + 1 + 1); // size + write(PUBLISH_SERVO_EVENT); // msgType = 40 + write(deviceId); + write(eventType); + write(currentPos); + write(targetPos); + flush(); + reset(); +} + void Msg::publishSerialData( byte deviceId, const byte* data, byte dataSize) { write(MAGIC_NUMBER); write(1 + 1 + (1 + dataSize)); // size - write(PUBLISH_SERIAL_DATA); // msgType = 44 + write(PUBLISH_SERIAL_DATA); // msgType = 43 write(deviceId); write((byte*)data, dataSize); flush(); @@ -225,7 +233,7 @@ void Msg::publishSerialData( byte deviceId, const byte* data, byte dataSize) { void Msg::publishUltrasonicSensorData( byte deviceId, int echoTime) { write(MAGIC_NUMBER); write(1 + 1 + 2); // size - write(PUBLISH_ULTRASONIC_SENSOR_DATA); // msgType = 48 + write(PUBLISH_ULTRASONIC_SENSOR_DATA); // msgType = 47 write(deviceId); writeb16(echoTime); flush(); @@ -285,12 +293,6 @@ void Msg::processCommand() { mrlComm->echo( myFloat, myByte, secondFloat); break; } - case CONTROLLER_ATTACH: { // controllerAttach - byte serialPort = ioCmd[startPos+1]; // bu8 - startPos += 1; - mrlComm->controllerAttach( serialPort); - break; - } case CUSTOM_MSG: { // customMsg const byte* msg = ioCmd+startPos+2; byte msgSize = ioCmd[startPos+1]; @@ -459,14 +461,6 @@ void Msg::processCommand() { mrlComm->servoDetachPin( deviceId); break; } - case SERVO_SET_MAX_VELOCITY: { // servoSetMaxVelocity - byte deviceId = ioCmd[startPos+1]; // bu8 - startPos += 1; - int maxVelocity = b16(ioCmd, startPos+1); - startPos += 2; //b16 - mrlComm->servoSetMaxVelocity( deviceId, maxVelocity); - break; - } case SERVO_SET_VELOCITY: { // servoSetVelocity byte deviceId = ioCmd[startPos+1]; // bu8 startPos += 1; diff --git a/src/resource/Arduino/MRLComm/Msg.h b/src/resource/Arduino/MRLComm/Msg.h index a9a2dc4e16..67b34d2dd1 100644 --- a/src/resource/Arduino/MRLComm/Msg.h +++ b/src/resource/Arduino/MRLComm/Msg.h @@ -100,6 +100,7 @@ class Msg { void publishI2cData( byte deviceId, const byte* data, byte dataSize); void publishDebug(const char* debugMsg, byte debugMsgSize); void publishPinArray(const byte* data, byte dataSize); + void publishServoEvent( byte deviceId, byte eventType, byte currentPos, byte targetPos); void publishSerialData( byte deviceId, const byte* data, byte dataSize); void publishUltrasonicSensorData( byte deviceId, int echoTime); diff --git a/src/resource/Arduino/generate/arduinoMsgs.schema b/src/resource/Arduino/generate/arduinoMsgs.schema index 8a434908e2..5ae6135d04 100644 --- a/src/resource/Arduino/generate/arduinoMsgs.schema +++ b/src/resource/Arduino/generate/arduinoMsgs.schema @@ -54,9 +54,6 @@ > echo/f32 myFloat/myByte/f32 secondFloat < publishEcho/f32 myFloat/myByte/f32 secondFloat -# calamity we should discuss -> controllerAttach/serialPort - # Custom Messages > customMsg/[] msg < publishCustomMsg/[] msg @@ -107,7 +104,6 @@ > servoAttach/deviceId/pin/b16 initPos/b16 initVelocity/str name > servoAttachPin/deviceId/pin > servoDetachPin/deviceId -> servoSetMaxVelocity/deviceId/b16 maxVelocity > servoSetVelocity/deviceId/b16 velocity > servoSweepStart/deviceId/min/max/step > servoSweepStop/deviceId @@ -117,7 +113,7 @@ # > servoWriteMicroseconds/deviceId/b16 ms > servoSetAcceleration/deviceId/b16 acceleration # not used as originally intended - as event information -# < publishServoEvent/deviceId/eventType/currentPos/targetPos +< publishServoEvent/deviceId/eventType/currentPos/targetPos # Serial - for relaying serial data to a different pin # I suspect this should model the same pattern as Servo From a3f33c60954b9857b954317ba2914ad729faa3d2 Mon Sep 17 00:00:00 2001 From: kwatters Date: Tue, 31 Jan 2017 15:54:27 -0500 Subject: [PATCH 4/8] fixing a concurrent modification error on the registry iterator. --- src/org/myrobotlab/service/Runtime.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/org/myrobotlab/service/Runtime.java b/src/org/myrobotlab/service/Runtime.java index cfe0345a0d..4c7ea7376c 100644 --- a/src/org/myrobotlab/service/Runtime.java +++ b/src/org/myrobotlab/service/Runtime.java @@ -917,7 +917,7 @@ public static List getServices() { * @param interfaze * @return services which match */ - public static ArrayList getServicesFromInterface(Class interfaze) { + public static synchronized ArrayList getServicesFromInterface(Class interfaze) { ArrayList ret = new ArrayList(); Iterator it = registry.keySet().iterator(); From 336d4ea683eba232db89bbefb1bcb594a71eeee7 Mon Sep 17 00:00:00 2001 From: kwatters Date: Tue, 31 Jan 2017 16:49:02 -0500 Subject: [PATCH 5/8] adding some configuration options to the opencv floor finder filter. --- .../opencv/OpenCVFilterFloorFinder.java | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/org/myrobotlab/opencv/OpenCVFilterFloorFinder.java b/src/org/myrobotlab/opencv/OpenCVFilterFloorFinder.java index 7ba5c49639..5e7583f2aa 100644 --- a/src/org/myrobotlab/opencv/OpenCVFilterFloorFinder.java +++ b/src/org/myrobotlab/opencv/OpenCVFilterFloorFinder.java @@ -46,9 +46,9 @@ public class OpenCVFilterFloorFinder extends OpenCVFilter { transient IplImage buffer = null; CvPoint startPoint = cvPoint(180, 120); - CvScalar fillColor = cvScalar(255.0, 0.0, 0.0, 1.0); - CvScalar lo_diff = CV_RGB(20.0, 20.0, 20.0);// cvScalar(20, 0.0, 0.5, 1.0); - CvScalar up_diff = CV_RGB(20.0, 20.0, 20.0); + CvScalar fillColor = cvScalar(255.0, 255.0, 255.0, 1.0); + CvScalar lo_diff = CV_RGB(2, 2, 2);// cvScalar(20, 0.0, 0.5, 1.0); + CvScalar up_diff = CV_RGB(2, 2, 2); public OpenCVFilterFloorFinder() { super(); @@ -61,26 +61,26 @@ public OpenCVFilterFloorFinder(String name) { @Override public void imageChanged(IplImage image) { // TODO Auto-generated method stub + } + public void updateLoDiff(int r, int g, int b) { + lo_diff = CV_RGB(r, g, b); } + public void updateUpDiff(int r, int g, int b) { + up_diff = CV_RGB(r, g, b); + } + + public void updateFillColor(int r, int g, int b) { + fillColor = cvScalar(r, g, b, 1.0); + } + @Override public IplImage process(IplImage image, OpenCVData data) { - // if (startPoint == null) - { - startPoint = cvPoint(image.width() / 2, image.height() - 4); - } - - fillColor = cvScalar(255.0, 255.0, 255.0, 1.0); - - lo_diff = CV_RGB(1, 12, 13);// cvScalar(20, 0.0, 0.5, 1.0); - up_diff = CV_RGB(1, 12, 13); - + // if (startPoint == null) { + startPoint = cvPoint(image.width() / 2, image.height() - 4); + //} cvFloodFill(image, startPoint, fillColor, lo_diff, up_diff, null, 4, null); - - fillColor = cvScalar(0.0, 255.0, 0.0, 1.0); - cvDrawRect(image, startPoint, startPoint, fillColor, 2, 1, 0); - return image; } From cd5c253370704af01e6c88dd425d8aa5c9536545 Mon Sep 17 00:00:00 2001 From: Christian Date: Wed, 1 Feb 2017 18:34:53 -0500 Subject: [PATCH 6/8] Websocket in MrlComm --- .../Arduino/MRLComm/ArduinoMsgCodec.h | 6 +- src/resource/Arduino/MRLComm/MRLComm.ino | 21 +++ src/resource/Arduino/MRLComm/MrlComm.cpp | 21 ++- src/resource/Arduino/MRLComm/MrlComm.h | 10 ++ src/resource/Arduino/MRLComm/MrlI2cBus.cpp | 4 +- src/resource/Arduino/MRLComm/MrlNeopixel.cpp | 4 +- src/resource/Arduino/MRLComm/MrlNeopixel.h | 7 +- src/resource/Arduino/MRLComm/MrlWS.cpp | 125 ++++++++++++++++++ src/resource/Arduino/MRLComm/MrlWS.h | 32 +++++ src/resource/Arduino/MRLComm/Msg.cpp | 18 ++- src/resource/Arduino/MRLComm/Msg.h | 17 ++- 11 files changed, 252 insertions(+), 13 deletions(-) create mode 100644 src/resource/Arduino/MRLComm/MrlWS.cpp create mode 100644 src/resource/Arduino/MRLComm/MrlWS.h diff --git a/src/resource/Arduino/MRLComm/ArduinoMsgCodec.h b/src/resource/Arduino/MRLComm/ArduinoMsgCodec.h index c5afb03142..10a69926eb 100644 --- a/src/resource/Arduino/MRLComm/ArduinoMsgCodec.h +++ b/src/resource/Arduino/MRLComm/ArduinoMsgCodec.h @@ -130,6 +130,7 @@ #define BOARD_TYPE_ID_MEGA_ADK 3 #define BOARD_TYPE_ID_NANO 4 #define BOARD_TYPE_ID_PRO_MINI 5 +#define BOARD_TYPE_ID_ESP8266 6 #if defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_AVR_ADK) #define BOARD BOARD_TYPE_ID_MEGA @@ -141,8 +142,11 @@ #define BOARD BOARD_TYPE_ID_NANO #elif defined(ARDUINO_AVR_PRO) #define BOARD BOARD_TYPE_ID_PRO_MINI +#elif defined(ARDUINO_ESP8266_ESP12) + #define BOARD BOARD_TYPE_ID_ESP8266 + //#define ESP8266 #else #define BOARD BOARD_TYPE_ID_UNKNOWN #endif -#endif +#endif diff --git a/src/resource/Arduino/MRLComm/MRLComm.ino b/src/resource/Arduino/MRLComm/MRLComm.ino index 14ae8a4ec0..3ce97adb7e 100644 --- a/src/resource/Arduino/MRLComm/MRLComm.ino +++ b/src/resource/Arduino/MRLComm/MRLComm.ino @@ -51,12 +51,23 @@ // Included as a 3rd party arduino library from here: https://github.com/ivanseidel/LinkedList/ #include "LinkedList.h" #include "MrlComm.h" +#include +#if defined(ESP8266) + #include +#endif /*********************************************************************** * GLOBAL VARIABLES * TODO - work on reducing globals and pass as parameters */ MrlComm mrlComm; +#if defined(ESP8266) + WebSocketsServer webSocket = WebSocketsServer(81); + + void webSocketEvent(unsigned char num, WStype_t type, unsigned char* payload, unsigned int lenght){ + mrlComm.webSocketEvent(num, type, payload, lenght); + } +#endif /*********************************************************************** * STANDARD ARDUINO BEGIN * setup() is called when the serial port is opened unless you hack the @@ -66,10 +77,17 @@ MrlComm mrlComm; */ void setup() { + Wire.begin(); + Serial.begin(115200); // start with standard serial & rate +#if defined(ESP8266) + webSocket.onEvent(webSocketEvent); + mrlComm.begin(webSocket); +#else mrlComm.begin(Serial); +#endif } /** @@ -80,6 +98,7 @@ void setup() { void loop() { // get a command and process it from // the serial port (if available.) +// wdt_disable(); if (mrlComm.readMsg()) { mrlComm.processCommand(); } @@ -88,5 +107,7 @@ void loop() { // send back load time and memory // driven by getBoardInfo now !!! // mrlComm.publishBoardStatus(); + webSocket.loop(); } // end of big loop + diff --git a/src/resource/Arduino/MRLComm/MrlComm.cpp b/src/resource/Arduino/MRLComm/MrlComm.cpp index 6f9fb6b600..7b7018078e 100644 --- a/src/resource/Arduino/MRLComm/MrlComm.cpp +++ b/src/resource/Arduino/MRLComm/MrlComm.cpp @@ -38,9 +38,14 @@ MrlComm::~MrlComm() { int MrlComm::getFreeRam() { // KW: In the future the arduino might have more than an 32/64k of ram. an int might not be enough here to return. + #ifndef ESP8266 extern int __heap_start, *__brkval; int v; return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); + #else + return system_get_free_heap_size(); + #endif + return 0; } /** @@ -174,6 +179,16 @@ bool MrlComm::readMsg() { return msg->readMsg(); } +#if defined(ESP8266) +void MrlComm::begin(WebSocketsServer& wsServer) { + msg->begin(wsServer); +} + +void MrlComm::webSocketEvent(unsigned char num, WStype_t type, unsigned char* payload, unsigned int lenght) { + msg->webSocketEvent(num, type, payload, lenght); +} +#else + void MrlComm::begin(HardwareSerial& serial) { // TODO: the arduino service might get a few garbage bytes before we're able @@ -199,6 +214,8 @@ void MrlComm::begin(HardwareSerial& serial) { } } +#endif + /*********************************************************************** * PUBLISH_BOARD_INFO This function updates the average time it took to run * the main loop and reports it back with a publishBoardStatus MRLComm message @@ -216,7 +233,9 @@ void MrlComm::publishBoardInfo(){ } long now = micros(); - msg->publishBoardInfo(MRLCOMM_VERSION, BOARD, (int)((now - lastBoardInfoUs)/loopCount), getFreeRam(), pinList.size(), deviceSummary, sizeof(deviceSummary)); + int load = (now - lastBoardInfoUs)/loopCount; + //msg->publishBoardInfo(MRLCOMM_VERSION, BOARD, (int)((now - lastBoardInfoUs)/loopCount), getFreeRam(), pinList.size(), deviceSummary, sizeof(deviceSummary)); + msg->publishBoardInfo(MRLCOMM_VERSION, BOARD, load, getFreeRam(), pinList.size(), deviceSummary, sizeof(deviceSummary)); lastBoardInfoUs = now; loopCount = 0; } diff --git a/src/resource/Arduino/MRLComm/MrlComm.h b/src/resource/Arduino/MRLComm/MrlComm.h index 8ac24bb042..575ae15a37 100644 --- a/src/resource/Arduino/MRLComm/MrlComm.h +++ b/src/resource/Arduino/MRLComm/MrlComm.h @@ -3,6 +3,12 @@ #include "ArduinoMsgCodec.h" #include "MrlSerialRelay.h" +#if defined(ESP8266) + #include + extern "C" { + #include "user_interface.h" + } +#endif // forward defines to break circular dependency class Device; @@ -152,6 +158,10 @@ class MrlComm{ void begin(HardwareSerial& serial); bool readMsg(); void onDisconnect(); +#if defined(ESP8266) + void begin(WebSocketsServer& wsServer); + void webSocketEvent(unsigned char num, WStype_t type, unsigned char* payload, unsigned int lenght); +#endif }; #endif diff --git a/src/resource/Arduino/MRLComm/MrlI2cBus.cpp b/src/resource/Arduino/MRLComm/MrlI2cBus.cpp index 6f2dc4f9aa..e53b0c45e4 100644 --- a/src/resource/Arduino/MRLComm/MrlI2cBus.cpp +++ b/src/resource/Arduino/MRLComm/MrlI2cBus.cpp @@ -4,11 +4,11 @@ MrlI2CBus::MrlI2CBus(int deviceId) : Device(deviceId, DEVICE_TYPE_I2C) { - if (TWCR == 0) { //// do this check so that Wire only gets initialized once +// if (TWCR == 0) { //// do this check so that Wire only gets initialized once Wire.begin(); // Force 400 KHz i2c Wire.setClock(400000L); - } +// } } bool MrlI2CBus::attach(byte bus) { diff --git a/src/resource/Arduino/MRLComm/MrlNeopixel.cpp b/src/resource/Arduino/MRLComm/MrlNeopixel.cpp index 687174dd4c..f0d7d61a86 100644 --- a/src/resource/Arduino/MRLComm/MrlNeopixel.cpp +++ b/src/resource/Arduino/MRLComm/MrlNeopixel.cpp @@ -55,7 +55,7 @@ bool MrlNeopixel::attach(byte pin, long numPixels) { //msg->publishDebug("Neopixel attached"); return true; } - +#ifndef ESP8266 inline void MrlNeopixel::sendBitB(bool bitVal) { #ifndef VIRTUAL_ARDUINO_H uint8_t bit = bitmask; @@ -562,7 +562,7 @@ inline void MrlNeopixel::sendBitD(bool bitVal) { } #endif } - +#endif inline void MrlNeopixel::sendByte(unsigned char byte) { //msg->publishDebug("MrlNeopixel.sendByte !"); for (unsigned char bit = 0; bit < 8; bit++) { diff --git a/src/resource/Arduino/MRLComm/MrlNeopixel.h b/src/resource/Arduino/MRLComm/MrlNeopixel.h index 7be6a1ff1d..e054a2f627 100644 --- a/src/resource/Arduino/MRLComm/MrlNeopixel.h +++ b/src/resource/Arduino/MRLComm/MrlNeopixel.h @@ -17,6 +17,7 @@ #define CYCLES_PER_SEC (F_CPU) #define NS_PER_CYCLE ( NS_PER_SEC / CYCLES_PER_SEC ) #define NS_TO_CYCLES(n) ( (n) / NS_PER_CYCLE ) +#ifndef ESP8266 // Arduino Mega Pins #if defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_AVR_ADK) @@ -36,7 +37,9 @@ #define digitalPinToSendBit(P,V) \ (((P) >= 0 && (P) <= 7) ? sendBitD(V) : (((P) >= 8 && (P) <= 13) ? sendBitB(V) : sendBitC(V))) #endif - +#else + #define digitalPinToSendBit(P,V) return +#endif #define NEOPIXEL_ANIMATION_NO_ANIMATION 0 #define NEOPIXEL_ANIMATION_STOP 1 #define NEOPIXEL_ANIMATION_COLOR_WIPE 2 @@ -90,6 +93,7 @@ class MrlNeopixel:public Device{ MrlNeopixel(int deviceId); ~MrlNeopixel(); bool attach(byte pin, long numPixels); +#ifndef ESP8266 inline void sendBitB(bool bitVal); inline void sendBitC(bool bitVal); inline void sendBitD(bool bitVal); @@ -102,6 +106,7 @@ class MrlNeopixel:public Device{ inline void sendBitJ(bool bitVal); inline void sendBitK(bool bitVal); inline void sendBitL(bool bitVal); +#endif #endif inline void sendByte(unsigned char byte); inline void sendPixel(Pixel p); diff --git a/src/resource/Arduino/MRLComm/MrlWS.cpp b/src/resource/Arduino/MRLComm/MrlWS.cpp new file mode 100644 index 0000000000..73d3aaa787 --- /dev/null +++ b/src/resource/Arduino/MRLComm/MrlWS.cpp @@ -0,0 +1,125 @@ +#include "MrlWS.h" +#include +#if defined(ESP8266) +#define USE_SERIAL Serial + +MrlWS::MrlWS(WebSocketsServer& wsServer) { + webSocket = &wsServer; + inputHead = 0; + inputTail = 0; + + USE_SERIAL.setDebugOutput(true); + + USE_SERIAL.println(); + USE_SERIAL.println(); + USE_SERIAL.println(); + + for(uint8_t t = 4; t > 0; t--) { + USE_SERIAL.printf("[SETUP] BOOT WAIT %d...\n", t); + USE_SERIAL.flush(); + delay(1000); +} +WiFiMulti.addAP("VIDEOTRON9077","UWMF97AMJ9T4N"); + while (WiFiMulti.run() != WL_CONNECTED) { + delay(100); + } + webSocket->begin(); + USE_SERIAL.println("webSocketStarted"); +} + +void MrlWS::write(unsigned char b) { + //unsigned char bu[1]; +// bu[0] = b; +// write(bu,1); + outBuffer.concat(String(b)); + outBuffer.concat("/"); +} + +void MrlWS::write(const unsigned char* b, int len) { + for (int i = 0; i < len; i++) { + outBuffer.concat(String(b[i])); + outBuffer.concat("/"); + } +} + +void MrlWS::flush() { + webSocket->sendTXT(num,outBuffer); + outBuffer = ""; +} + +int MrlWS::available() { + if (inputTail >= inputHead) { + //USE_SERIAL.printf("inputHead: [%u] inputTail: [%u] available + return inputTail - inputHead; + } + else { + return MAX_MSG_SIZE - inputHead + inputTail; + } +return 0; +} + +unsigned char MrlWS::read() { + //USE_SERIAL.print("read called"); + if (inputHead == inputTail) { + return 0; + } + else { + unsigned char retVal = inputBuffer[inputHead++]; + if (inputHead == MAX_MSG_SIZE) { + inputHead = 0; + } + //USE_SERIAL.println(retVal); + return retVal; + } +return 0; +} + +void MrlWS::webSocketEvent(unsigned char num, WStype_t type, unsigned char* payload, unsigned int lenght) { + USE_SERIAL.print("in web Socket Event"); + this->num = num; + switch(type) { + case WStype_DISCONNECTED: + USE_SERIAL.printf("[%u] Disconnected!\n", num); + break; + case WStype_CONNECTED: + { + IPAddress ip = webSocket->remoteIP(num); + USE_SERIAL.printf("[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload); + + // send message to client + //webSocket->sendTXT(num, "Connected"); + } + break; + case WStype_TEXT: + USE_SERIAL.printf("[%u] get Text: %s\n", num, payload); + + for (int i = 0; i < lenght; i++) { + inputBuffer[inputTail++] = payload[i]; + USE_SERIAL.println(payload[i]); + if (inputTail == MAX_MSG_SIZE) { + inputTail = 0; + } + if (inputTail == inputHead) { + break; + } + } + + // send message to client + // webSocket->sendTXT(num, "message here"); + + // send data to all connected clients + // webSocket.broadcastTXT("message here"); + break; + case WStype_BIN: + USE_SERIAL.printf("[%u] get binary lenght: %u\n", num, lenght); + hexdump(payload, lenght); + + // send message to client + // webSocket.sendBIN(num, payload, lenght); + break; + } + +} + +#endif + diff --git a/src/resource/Arduino/MRLComm/MrlWS.h b/src/resource/Arduino/MRLComm/MrlWS.h new file mode 100644 index 0000000000..9bd7192ef7 --- /dev/null +++ b/src/resource/Arduino/MRLComm/MrlWS.h @@ -0,0 +1,32 @@ +#ifndef MrlWS_h +#define MrlWS_h + +#if defined(ESP8266) + +#include +#include +#include +#include +#include "ArduinoMsgCodec.h" + +class MrlWS { + private: + ESP8266WiFiMulti WiFiMulti; + WebSocketsServer* webSocket; + unsigned char inputBuffer[MAX_MSG_SIZE]; + byte inputHead; + byte inputTail; + unsigned char num; + String outBuffer; + public: + MrlWS(WebSocketsServer& wsServer); + int available(); + void flush(); + void write(const unsigned char*, int); + void write(unsigned char); + unsigned char read(); + void webSocketEvent(unsigned char num, WStype_t type, unsigned char* payload, unsigned int lenght); +}; +#endif +#endif + diff --git a/src/resource/Arduino/MRLComm/Msg.cpp b/src/resource/Arduino/MRLComm/Msg.cpp index 5600acac04..9505acaf3f 100644 --- a/src/resource/Arduino/MRLComm/Msg.cpp +++ b/src/resource/Arduino/MRLComm/Msg.cpp @@ -1,3 +1,4 @@ +#include /** * 
  *
@@ -646,7 +647,7 @@ bool Msg::readMsg() {
 			// if received header + msg
 			if (byteCount == 2 + msgSize) {
 				// we've reach the end of the command, just return true .. we've got it
-				byteCount = 0;
+ 			byteCount = 0;
 				return true;
 			}
 		}
@@ -719,10 +720,21 @@ void Msg::flush() {
 	return serial->flush();
 }
 
+#if defined(ESP8266)
+void Msg::begin(WebSocketsServer& wsServer){
+  serial = new MrlWS(wsServer);
+}
+
+void Msg::webSocketEvent(uint8_t num, WStype_t type, uint8_t* payload, size_t lenght) {
+  serial->webSocketEvent(num, type, payload, lenght);
+}
+#else
 void Msg::begin(HardwareSerial& hardwareSerial){
-	serial = &hardwareSerial;
+  serial = &hardwareSerial;
 }
 
+#endif
+
 byte Msg::getMethod(){
 	return ioCmd[0];
-}
+}
diff --git a/src/resource/Arduino/MRLComm/Msg.h b/src/resource/Arduino/MRLComm/Msg.h
index 67b34d2dd1..7f4eb873eb 100644
--- a/src/resource/Arduino/MRLComm/Msg.h
+++ b/src/resource/Arduino/MRLComm/Msg.h
@@ -31,6 +31,9 @@
 
 #include 
 #include "ArduinoMsgCodec.h"
+#if defined(ESP8266)
+  #include "MrlWS.h"
+#endif
 
 // forward defines to break circular dependency
 class MrlComm;
@@ -51,7 +54,11 @@ class Msg {
 	byte sendBuffer[MAX_MSG_SIZE];
 
 	// serial references
+#ifndef ESP8266
 	HardwareSerial* serial;
+#else
+  MrlWS* serial;
+#endif
 
 	// heartbeat
 	bool heartbeat;
@@ -109,7 +116,6 @@ class Msg {
 	void processCommand();
 
 	// io
-	void begin(HardwareSerial& hardwareSerial);
 	void write(const unsigned char value);
 	void writebool(const bool value);
 	void writeb16(const int value);
@@ -119,7 +125,12 @@ class Msg {
 	void write(const unsigned char* buffer, int len);
 	bool readMsg();
 	byte getMethod();
-
+#if defined(ESP8266)
+  void begin(WebSocketsServer& wsServer);
+  void webSocketEvent(uint8_t num, WStype_t type, uint8_t* payload, size_t lenght);
+#else
+  void begin(HardwareSerial& hardwareSerial);
+#endif
 };
 
-#endif // Mrl_h
+#endif // Mrl_h

From c126f88a52507eb24d78f2cf429424eccdfe5e19 Mon Sep 17 00:00:00 2001
From: Christian 
Date: Wed, 1 Feb 2017 18:46:54 -0500
Subject: [PATCH 7/8] Websocket in MRLComm

---
 src/resource/Arduino/MRLComm/MrlWS.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/resource/Arduino/MRLComm/MrlWS.cpp b/src/resource/Arduino/MRLComm/MrlWS.cpp
index 73d3aaa787..647314a4a8 100644
--- a/src/resource/Arduino/MRLComm/MrlWS.cpp
+++ b/src/resource/Arduino/MRLComm/MrlWS.cpp
@@ -19,7 +19,7 @@ MrlWS::MrlWS(WebSocketsServer& wsServer) {
         USE_SERIAL.flush();
         delay(1000);
 }
-WiFiMulti.addAP("VIDEOTRON9077","UWMF97AMJ9T4N");
+WiFiMulti.addAP("SSID","PASS");
   while (WiFiMulti.run() != WL_CONNECTED) {
     delay(100);
   }

From 5e7f8cba3be72e376ea85f8b99d37762b99431ac Mon Sep 17 00:00:00 2001
From: onnerbm 
Date: Thu, 2 Feb 2017 10:21:34 +0100
Subject: [PATCH 8/8] Updated MRLComm so that it compilies on other platforms
 than the ESP8266

---
 src/resource/Arduino/MRLComm/MRLComm.ino | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/resource/Arduino/MRLComm/MRLComm.ino b/src/resource/Arduino/MRLComm/MRLComm.ino
index 3ce97adb7e..4d4174f81b 100644
--- a/src/resource/Arduino/MRLComm/MRLComm.ino
+++ b/src/resource/Arduino/MRLComm/MRLComm.ino
@@ -107,7 +107,9 @@ void loop() {
 	// send back load time and memory
         // driven by getBoardInfo now !!!
         // mrlComm.publishBoardStatus();
-  webSocket.loop();
+  #if defined(ESP8266)
+    webSocket.loop();
+  #endif
 } // end of big loop