Связь между Java и Arduino не синхронизирована - скачок движения - PullRequest
Новая
       101

Связь между Java и Arduino не синхронизирована - скачок движения

0 голосов
/ 30 января 2019

Я пытаюсь управлять роботизированной рукой с помощью контроллера прыжкового движения.Сейчас я просто управляю двумя сервоприводами.Я использую 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:000 Arduino сообщает мне, что он получил все команды по отдельности и правильно отправляет все команды на сервоприводы (это видно по подергиванию сервоприводов) и заканчивается отправкой "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 используется для последовательного считывания и отладки) enter image description here

Я очень запуталсяотносительно того, почему это происходит.Я пытался:

  • Уменьшение скорости передачи (от 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: enter image description here

Однако, когда я запускаю свой код со всеми закомментированными инструкциями отладки, за исключением первого значения данных и номера цикла, он может успешно отслеживать данные из 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");

дает мне:

enter image description here

Это приводит меня к мысли, что либо данные теряются при последовательной связи, так как известно, что эта проблема возникла в программном обеспечении (чтоимеет смысл, потому что меньше считывания -> меньше перемешанных данных), или что у меня могут возникнуть проблемы с памятью на Arduino.Может ли быть что-то из этого?У меня все еще есть оригинальная проблема, просто подумал, что эти идеи могут помочь.

1 Ответ

0 голосов
/ 26 февраля 2019

Я нашел решение: моему коду arduino требовалось ~ 30 мсек, чтобы пройти один цикл, но мой код на стороне java зацикливался быстрее, чем это.64-байтовый последовательный буфер arduino заполнялся после нескольких циклов, и, поскольку 64 не кратен 19 байтам, которые я отправлял, отброшенные байты означали бы, что формат number:number будет испорчен.

Если это кому-нибудь поможет, я просто отследил время цикла arduino и добавил задержку 50 мс в код на стороне java, чтобы сторона на arduino могла наверстать упущенное.

...