Java to Arduino communication out of sync - leap motion
I'm trying to control a robotic arm with a leap motion controller. Right now I'm just controlling two servos. I'm using java to read data from the leap motion, process and format it, and send it to the Arduino. The Arduino just receives the data, translates it, and sends it to the servos.
The format that I send the data to the Arduino in is, in string form:
z-rotation:shoulderPos:elbowAngle:wristAngle:clawPos
with each of these variables formatted with leading zeroes so that exactly 19 bytes are always sent to the Arduino at a time.
The issue is that data seems to be being lost in communication between the java on my laptop and the Arduino. If I send one command string, "000:180:000:000:000"
for example, the Arduino tells me that it's received "000:180:000:000:000"
and it correctly sends "000" to one servo and "180" to the second servo.
If I send a string of nine commands:
000:000:000:000:000180:000:000:000:000000:000:000:000:000180:000:000:000:000000:000:000:000:000180:000:000:000:000000:000:000:000:000180:000:000:000:000
The Arduino tells me that it's received all of the commands individually and it correctly sends all of the commands to the servos (evident by the twitching of the servos) and ends with sending "000" to both servos.
However, when I run my code with the leap motion, which effectively constantly transmits strings of 19 bytes to the Arduino, the servos just begin to twitch, moving between 0, 180, and the position that I'm sending to them. When I move my hand closer to the 100 position, the twitching servos have a net movement towards the 100 position, but never actually reaches it. The Arduino tells me that it's receiving the commands correctly for a few seconds before beginning to receive distorted messages like "0:180:0000:0018:00". I can only assume that the transmission and reception of commands are getting out of sync, but I'm not sure.
Here's my Java code:
import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.OutputStream;
import java.text.DecimalFormat;
import java.util.ArrayList;
import com.leapmotion.leap.*;
import gnu.io.CommPort;
import gnu.io.CommPortIdentifier;
import gnu.io.SerialPort;
import gnu.io.SerialPortEvent;
import gnu.io.SerialPortEventListener;
class SampleListenerMain extends Listener {
//define position lock booleans
public boolean leftLock = false;
public boolean rightLock = false;
String data = "";
static DecimalFormat df = new DecimalFormat("000");
//Displacement variables
double deltaX, deltaY, deltaZ, angle;
public void onInit(Controller controller) {
System.out.println("Initialized");
}
public void onConnect(Controller controller) {
System.out.println("Connected");
controller.enableGesture(Gesture.Type.TYPE_CIRCLE);
controller.enableGesture(Gesture.Type.TYPE_KEY_TAP);
}
public void onDisconnect(Controller controller) {
System.out.println("Disconnected");
}
public void onExit(Controller controller) {
System.out.println("Exited");
}
public void onFrame(Controller controller) {
//Define position variables
double shoulderAngle, elbowAngle, wristPos, clawPos, zRotationPos, wristAngle;
//Define object variables
//Frame
Frame frame = controller.frame();
//Hands
Hand leftHand = frame.hands().leftmost();
Hand rightHand = frame.hands().rightmost();
//Arms
Arm leftArm = leftHand.arm();
Arm rightArm = rightHand.arm();
/* Control of robotic arm with Z-rotation based on the left hand, arm 'wrist' position based on the wrist,
* arm 'elbow position based on the elbow, and claw based on the fingers. 'Shoulder' is based on the left elbow
*/
//Control position locks for left hand controls and right hand controls
//Gesture gesture = new Gesture(gesture);
for(Gesture gesture : frame.gestures()) {
HandList handsForGesture = gesture.hands();
switch(gesture.type()) {
case TYPE_KEY_TAP:
System.out.println("Key tap from" + handsForGesture + " Hand");
try {
wait(500);
} catch (InterruptedException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
leftLock = !leftLock;
break;
default:
System.out.println("Unrecognized gesture");
break;
}
}
//'Shoulder' control
//find angle between the left elbow and the left wrist center
Vector leftElbow = leftArm.elbowPosition();
Vector leftWrist = leftArm.wristPosition();
deltaZ = leftElbow.getZ() - leftWrist.getZ();
deltaY = leftElbow.getY() - leftWrist.getY();
angle = Math.atan(deltaY/deltaZ);
//map angle so servo can understand it
shoulderAngle = leapArm.map(angle, 0, 90, 0, 180);
//System.out.println("ShoulderPos: " + shoulderAngle);
//Write position to 'shoulder'
//Z-rotation control
Vector leftHandPos = leftHand.palmPosition();
//rotate z-axis with speed proportional to left hand X position
//map X position to motor power
zRotationPos = leapArm.map(leftHandPos.getX(), -230, 230, 0, 180);
//System.out.println("zRotationPos: " + zRotationPos);
data += df.format(zRotationPos);
data += ":" + df.format(shoulderAngle);
//write power to rotational servo
//'elbow' control
//find angle between the right elbow and right wrist center
Vector rightElbow = rightArm.elbowPosition();
Vector rightWrist = rightArm.wristPosition();
//refresh deltas and angle
deltaZ = rightElbow.getZ() - rightWrist.getZ();
deltaY = rightElbow.getY() - rightWrist.getY();
angle = Math.atan(deltaY/deltaZ);
//map angle so the servo can understand it
elbowAngle = leapArm.map(angle, -1.25, 0, 0, 180);
data+= ":" + df.format(elbowAngle);
//System.out.println("ElbowPos: " + elbowAngle);
//'wrist' control
//update vectors
rightWrist = rightArm.wristPosition();
Vector rightHandPos = rightHand.palmPosition();
//update deltas
deltaZ = rightWrist.getZ() - rightHandPos.getZ();
deltaY = rightWrist.getY() - rightHandPos.getY();
System.out.println("Wrist pos: " + rightWrist.getX() + ", " + rightWrist.getY() + ", " + rightWrist.getZ());
System.out.println("Right hand pos: " + rightHandPos.getX() + ", " + rightHandPos.getY() + ", " + rightHandPos.getZ());
angle = Math.atan(deltaY/deltaZ);
wristAngle = leapArm.map(angle, -0.5, 0.5, 0, 180);
data += ":" + df.format(wristAngle);
//System.out.println("wristAngle: " + wristAngle + " degrees");
//pinch control
//define fingers
FingerList fingerList = rightHand.fingers().fingerType(Finger.Type.TYPE_INDEX);
Finger rightIndexFinger = fingerList.get(0);
fingerList = rightHand.fingers().fingerType(Finger.Type.TYPE_THUMB);
Finger rightThumb = fingerList.get(0);
//find the distance between the bones to detect pinch
Vector rightIndexDistal = rightIndexFinger.bone(Bone.Type.TYPE_DISTAL).center();
Vector rightThumbDistal = rightThumb.bone(Bone.Type.TYPE_DISTAL).center();
//Calculate distance between joints
double distalDistance = Math.sqrt(Math.pow((rightIndexDistal.getX()-rightThumbDistal.getX()),2) + Math.pow((rightIndexDistal.getY()-rightThumbDistal.getY()),2) + Math.pow((rightIndexDistal.getZ()-rightThumbDistal.getZ()),2));
if(distalDistance <= 10) {
clawPos = 180;
} else {
clawPos = 0;
}
data += ":" + df.format(clawPos);
System.out.println("ClawPos: " + clawPos);
/* Write data to arduino
* FORMAT: z-rotation:shoulderPos:elbowAngle:wristAngle:clawPos
*/
System.out.println("Data: " + data);
/* wait for arduino to catch up ~30 packets/sec
* basically see how long the arduino takes to process one packet and flush the receiving arrays to prevent 'pollution'.
*/
try {
Thread.sleep(50);
} catch (InterruptedException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
//send to arduino
leapArm.writeToArduino(data);
System.out.println("Sent");
}
}
public class leapArm implements SerialPortEventListener {
public static double map(double input, double in_min, double in_max, double out_min, double out_max) {
return ((input - in_min) * (out_max - out_min) / (in_max - in_min)) + out_min;
}
static OutputStream out = null;
static BufferedReader input;
public static void main(String[] args) {
//Connect to COM port
try
{
//Device
(new leapArm()).connect("/dev/cu.usbmodem14101");
Thread.sleep(3000);
//leapArm.writeToArduino("000:000:000:000:000180:000:000:000:000000:000:000:000:000180:000:000:000:000000:000:000:000:000180:000:000:000:000000:000:000:000:000180:000:000:000:000");
//System.out.println("sent");
}
catch ( Exception e )
{
e.printStackTrace();
System.exit(0);
}
// Create a sample listener and controller
SampleListenerMain listener = new SampleListenerMain();
Controller controller = new Controller();
// Have the sample listener receive events from the controller
controller.addListener(listener);
// Keep this process running until Enter is pressed
System.out.println("Press Enter to quit...");
try {
System.in.read();
} catch (IOException e) {
e.printStackTrace();
}
// Remove the sample listener when done
controller.removeListener(listener);
}
void connect ( String portName ) throws Exception {
CommPortIdentifier portIdentifier = CommPortIdentifier.getPortIdentifier(portName);
if ( portIdentifier.isCurrentlyOwned() )
{
System.out.println("Error: Port is currently in use");
}
else
{
CommPort commPort = portIdentifier.open(this.getClass().getName(),2000);
if ( commPort instanceof SerialPort )
{
SerialPort serialPort = (SerialPort) commPort;
serialPort.setSerialPortParams(4800,SerialPort.DATABITS_8,SerialPort.STOPBITS_1,
SerialPort.PARITY_NONE);
out = serialPort.getOutputStream();
//input = new BufferedReader(new InputStreamReader(serialPort.getInputStream()));
// add event listeners
try {
serialPort.addEventListener(this);
serialPort.notifyOnDataAvailable(true);
} catch (Exception e) {
System.err.println(e.toString());
}
}
else
{
System.out.println("Selected port is not a Serial Port");
}
}
}
public static void writeToArduino(String data)
{
String tmpStr = data;
byte bytes[] = tmpStr.getBytes();
try {
/*System.out.println("Sending Bytes: ");
for(int i = 0; i<bytes.length; i++) {
System.out.println(bytes[i]);
}*/
out.write(bytes);
} catch (IOException e) { }
}
public synchronized void serialEvent(SerialPortEvent oEvent) {
if (oEvent.getEventType() == SerialPortEvent.DATA_AVAILABLE) {
try {
String inputLine=input.readLine();
System.out.println("Received: " + inputLine);
} catch (Exception e) {
System.err.println(e.toString());
}
}
// Ignore all the other eventTypes, but you should consider the other ones.
}
}
And here's my Arduino code:
#include <SoftwareSerial.h>
#include <Servo.h>
Servo shoulder1, shoulder2;
SoftwareSerial mySerial(5,3); //RX, TX
char *strings[19];
char chars[19];
int loopno = 0;
byte index = 0;
void setup() {
// put your setup code here, to run once:
Serial.begin(4800);
mySerial.begin(9600);
mySerial.println("Started");
shoulder1.attach(2);
shoulder2.attach(4);
chars[19] = NULL;
}
void loop() {
// put your main code here, to run repeatedly:
if(Serial.available()>18) {
loopno++;
Serial.readBytes(chars, 19);
/*for(int i = 0; i< sizeof(chars); i++) {
mySerial.print("Character ");
mySerial.print(i);
mySerial.print(": ");
mySerial.println(chars[i]);
}*/
String str(chars);
/*mySerial.print("In string form: ");
mySerial.println(str);*/
char* ptr = NULL;
index = 0;
ptr = strtok(chars, ":");
while(ptr != NULL) {
/* mySerial.print("Pointer: ");
mySerial.println(ptr);*/
strings[index] = ptr;
index++;
ptr = strtok(NULL, ":");
}
//mySerial.print("shoulder1: ");
mySerial.println(atoi(strings[0]));
/*mySerial.print("shoulder2: ");
mySerial.println(atoi(strings[0]));
mySerial.print("Loop no: ");*/
mySerial.println(loopno);
shoulder1.write(atoi(strings[0]));
shoulder2.write(atoi(strings[1]));
}
flush();
}
void flush() {
for(int i = 0; i<19; i++) {
chars[i] = NULL;
strings[i] = NULL;
}
}
And here's the circuit that I'm using (The top Arduino is used for serial readout and debugging)
I'm very confused as to why this is happening. I've tried:
- Decreasing the baud rate (115200 to 4800).
- Sending commands one at a time or in small groups as described earlier.
- Commenting out all debugging and unnecessary
printstatements to decrease processing time and reduce the amount ofSerialcalls in the Arduino program. - Commenting out all
printstatements in the Java code as well as rewriting my formatting and transmission code with an eye for effifiency to increase data collection -> transmission speed.
I'd appreciate it if anyone has experience with this or knows what the probelem could be!
Thanks, Gabe
Edit:
I messed around with it when I got home and I think I might have isolated (one of) the issue(s). When I have all my debugging print statements uncommented and I send the arduino `
Tests.writeToArduino("000:000:000:000:000000:000:000:000:000180:000:000:000:000"); Tests.writeToArduino("180:000:000:000:000000:000:000:000:000000:000:000:000:000");
Tests.writeToArduino("000:000:000:000:000000:000:000:000:000000:000:000:000:000");
Tests.writeToArduino("180:000:000:000:000000:000:000:000:000000:000:000:000:000");, it starts to give me weird feedback starting at loop 7:
However, when I run my code with all the debugging statements commented out except for the first data value and the loop number, it is able to successfully keep track of 51 loops worth of data:
Tests.writeToArduino("000:000:000:000:000000:000:000:000:000180:000:000:000:000");
Tests.writeToArduino("180:000:000:000:000000:000:000:000:000000:000:000:000:000");
Tests.writeToArduino("000:000:000:000:000000:000:000:000:000000:000:000:000:000");
Tests.writeToArduino("180:000:000:000:000000:000:000:000:000000:000:000:000:000");
Tests.writeToArduino("000:000:000:000:000000:000:000:000:000000:000:000:000:000");
Tests.writeToArduino("180:000:000:000:000000:000:000:000:000000:000:000:000:000");
Tests.writeToArduino("000:000:000:000:000000:000:000:000:000000:000:000:000:000");
Tests.writeToArduino("180:000:000:000:000000:000:000:000:000000:000:000:000:000");
Tests.writeToArduino("000:000:000:000:000000:000:000:000:000000:000:000:000:000");
Tests.writeToArduino("180:000:000:000:000000:000:000:000:000000:000:000:000:000");
Tests.writeToArduino("000:000:000:000:000000:000:000:000:000180:000:000:000:000");
Tests.writeToArduino("000:000:000:000:000000:000:000:000:000180:000:000:000:000");
Tests.writeToArduino("000:000:000:000:000000:000:000:000:000180:000:000:000:000");
Tests.writeToArduino("000:000:000:000:000000:000:000:000:000180:000:000:000:000");
Tests.writeToArduino("000:000:000:000:000000:000:000:000:000180:000:000:000:000");
Tests.writeToArduino("000:000:000:000:000000:000:000:000:000180:000:000:000:000");
Tests.writeToArduino("000:000:000:000:000000:000:000:000:000777:000:000:000:000");
gives me:
This leads me to believe that either data is being lost in serial communication, as softwareserial is known to have this issue (which would make sense because less readout -> less jumbled data), or that I might be running into memory issues on the Arduino. Could either of these be the case? I'm still having the original issue, just thought these insights might help.
I found the solution: my arduino code was taking ~30ms to go through one loop but my java-side code was looping faster than that. The 64 byte arduino serial buffer was getting filled up after a few loops, and, since 64 is not a multiple of the 19 bytes I was sending, the dropped bytes would mean that the number:number format would get messed up.
If it helps anyone, I just tracked the time of the arduino loop and added a 50ms delay to the java-side code so that the arduino-side can catch up.
- Keep numbers which appear in both columns, in J lang
- Differentiation in J
- How to reshape an array with an arbitrary size in one dimension?
- Why is Insert (fold) right associative
- Write 4 : 'x&{.&.;: y' tacitly
- Alignment issue when printing formatted prime numbers in J language
- How can I define a verb in J that applies a different verb alternately to each atom in a list?
- How to get user input in the J programming language
- How to unbox a list of boxed lists of differing lengths in J?
- How can I fix 'noun result was required' error in J?
- In j, how can I define a verb locally in one scope and pass it to a defined adverb?
- Convert boxed array to normal array?
- Read column of CSV file as array
- Replace atom in array of strings
- What does the dyad `=` do to boxed strings?
- Index of minimum element using J
- How can I take the outer product of string vectors in J?
- Building an array of verbs in J
- Reading in multidigit command line parameter
- Amend with bond to new data shows unexpected behaviour
- How to turn a table or matrix into a (flat) list in J
- How to run dissect in J?
- How to define selection using index function in J
- How to exit the J console?
- Find 4-neighbors using J
- Writing custom verbs in J
- How do I negate a selector in J lang?
- How to use arbitrary selector in interchange in J lang?
- different result once square root is added inside tacit
- Sum of arrays with repeated indices