diff --git a/arduino.c b/arduino.c new file mode 100644 index 0000000..17ee928 --- /dev/null +++ b/arduino.c @@ -0,0 +1,178 @@ +#include + +LobotServoController myse(Serial1); +int r = 1; +String incoming = ""; // for incoming serial string data + +void setup() { + pinMode(13, OUTPUT); + Serial.begin(115200); // Communicate with PI + //while(!Serial); + Serial.setTimeout(300); // Set the timeout to 300 ms. + + Serial1.begin(9600); // Communication with the Robot + while(!Serial1); + digitalWrite(13,HIGH); + + // Reset + myse.moveServo(0,1500,1000); + delay(2000); + + myse.moveServo(1,1500,1000); + delay(2000); + + myse.moveServo(2,1500,1000); + delay(2000); + + myse.moveServo(3,1500,1000); + delay(2000); + + myse.moveServo(4,1500,1000); + delay(2000); + + myse.moveServo(5,1500,1000); + delay(2000); + +// Continous run #100 action group +// myse.runActionGroup(100,0); +// delay(5000); + +// Stop the action group +// myse.stopActionGroup(); +// delay(2000); + +// Set the action speed of #100 to 200% +// myse.setActionGroupSpeed(100,200); +// delay(2000); + +// Run action group #100 for 5 times +// myse.runActionGroup(100,5); +// delay(5000); + +// myse.stopActionGroup(); +// delay(2000); + +// Move #1 servo to 1500 within 1000ms +// myse.moveServo(1,1500,1000); +// delay(2000); +// myse.moveServo(2,800,1000); +// delay(2000); + + +// Control 5 servos, transition time is 1000ms， +// - #0 servo to position of 1300 +// - #2 servo to position of 700， +// - #4 servo to position of 600， +// - #6 servo to position of 900， +// - #8 servo to position of 790 +// myse.moveServos(5,1000,0,1300,2,700,4,600,6,900,8,790); +// delay(2000); +// + +// Control two servos, transition time is 1000ms +// LobotServo servos[2]; //servo position array +// servos[0].ID = 2; //#2 servo +// servos[0].Position = 1400; //position of 1400 +// servos[1].ID = 4; //#4 servo +// servos[1].Position = 700; //position of 700 +// myse.moveServos(servos,2,1000); +} + +String findTheNthWord(String input, int n) { + String rc = ""; + input.trim(); + if (input.length() <= 0) { + return rc; + } + + int i = 0; + int currPosition = 0; + while(i++ < n) { + currPosition = input.indexOf(' ', currPosition); + if (currPosition != -1) { + currPosition = currPosition + 1; + while (input.charAt(currPosition) == ' ' && currPosition < input.length()) { + currPosition += 1; + } + } else { + break; + } + } + + if (currPosition == -1) { + return rc; + } else { + int end = input.indexOf(' ', currPosition); + if (end == -1) { + end = input.length(); + } + rc = input.substring(currPosition, end); + } + + return rc; +} + +void loop() { + // send data only when you receive data: + if (Serial.available() > 0) { + // read the incoming: + incoming = Serial.readString(); + incoming.trim(); + + //Serial.println(incoming); + if (incoming.startsWith("servo ")) { + // I.E "servo 1 1500 1000" -- Move servo 1's position to 1500 in 1 second + String servoId = findTheNthWord(incoming, 1); + servoId.trim(); + int servoIdInt = servoId.toInt(); + + String servoPosition = findTheNthWord(incoming, 2); + servoPosition.trim(); + int servoPositionInt = servoPosition.toInt(); + + String servoSpeed = findTheNthWord(incoming, 3); + servoSpeed.trim(); + int servoSpeedInt = servoSpeed.toInt(); + + if (servoId.length() > 0 && servoPosition.length() > 0 && servoSpeed.length() > 0) { + Serial.println("move servo: \n" + servoId + " " + servoPosition + " " + servoSpeed); + Serial.flush(); + + if (servoIdInt == 2 || servoIdInt == 3 || servoIdInt == 4) { + myse.moveServo(servoIdInt + 1, servoPositionInt, 2000); + delay(3000); + } else { + myse.moveServo(servoIdInt + 1, servoPositionInt, 1000); + delay(2000); + } + } + } + else if (incoming.startsWith("group ")) { + String groupAction = findTheNthWord(incoming, 1); + groupAction.trim(); + + if (groupAction.equals("start")) { + String groupId = findTheNthWord(incoming, 2); + long groupIdInt = groupId.toInt(); + myse.runActionGroup(groupIdInt, 1); //run the action group once + Serial.println("Started the group action of: " + groupId); + Serial.flush(); + delay(5000); + } else if (groupAction.equals("stop")) { + myse.stopActionGroup(); //stop running the action group. + Serial.println("Stopped the group action."); + Serial.flush(); + delay(2000); + } else { + Serial.println("Invalid group action command of: \n" + incoming); + Serial.flush(); + } + } + else { + //junk + Serial.println("Invalid command of: \n" + incoming); + Serial.flush(); + incoming = ""; + } + } +} diff --git a/flows.json b/flows.json new file mode 100644 index 0000000..0dea8d5 --- /dev/null +++ b/flows.json @@ -0,0 +1 @@ +[{"id":"4d928388.34713c","type":"ui_slider","z":"3eb563f4.6d4ebc","name":"","label":"Servo1","tooltip":"","group":"9a50a3b3.867","order":0,"width":0,"height":0,"passthru":true,"outs":"end","topic":"","min":"900","max":"1630","step":"1","x":380,"y":400,"wires":[["6138c868.f1a438"]]},{"id":"181ba5b8.02e47a","type":"debug","z":"3eb563f4.6d4ebc","name":"","active":true,"console":"false","complete":"false","x":1210,"y":560,"wires":[]},{"id":"6b8268f7.66ef78","type":"http request","z":"3eb563f4.6d4ebc","name":"Update Request","method":"POST","ret":"txt","paytoqs":false,"url":"https://api.us-east-1.mbedcloud.com/v2/device-requests/017328b713386aa761d6b4e400300000?async-id=update-position","tls":"","persist":false,"proxy":"","authType":"bearer","x":1021,"y":560,"wires":[["181ba5b8.02e47a"]]},{"id":"3e93e49a.d58c2c","type":"function","z":"3eb563f4.6d4ebc","name":"Positioner 0","func":"msg.payload = {\n \"method\": \"PUT\",\n \"uri\": \"/3337/0/5536\",\n \"accept\": \"text/plain\",\n \"content-type\": \"text/plain\",\n \"payload-b64\": msg.payload\n}\nreturn msg;","outputs":1,"noerr":0,"x":730,"y":400,"wires":[["6b8268f7.66ef78"]]},{"id":"6138c868.f1a438","type":"base64","z":"3eb563f4.6d4ebc","name":"","action":"str","property":"payload","x":520,"y":400,"wires":[["3e93e49a.d58c2c"]]},{"id":"57f29b51.b20664","type":"ui_slider","z":"3eb563f4.6d4ebc","name":"","label":"Servo2","tooltip":"","group":"9a50a3b3.867","order":0,"width":0,"height":0,"passthru":true,"outs":"end","topic":"","min":"900","max":"2000","step":"1","x":380,"y":460,"wires":[["1798ca7f.541bf6"]]},{"id":"73e867c6.405fe8","type":"function","z":"3eb563f4.6d4ebc","name":"Positioner 1","func":"msg.payload = {\n \"method\": \"PUT\",\n \"uri\": \"/3337/1/5536\",\n \"accept\": \"text/plain\",\n \"content-type\": \"text/plain\",\n \"payload-b64\": msg.payload\n}\nreturn msg;","outputs":1,"noerr":0,"x":730,"y":460,"wires":[["6b8268f7.66ef78"]]},{"id":"cbac04e2.2240f8","type":"ui_slider","z":"3eb563f4.6d4ebc","name":"","label":"Servo3","tooltip":"","group":"9a50a3b3.867","order":0,"width":0,"height":0,"passthru":true,"outs":"end","topic":"","min":"900","max":"2000","step":"1","x":380,"y":520,"wires":[["4f1edd27.62eb14"]]},{"id":"e7cc5b63.b1ac28","type":"function","z":"3eb563f4.6d4ebc","name":"Positioner 2","func":"msg.payload = {\n \"method\": \"PUT\",\n \"uri\": \"/3337/2/5536\",\n \"accept\": \"text/plain\",\n \"content-type\": \"text/plain\",\n \"payload-b64\": msg.payload\n}\nreturn msg;","outputs":1,"noerr":0,"x":730,"y":520,"wires":[["6b8268f7.66ef78"]]},{"id":"47e6e79e.e77418","type":"ui_slider","z":"3eb563f4.6d4ebc","name":"","label":"Servo4","tooltip":"","group":"9a50a3b3.867","order":0,"width":0,"height":0,"passthru":true,"outs":"end","topic":"","min":"900","max":"1800","step":"1","x":380,"y":580,"wires":[["a47ceb51.d89b78"]]},{"id":"84253783.7ee488","type":"function","z":"3eb563f4.6d4ebc","name":"Positioner 3","func":"msg.payload = {\n \"method\": \"PUT\",\n \"uri\": \"/3337/3/5536\",\n \"accept\": \"text/plain\",\n \"content-type\": \"text/plain\",\n \"payload-b64\": msg.payload\n}\nreturn msg;","outputs":1,"noerr":0,"x":730,"y":580,"wires":[["6b8268f7.66ef78"]]},{"id":"67d3ecb1.a93ac4","type":"ui_slider","z":"3eb563f4.6d4ebc","name":"","label":"Servo5","tooltip":"","group":"9a50a3b3.867","order":0,"width":0,"height":0,"passthru":true,"outs":"end","topic":"","min":"900","max":"1800","step":"1","x":380,"y":640,"wires":[["427d154c.77e68c"]]},{"id":"ff6223dc.9268c","type":"function","z":"3eb563f4.6d4ebc","name":"Positioner 4","func":"msg.payload = {\n \"method\": \"PUT\",\n \"uri\": \"/3337/4/5536\",\n \"accept\": \"text/plain\",\n \"content-type\": \"text/plain\",\n \"payload-b64\": msg.payload\n}\nreturn msg;","outputs":1,"noerr":0,"x":730,"y":640,"wires":[["6b8268f7.66ef78"]]},{"id":"c38e435a.5d297","type":"ui_slider","z":"3eb563f4.6d4ebc","name":"","label":"Servo6","tooltip":"","group":"9a50a3b3.867","order":0,"width":0,"height":0,"passthru":true,"outs":"end","topic":"","min":"900","max":"1800","step":"1","x":380,"y":700,"wires":[["96c2d0e1.eff9"]]},{"id":"a64b184e.3c8d38","type":"function","z":"3eb563f4.6d4ebc","name":"Positioner 5","func":"msg.payload = {\n \"method\": \"PUT\",\n \"uri\": \"/3337/5/5536\",\n \"accept\": \"text/plain\",\n \"content-type\": \"text/plain\",\n \"payload-b64\": msg.payload\n}\nreturn msg;","outputs":1,"noerr":0,"x":730,"y":700,"wires":[["6b8268f7.66ef78"]]},{"id":"1798ca7f.541bf6","type":"base64","z":"3eb563f4.6d4ebc","name":"","action":"str","property":"payload","x":520,"y":460,"wires":[["73e867c6.405fe8"]]},{"id":"4f1edd27.62eb14","type":"base64","z":"3eb563f4.6d4ebc","name":"","action":"str","property":"payload","x":520,"y":520,"wires":[["e7cc5b63.b1ac28"]]},{"id":"a47ceb51.d89b78","type":"base64","z":"3eb563f4.6d4ebc","name":"","action":"str","property":"payload","x":520,"y":580,"wires":[["84253783.7ee488"]]},{"id":"427d154c.77e68c","type":"base64","z":"3eb563f4.6d4ebc","name":"","action":"str","property":"payload","x":520,"y":640,"wires":[["ff6223dc.9268c"]]},{"id":"96c2d0e1.eff9","type":"base64","z":"3eb563f4.6d4ebc","name":"","action":"str","property":"payload","x":520,"y":700,"wires":[["a64b184e.3c8d38"]]},{"id":"fda5a7d9.7ce8a8","type":"inject","z":"3eb563f4.6d4ebc","name":"","topic":"","payload":"1500","payloadType":"str","repeat":"","crontab":"","once":false,"onceDelay":0.1,"x":150,"y":560,"wires":[["4d928388.34713c","57f29b51.b20664","cbac04e2.2240f8","47e6e79e.e77418","67d3ecb1.a93ac4","c38e435a.5d297"]]},{"id":"7e6d0390.5eb88c","type":"ui_button","z":"3eb563f4.6d4ebc","name":"GroupAction1","group":"7e9c6a7d.450174","order":0,"width":0,"height":0,"passthru":true,"label":"Group Action 1","tooltip":"","color":"","bgcolor":"","icon":"","payload":"1","payloadType":"num","topic":"","x":360,"y":780,"wires":[["aae799cb.97e5e8"]]},{"id":"6c375ac.a984ea4","type":"inject","z":"3eb563f4.6d4ebc","name":"","topic":"","payload":"","payloadType":"date","repeat":"","crontab":"","once":false,"onceDelay":0.1,"x":160,"y":780,"wires":[["7e6d0390.5eb88c"]]},{"id":"8eb2c2fd.6818","type":"ui_button","z":"3eb563f4.6d4ebc","name":"GroupAction2","group":"7e9c6a7d.450174","order":0,"width":0,"height":0,"passthru":true,"label":"Group Action 2","tooltip":"","color":"","bgcolor":"","icon":"","payload":"2","payloadType":"num","topic":"","x":360,"y":820,"wires":[["ab09e95b.292048"]]},{"id":"969f3d76.a3bc8","type":"inject","z":"3eb563f4.6d4ebc","name":"","topic":"","payload":"","payloadType":"date","repeat":"","crontab":"","once":false,"onceDelay":0.1,"x":160,"y":820,"wires":[["8eb2c2fd.6818"]]},{"id":"32059a2c.a64d76","type":"ui_button","z":"3eb563f4.6d4ebc","name":"GroupAction3","group":"7e9c6a7d.450174","order":0,"width":0,"height":0,"passthru":true,"label":"Group Action 3","tooltip":"","color":"","bgcolor":"","icon":"","payload":"3","payloadType":"num","topic":"","x":360,"y":860,"wires":[["788b904f.63f16"]]},{"id":"d584fd4a.90ac5","type":"inject","z":"3eb563f4.6d4ebc","name":"","topic":"","payload":"","payloadType":"date","repeat":"","crontab":"","once":false,"onceDelay":0.1,"x":160,"y":860,"wires":[["32059a2c.a64d76"]]},{"id":"59b2adef.11db54","type":"function","z":"3eb563f4.6d4ebc","name":"Group Action","func":"msg.payload = {\n \"method\": \"POST\",\n \"uri\": \"/10315/0/7\",\n \"payload-b64\": msg.payload\n}\nreturn msg;","outputs":1,"noerr":0,"x":730,"y":860,"wires":[["6b8268f7.66ef78"]]},{"id":"aae799cb.97e5e8","type":"base64","z":"3eb563f4.6d4ebc","name":"","action":"str","property":"payload","x":520,"y":780,"wires":[["59b2adef.11db54"]]},{"id":"ab09e95b.292048","type":"base64","z":"3eb563f4.6d4ebc","name":"","action":"str","property":"payload","x":520,"y":820,"wires":[["59b2adef.11db54"]]},{"id":"788b904f.63f16","type":"base64","z":"3eb563f4.6d4ebc","name":"","action":"str","property":"payload","x":520,"y":860,"wires":[["59b2adef.11db54"]]},{"id":"5b4d3b95.5ad924","type":"ui_button","z":"3eb563f4.6d4ebc","name":"GroupAction4","group":"7e9c6a7d.450174","order":0,"width":0,"height":0,"passthru":true,"label":"Group Action 4","tooltip":"","color":"","bgcolor":"","icon":"","payload":"4","payloadType":"num","topic":"","x":360,"y":900,"wires":[["4ed1eb2c.d94e14"]]},{"id":"8388acfa.d7327","type":"inject","z":"3eb563f4.6d4ebc","name":"","topic":"","payload":"","payloadType":"date","repeat":"","crontab":"","once":false,"onceDelay":0.1,"x":160,"y":900,"wires":[["5b4d3b95.5ad924"]]},{"id":"4ed1eb2c.d94e14","type":"base64","z":"3eb563f4.6d4ebc","name":"","action":"str","property":"payload","x":520,"y":900,"wires":[["59b2adef.11db54"]]},{"id":"28730bcb.2be234","type":"ui_button","z":"3eb563f4.6d4ebc","name":"GroupAction5","group":"7e9c6a7d.450174","order":0,"width":0,"height":0,"passthru":true,"label":"Group Action 5","tooltip":"","color":"","bgcolor":"","icon":"","payload":"5","payloadType":"num","topic":"","x":360,"y":940,"wires":[["9a14ba8c.b0dbc8"]]},{"id":"f442a725.cca688","type":"inject","z":"3eb563f4.6d4ebc","name":"","topic":"","payload":"","payloadType":"date","repeat":"","crontab":"","once":false,"onceDelay":0.1,"x":160,"y":940,"wires":[["28730bcb.2be234"]]},{"id":"9a14ba8c.b0dbc8","type":"base64","z":"3eb563f4.6d4ebc","name":"","action":"str","property":"payload","x":520,"y":940,"wires":[["59b2adef.11db54"]]},{"id":"49996357.11c7fc","type":"ui_button","z":"3eb563f4.6d4ebc","name":"GroupAction6","group":"7e9c6a7d.450174","order":0,"width":0,"height":0,"passthru":true,"label":"Group Action 6","tooltip":"","color":"","bgcolor":"","icon":"","payload":"6","payloadType":"num","topic":"","x":360,"y":980,"wires":[["d4ae3ba5.ffa838"]]},{"id":"4fdd70fe.7848a","type":"inject","z":"3eb563f4.6d4ebc","name":"","topic":"","payload":"","payloadType":"date","repeat":"","crontab":"","once":false,"onceDelay":0.1,"x":160,"y":980,"wires":[["49996357.11c7fc"]]},{"id":"d4ae3ba5.ffa838","type":"base64","z":"3eb563f4.6d4ebc","name":"","action":"str","property":"payload","x":520,"y":980,"wires":[["59b2adef.11db54"]]},{"id":"9a50a3b3.867","type":"ui_group","z":"","name":"Servo Control","tab":"244c7271.0b95ee","order":1,"disp":true,"width":"20","collapse":true},{"id":"7e9c6a7d.450174","type":"ui_group","z":"","name":"Group Control","tab":"244c7271.0b95ee","order":2,"disp":true,"width":"6","collapse":false},{"id":"244c7271.0b95ee","type":"ui_tab","z":"","name":"6DoF-Robot","icon":"dashboard","disabled":false,"hidden":false}] \ No newline at end of file diff --git a/pictures/node-red-dashboard.png b/pictures/node-red-dashboard.png new file mode 100644 index 0000000..1f32089 Binary files /dev/null and b/pictures/node-red-dashboard.png differ diff --git a/pictures/node-red-flow.jpg b/pictures/node-red-flow.jpg new file mode 100644 index 0000000..e69de29 diff --git a/pictures/node-red-flow.png b/pictures/node-red-flow.png new file mode 100644 index 0000000..c18cc98 Binary files /dev/null and b/pictures/node-red-flow.png differ diff --git a/robot-pt.js b/robot-pt.js index 60ab01a..5380502 100644 --- a/robot-pt.js +++ b/robot-pt.js @@ -46,7 +46,7 @@ serialPort.on('open', function() { }); serialPort.on('data', function (data) { - console.log('Data:', data) + console.log('Data:', data.toString()) }) // open errors will be emitted as an error event @@ -127,10 +127,28 @@ RobotPTDemo.prototype._createDeviceParams = function(deviceId, currentPositionVa transitionTime.writeFloatBE(transitionTimeValue); transitionTime = transitionTime.toString('base64'); + // let groupId = Buffer.allocUnsafe(4); + // groupAction.writeFloatBE(1300); + // groupAction = groupAction.toString('base64'); + // A servo object list params = { deviceId: deviceId, objects: [{ + objectId: 10315, + objectInstances: [{ + objectInstanceId: 0, + resources: [{ + resourceId: 7, + operations: OPERATIONS.EXECUTE, + type: 'float' + }, { + resourceId: 8, + operations: OPERATIONS.EXECUTE, + type: 'float' + }] + }] + }, { objectId: 3337, objectInstances: [{ objectInstanceId: 0, @@ -278,7 +296,7 @@ RobotPTDemo.prototype.unregisterRobotDevice = async function(deviceId) { RobotPTDemo.prototype.exposeWriteMethod = function() { let self = this; self.client.expose('write', (params, response) => { - let value = new Buffer.from(params.value, 'base64').readDoubleBE(); + let resourcePath = params.uri.objectId + '/' + params.uri.objectInstanceId + '/' + params.uri.resourceId; let deviceId = params.uri.deviceId; @@ -292,19 +310,32 @@ RobotPTDemo.prototype.exposeWriteMethod = function() { operation = 'unknown'; } - received = { - deviceId: deviceId, - resourcePath: resourcePath, - operation: operation, - value: value - } - console.log(GREEN, 'Received a write method with data:'); - console.log(received); - console.log(GREEN, 'The raw received JSONRPC 2.0 params:'); - console.log(params); - if (operation == 'write') { - self.ExecuteWriteOperation(resourcePath, value); + let value = new Buffer.from(params.value, 'base64').readDoubleBE(); + + received = { + deviceId: deviceId, + resourcePath: resourcePath, + operation: operation, + value: value + } + console.log(GREEN, 'Received a write method with data:'); + console.log(received); + console.log(GREEN, 'The raw received JSONRPC 2.0 params:'); + console.log(params); + self.WriteOperation(resourcePath, value); + + } else if (operation == 'execute') { + let value = new Buffer.from(params.value, 'base64').toString(); + console.log(GREEN, 'Received execute action with data:'); + received = { + deviceId, + resourcePath, + operation, + value + } + console.log(received); + self.ExecuteOperation(resourcePath, value); } @@ -315,28 +346,56 @@ RobotPTDemo.prototype.exposeWriteMethod = function() { */ response(/* no error */ null, /* success */ 'ok'); }); + + }; -RobotPTDemo.prototype.ExecuteWriteOperation = async function (resourcePath, value) { +RobotPTDemo.prototype.WriteOperation = async function (resourcePath, value) { objectId = resourcePath.split("/")[0] - servoId = resourcePath.split("/")[1]; + instanceId = resourcePath.split("/")[1]; resourceId = resourcePath.split("/")[2]; if (objectId === '3337' && resourceId === '5536') { - serialCmd = "servo " + servoId + " " + value + " 1000" + serialCmd = "servo " + instanceId + " " + value + " 1000" console.log(GREEN, serialCmd); serialPort.write(serialCmd, function(err) { if (err) { - return console.log('Error on write: ', err.message); + return console.log(RED, 'Error on write to serial: ', err.message); } - console.log("Serial command " + serialCmd + " executed"); + console.log(GREEN, "Serial command: <" + serialCmd + "> executed"); }); } else if (objectId === '3337' && resourceId === '5537') { - console.log(GREEN, "Servo " + servoId + " transition time changed successfully."); + console.log(GREEN, "Servo " + instanceId + " transition time changed successfully."); } else { console.log(RED, 'Will not hanlde the write'); } } +RobotPTDemo.prototype.ExecuteOperation = async function (resourcePath, value) { + objectId = resourcePath.split("/")[0] + instanceId = resourcePath.split("/")[1]; + resourceId = resourcePath.split("/")[2]; + serialCmd = ""; + + if (objectId === '10315' && instanceId === '0' && resourceId === '7') { + serialCmd = "group start " + value + + } else if (objectId === '10315' && instanceId === '0' && resourceId === '8') { + serialCmd = "group stop" + } else { + console.log(RED, 'Invalide, will not hanlde the execute.'); + } + + if (serialCmd.length > 0) { + console.log(GREEN, serialCmd); + serialPort.write(serialCmd, function(err) { + if (err) { + return console.log(RED, 'Error on write to serial: ', err.message); + } + console.log(GREEN, "Serial command: <" + serialCmd + "> executed"); + }); + } +} + const holdProgress = async (message) => { process.stdin.setRawMode(true) console.log(YELLOW, util.format('\x1b[1m%s\x1b[0m', message)); @@ -376,7 +435,7 @@ const holdProgress = async (message) => { console.log(GREEN, 'Registered an example device. Response:', response); await holdProgress('Press any key to unregister the example device.'); - response = await edge.unregisterRobotDevice('example-device-1'); + response = await edge.unregisterRobotDevice('robot-arm-1'); console.log(GREEN, 'Example device unregistered. Response:', response); console.log(GREEN, 'Kill the example with Ctrl+C');