问Java、Dynamixel AX-12A与树莓派3的通信

EN

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)

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

    }

}