[SOLVED] Communication between Java, Dynamixel AX-12A and Raspberry Pi 3

Good Evening

We're working with PI4J and we are tring to transcribe this python code to Java:

from time import sleep
from serial import Serial
import RPi.GPIO as GPIO

class Ax12:
    # important AX-12 constants
    AX_START = 255
    AX_GOAL_LENGTH = 5
    AX_WRITE_DATA = 3
    AX_GOAL_POSITION_L = 30
    TX_DELAY_TIME = 0.00002

    # RPi constants
    RPI_DIRECTION_PIN = 2
    RPI_DIRECTION_TX = GPIO.HIGH
    RPI_DIRECTION_RX = GPIO.LOW
    RPI_DIRECTION_SWITCH_DELAY = 0.0001

    # static variables
    port = None
    gpioSet = False

    def __init__(self):
        if(Ax12.port == None):
            Ax12.port = Serial("/dev/ttyAMA0", baudrate=1000000, timeout=0.001)
        if(not Ax12.gpioSet):
            GPIO.setwarnings(False)
            GPIO.setmode(GPIO.BCM)
            GPIO.setup(Ax12.RPI_DIRECTION_PIN, GPIO.OUT)
            Ax12.gpioSet = True
        self.direction(Ax12.RPI_DIRECTION_RX)

    def direction(self,d):
        GPIO.output(Ax12.RPI_DIRECTION_PIN, d)
        sleep(Ax12.RPI_DIRECTION_SWITCH_DELAY)

    def move(self, id, position):
        self.direction(Ax12.RPI_DIRECTION_TX)
        Ax12.port.flushInput()
        p = [position&0xff, position>>8]
        checksum = (~(id + Ax12.AX_GOAL_LENGTH + Ax12.AX_WRITE_DATA + Ax12.AX_GOAL_POSITION_L + p[0] + p[1]))&0xff
        outData = chr(Ax12.AX_START)
        outData += chr(Ax12.AX_START)
        outData += chr(id)
        outData += chr(Ax12.AX_GOAL_LENGTH)
        outData += chr(Ax12.AX_WRITE_DATA)
        outData += chr(Ax12.AX_GOAL_POSITION_L)
        outData += chr(p[0])
        outData += chr(p[1])
        outData += chr(checksum)
        print(Ax12.AX_START,Ax12.AX_START,id,Ax12.AX_GOAL_LENGTH,Ax12.AX_WRITE_DATA,Ax12.AX_GOAL_POSITION_L,p[0],p[1],checksum)
        print(chr(Ax12.AX_START),chr(Ax12.AX_START),chr(id),chr(Ax12.AX_GOAL_LENGTH),chr(Ax12.AX_WRITE_DATA),chr(Ax12.AX_GOAL_POSITION_L),chr(p[0]),chr(p[1]),chr(checksum))
        Ax12.port.write(outData)
        sleep(Ax12.TX_DELAY_TIME)

This code move the AX-12A motor to the desired position, and we are trying to do the same using Java (IDE: BlueJ). We have translated the most part of the code, and we just need a help to correct it.

import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinDigitalOutput;
import com.pi4j.io.gpio.PinState;
import com.pi4j.io.gpio.RaspiPin;
import com.pi4j.wiringpi.Serial;


public class Ax12 {

    # important AX-12 constants
    public static final int AX_START = 255;
    public static final int AX_GOAL_LENGTH = 5;
    public static final int AX_WRITE_DATA = 3;
    public static final int AX_GOAL_POSITION_L = 30;
    public static final long TX_DELAY_TIME = 2; #0.00002

    # RPi constants
    public static final int RPI_DIRECTION_PIN = 2;

    # Opening GPIO comunication and defining TX and RX ports

    final static GpioController raspi = GpioFactory.getInstance(); 
    final static GpioPinDigitalOutput RPI_DIRECTION_TX = raspi.provisionDigitalOutputPin(RaspiPin.GPIO_14, "TX", PinState.HIGH);
    final static GpioPinDigitalOutput RPI_DIRECTION_RX = raspi.provisionDigitalOutputPin(RaspiPin.GPIO_15, "RX", PinState.LOW);

    public static final long RPI_DIRECTION_SWITCH_DELAY = 1; # 0.0001

    # static variables
    static int port = 0;  # static Integer port = null;
    static boolean gpioSet = false;

    Ax12(){
        if(Ax12.port==0)
            Ax12.port = Serial.serialOpen(Serial.DEFAULT_COM_PORT, 1000000);

        if (!Ax12.gpioSet)
            Ax12.gpioSet = true;
    }


    public static void direction(int d) throws InterruptedException {

        if (d==1)
            raspi.provisionDigitalOutputPin(RaspiPin.GPIO_02, "2-H", PinState.HIGH);
        else
            raspi.provisionDigitalOutputPin(RaspiPin.GPIO_02, "2-L", PinState.LOW);

        Thread.sleep(RPI_DIRECTION_SWITCH_DELAY);
    }


    public void move (int id, int position) throws InterruptedException {

        Ax12.direction(1); # TX port on

        Serial.serialFlush(Ax12.port);
        System.out.println("it move!");

        int [] p = new int[2];

        p[0] = position&0xff;
        p[1] = position >> 8;

        int checksum = (~(id + Ax12.AX_GOAL_LENGTH + Ax12.AX_WRITE_DATA + Ax12.AX_GOAL_POSITION_L + p[0] + p[1]))&0xff;

        String outData;
        outData = "\\x"+Integer.toHexString(Ax12.AX_START); 
        outData += "\\x"+Integer.toHexString(Ax12.AX_START);
        outData += "\\x0"+Integer.toHexString(id);
        outData += "\\x0"+Integer.toHexString(Ax12.AX_GOAL_LENGTH);
        outData += "\\x0"+Integer.toHexString(Ax12.AX_WRITE_DATA);
        outData += "\\x"+Integer.toHexString(Ax12.AX_GOAL_POSITION_L);
        outData += "\\x0"+Integer.toHexString(p[0]);
        outData += "\\x0"+Integer.toHexString(p[1]);
        outData += "\\x"+Integer.toHexString(checksum);
        System.out.println(outData);
        Serial.serialPuts(Ax12.port, outData);
        Thread.sleep(Ax12.TX_DELAY_TIME);

    }

}

The code can be compiled, but does not move the motor. We need to understand where our mistake are. We believe that the GPIO library used is not the correct, but we have an uncertainty about that. If someone could help us, we thank you.

Note: the "\x" and the "\x0" is just to correct the characters in hexadecimal to motor four in position 512.