Java 到 Arduino 的通信不同步 - 跳跃运动

Question

我正在尝试使用跳跃运动 Controller 控制机械臂。现在我只是控制两个伺服系统。我正在使用 java 从跳跃运动中读取数据，对其进行处理和格式化，然后将其发送到 Arduino。 Arduino 只是接收数据，对其进行翻译，然后将其发送到伺服系统。

我将数据发送到 Arduino 的格式是字符串形式:z-rotation:shoulderPos:elbowAngle:wristAngle:clawPos这些变量中的每一个都用前导零格式化，以便一次总是将 19 个字节发送到 Arduino。

问题是我的笔记本电脑上的 java 和 Arduino 之间的通信似乎丢失了数据。如果我发送一个命令字符串，"000:180:000:000:000"例如，Arduino 告诉我它已收到 "000:180:000:000:000"它正确地将“000”发送到一个伺服器，将“180”发送到第二个伺服器。

如果我发送一串九个命令: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:000Arduino 告诉我它单独接收了所有命令，并且它正确地将所有命令发送到伺服系统(从伺服系统的抽动可以看出)，并以向两个伺服系统发送“000”结束。

然而，当我使用跳跃运动运行我的代码时，它有效地不断将 19 字节的字符串传输到 Arduino，伺服系统开始抽搐，在 0 之间移动。 , 180 ，以及我要发送给他们的职位。当我将手靠近 100位置，抽搐伺服系统向 100 方向移动。位置，但从未真正达到它。 Arduino 告诉我它在几秒钟内正确接收命令，然后开始接收失真消息，如 "0:180:0000:0018:00"。 .我只能假设命令的传输和接收不同步，但我不确定。

这是我的Java代码:

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.
        }

}

这是我的 Arduino 代码:

#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;

}

}

这是我正在使用的电路(顶部的 Arduino 用于串行读出和调试)

我很困惑为什么会这样。我试过了:

降低波特率(115200 到 4800)。

如前所述，一次或以小组形式发送一个命令。

注释掉所有调试和不必要的print减少处理时间和减少 Serial 数量的语句在 Arduino 程序中调用。

注释掉所有 print Java 代码中的语句以及重写我的格式和传输代码，着眼于提高数据收集 -> 传输速度的效率。

如果有人对此有经验或知道问题可能是什么，我将不胜感激!

谢谢，
加布

编辑:

当我回到家时，我把它搞砸了，我想我可能已经孤立了(一个)问题。当我没有注释所有调试打印语句并发送 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"); ，它从循环 7 开始给我奇怪的反馈:

但是，当我运行我的代码时，除了第一个数据值和循环号之外的所有调试语句都被注释掉了，它能够成功地跟踪 51 个循环的数据: 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");
给我:



这使我相信串行通信中的数据正在丢失，因为已知软件串行存在这个问题(这是有道理的，因为更少的读数 -> 更少的困惑数据)，或者我可能在 Arduino 上遇到内存问题.这两种情况都可以吗？我仍然遇到原始问题，只是认为这些见解可能会有所帮助。

Answer 1

我找到了解决方案:我的 arduino 代码需要大约 30 毫秒才能完成一个循环，但我的 java 端代码的循环速度比这快。几次循环后，64 字节的 arduino 串行缓冲区被填满，并且由于 64 不是我发送的 19 个字节的倍数，丢弃的字节意味着 number:number格式会搞砸。

如果它对任何人有帮助，我只是跟踪了 arduino 循环的时间，并为 java 端代码添加了 50 毫秒的延迟，以便 arduino 端可以 catch 。