-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
zhangwei-arm
committed
Jul 13, 2020
1 parent
aa4a07e
commit 7ffea7b
Showing
6 changed files
with
259 additions
and
21 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,178 @@ | ||
#include <LobotServoController.h> | ||
|
||
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 = ""; | ||
} | ||
} | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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}] |
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Oops, something went wrong.