I2Cを経由して、JAVAでサーボモータを制御する

この間、ディープラーニングは圧倒的にC++よりもJAVAが良いことがわかって、全てをロボットの制御システムを全てJAVAに変えようとしている。
センサー系は、SPIで制御するのだが、これはPi4Jプロジェクトのライブラリでなんとかなりそうなことはわかった。
サーボモーターは、I2C経由で制御する。これも、Pi4Jのライブラリを使えばなんとかなりそうだということで、昨夜から格闘して、ようやくなんとかなってきた。いつものように記録しておく。
pi4jには、サーボモーター制御のサンプルコードは、
PCA9685GpioServoExample.java
PCA9685GpioExample.java
の二つがある。Servoという言葉が入っている初めのものを使おうとしたが、結局、なぜかサーボが異常に熱くなってしまい、動かないので、２番目のを使った。
初めは、パルスの作り方などを細かく変えなくちゃいけないんじゃないかと思って色々いじったが、結局、ほとんど素のままで使えることがわかった。ありがたい。使うところだけ抜き出して、インターフェイスの部分の関数を付け加えると次のようになる。

package aicoro;
import com.pi4j.gpio.extension.pca.PCA9685GpioProvider;
import com.pi4j.gpio.extension.pca.PCA9685Pin;
import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinPwmOutput;
import com.pi4j.io.gpio.Pin;
import com.pi4j.io.i2c.I2CBus;
import com.pi4j.io.i2c.I2CFactory;
import java.io.IOException;
import java.math.BigDecimal;
/**
 * Pi4JのPCA9685GpioServoExample.javaを参照したもの
 */
public class Servo {
    // MG996R用に調整した
    private static final int SERVO_DURATION_MIN = 770;  // 1670/2=835
    private static final int SERVO_DURATION_NEUTRAL = 1570;
    private static final int SERVO_DURATION_MAX = 2370;
    final PCA9685GpioProvider provider;
    Servo() throws I2CFactory.UnsupportedBusNumberException, IOException{
        System.out.println("PCA9685を初期化します");
        //PCA9685GpioServoExample.javaの値をそのまま使っている
        BigDecimal frequency = new BigDecimal("48.828");
        BigDecimal frequencyCorrectionFactor = new BigDecimal("1.0578");
        // Create custom PCA9685 GPIO provider
        I2CBus bus = I2CFactory.getInstance(I2CBus.BUS_1);
        provider = new PCA9685GpioProvider(bus, 0x40, frequency, frequencyCorrectionFactor);
        // Define outputs in use for this example
        GpioPinPwmOutput[] myOutputs = provisionPwmOutputs(provider);
        // Reset outputs
        provider.reset();
    }
    void setAngle(int ch, double degree) {
        if (degree < -90 || degree > 90) {
            System.out.println("setAngle() range err give degree = " + degree);
            return;
        }
        int pulseWidth = SERVO_DURATION_NEUTRAL + (int) (SERVO_DURATION_NEUTRAL * (degree / 90.0));
        //cout << "setAngle() pulseWidth = " << pulseWidth << endl;
        Pin pin = PCA9685Pin.ALL[ch];
        provider.setPwm(pin,pulseWidth);
    }
    private static GpioPinPwmOutput[] provisionPwmOutputs(final PCA9685GpioProvider gpioProvider) {
        // サーボモータの名前を与える、どの関節に関わっているかを明確にする意味がある
        GpioController gpio = GpioFactory.getInstance();
        GpioPinPwmOutput myOutputs[] = {
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_00, "Pulse 00"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_01, "Pulse 01"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_02, "Pulse 02"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_03, "Pulse 03"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_04, "Pulse 04"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_05, "Pulse 05"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_06, "Pulse 06"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_07, "Pulse 07"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_08, "Pulse 08"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_09, "Pulse 09"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_10, "Always ON"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_11, "Always OFF"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_12, "Servo pulse MIN"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_13, "Servo pulse NEUTRAL"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_14, "Servo pulse MAX"),
                gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_15, "not used")};
        return myOutputs;
    }
}

必要ないかもしれないが、センターの位置だけ少し変えている。

void setAngle(int ch, double degree)

関数に、サーボモーターの番号と、角度を与えれば良い。javaのmain関数は、別に与えること。その中で、このクラスをインスタンス化して、上の関数を呼べば良い。
不要かもしれないが、動画をつけておく。