日記；回すだけⅥ ④ C++でプログラミング ＜リモート操作が完成＞

　ここまでで、リモート操作をするためのパーツの一通りの動作が確認できました。
　ジョイスティックを動かすと、それに連動して左右のモータが動き、車が動きます。
　ジョイスティックのデータはBLEで車側に送ります。
　BLEでデータを受け取ったマイコン・ボードは、角度とベクトル情報から、左右のモータを駆動する電圧を発生します。
　モータ・ドライバのEPOS4はラズパイのCANバス経由で動作します。ラズパイは、maxonのEPOS4駆動ライブラリを利用しています。

ジョイスティックのデータを送るBLEペリフェラル（CircuitPython）

　CircuitPython 8.0.0を使って、マイコン・ボードFeather nRF52840 Expressにつないだジョイスティックからアナログ値を読み出し、角度情報をBLEで送信するプログラムです。Feather nRF52840 Expressには3.7VのLiポリマ電池をつなげられるので、スタンドアロンで利用できます。
　熱収縮チューブの径の大きなものがなかったので、つなぎ合わせています。

mainのcode.py

　local nameは「Feather nRF52840 Express」です。セントラル側では、この名前の一致で受信を始めます。
　角度thとベクトル長Veを計算は、1回目を参照してください。
　

from board import *
import analogio
import time
import math
from adafruit_ble import BLERadio
from SwitchBot import SensorService
from adafruit_ble.advertising.standard import ProvideServicesAdvertisement

pinX = analogio.AnalogIn(A0)
pinY = analogio.AnalogIn(A1)
NUM_SAMPLES = 20

ble = BLERadio()
sService = SensorService()
advertisement = ProvideServicesAdvertisement(sService)
ble.name = "Feather nRF52840 Express"

def readXY():
    analogDataX=[]
    analogDataY=[]
    for i in range(NUM_SAMPLES):
        analogDataX.append(pinX.value)
        analogDataY.append(pinY.value)
        time.sleep(0.02)
    analogDataX.sort()
    analogDataY.sort()
    meanX = sum(analogDataX[5:NUM_SAMPLES-5]) / float(len(analogDataX[5:NUM_SAMPLES-5]))
    meanY = sum(analogDataY[5:NUM_SAMPLES-5]) / float(len(analogDataY[5:NUM_SAMPLES-5]))
    #print(analogData0[5:NUM_SAMPLES-5])
    x = 2*(((meanX-16300)/33100)-0.502)
    y = 2*(((meanY-15728)/31965)-0.5088)
    th = math.atan2(y,x)*180/3.141592
    Ve = math.sqrt(x*x + y*y)
    return th,Ve

while 1:
    print("Advertise services")
    ble.stop_advertising()  # you need to do this to stop any persistent old advertisement
    ble.start_advertising(advertisement)

    print("Waiting for connection...")
    while not ble.connected:
        pass
    ble.stop_advertising()
    print("Connected")
    while ble.connected:
        [th,Ve] = readXY()
        print('Theta ={:.4f} Vector ={:.2f}'.format(th,Ve))
        sService.sensorsTheta = th
        sService.sensorsVector = Ve
        time.sleep(0.2)

pinX.deinit()
pinY.deinit()
print("Disconnected")

SwitchBot.py

　実数の値を送るためのCharacteristicのクラスです。同じものをセントラル側でも使います。code.pyと同じディレクトリに保存しておきます。

# SPDX-FileCopyrightText: 2020 Mark Raleson
# SPDX-License-Identifier: MIT
from adafruit_ble.uuid import VendorUUID
from adafruit_ble.services import Service
from adafruit_ble.characteristics import Characteristic
from adafruit_ble.characteristics.float import FloatCharacteristic

class SensorService(Service):

    uuid = VendorUUID("51ad213f-e568-4e35-84e4-67af89c79ef0")

    sensorsTheta = FloatCharacteristic(
        uuid=VendorUUID("e077bdec-f18b-4944-9e9e-8b3a815162b4"),
        properties=Characteristic.READ | Characteristic.NOTIFY,
    )

    sensorsVector = FloatCharacteristic(
        uuid=VendorUUID("528ff74b-fdb8-444c-9c64-3dd5da4135ae"),
        properties=Characteristic.READ | Characteristic.NOTIFY,
    )

    def __init__(self, service=None):
        super().__init__(service=service)
        self.connectable = True

ジョイスティックのデータを受けるBLEペリフェラル（CircuitPython）

　ペリフェラルと接続して角度thとベクトル長Veを受け取ります。それをグラフィック・ディスプレイに表示します。
　第3回のプログラムを修正し、PWM出力を追加しています。
　角度thとベクトル長Veから、左右のモータに制御する電圧を出力します。このマイコン・ボードにはPWM出力しかないので、ディーティ比で電圧を表します。また、プラス・マイナスの出力を出したいのですが、PWMではその表現はできないので、0V～電源電圧3.3Vの中央付近の電圧のオフセットを作りました。0～中央付近の電圧はマイナスを表し、真ん中から電源電圧まではプラスの電圧という取り決めをしました。
　PWM出力はD10とD11ポートに出します。これを、EPOS4のアナログ入力1と2に接続します。

mainのcode.py

# SPDX-FileCopyrightText: 2020 Mark Raleson
# SPDX-License-Identifier: MIT
from SwitchBot import SensorService
from adafruit_ble import BLERadio
from adafruit_ble.advertising.standard import ProvideServicesAdvertisement
import time
import board
import terminalio
import displayio
from busio import SPI
from adafruit_display_text import label
from adafruit_bitmap_font import bitmap_font
from adafruit_st7789 import ST7789
from adafruit_display_shapes.circle import Circle
from adafruit_display_shapes.line import Line
import math
import gc
import pwmio

displayio.release_displays()  # Release any resources currently
spi = SPI(clock=board.SCK, MOSI=board.MOSI)
tft_cs  = board.D5
tft_dc  = board.D9
tft_rst = board.D6

display_bus = displayio.FourWire(spi, command=tft_dc, chip_select=tft_cs, reset=tft_rst)
display = ST7789(display_bus, width=320, height=170, colstart=35, rotation=90)

#main_group = displayio.Group(scale=1, x=180, y=20)

plot_group = displayio.Group(scale=1, x=80, y=80)

color_bitmap = displayio.Bitmap(display.width, display.height, 1)
color_palette = displayio.Palette(1)
color_palette[0] = 0x111111  # Moss Green
bg_sprite = displayio.TileGrid(color_bitmap, pixel_shader=color_palette, x=0, y=0)
circle80 = Circle(0, 0, 80, fill=0x444422, outline=0xaa00FF)
line0=Line(-80,0,80,0,0xaaaaaa)
line1=Line(0,-80,0,80,0xaaaaaa)
circle0 = Circle(-78, 0, 3, fill=0xee0000)
plot_group.append(bg_sprite)
plot_group.append(circle80)
plot_group.append(line0)
plot_group.append(line1)
plot_group.append(circle0)

color = 0xffffff
circle_radius = 6
posx=0
posy=0
leftSpeed=0
rightSpeed=0

plotG = displayio.Group(scale=1, x=2, y=2)
circle = Circle(posx, posy, circle_radius, fill=0x00FF00, outline=0x5555FF)
plot_group.append(circle)

text_group = displayio.Group(scale=1, x=100, y=-70)
text = "Position"
font = bitmap_font.load_font("/Helvetica-Bold-16.bdf")
text_area1 = label.Label(font, text=text, color=0xFF00FF)
text_group.append(text_area1)  # Subgroup for text
plot_group.append(text_group)

text_group = displayio.Group(scale=1, x=100, y=-50)
text = 'start'
text_area2 = label.Label(font, text=text, color=0xFFFF00)
text_group.append(text_area2)  # Subgroup for text
plot_group.append(text_group)

text_group = displayio.Group(scale=2, x=100, y=-10)
text = 'left'
text_area3 = label.Label(font, text=text, color=0xFFFF00)
text_group.append(text_area3)  # Subgroup for text
plot_group.append(text_group)

text_group = displayio.Group(scale=2, x=100, y=20)
text = 'right'
text_area4 = label.Label(font, text=text, color=0xFFFF00)
text_group.append(text_area4)  # Subgroup for text
plot_group.append(text_group)

text_group = displayio.Group(scale=1, x=80, y=50)
text = 'leftMotor'
text_area5 = label.Label(font, text=text, color=0xFF5555)
text_group.append(text_area5)  # Subgroup for text
plot_group.append(text_group)

text_group = displayio.Group(scale=1, x=80, y=67)
text = 'rightMotor'
text_area6 = label.Label(font, text=text, color=0xFF5555)
text_group.append(text_area6)  # Subgroup for text
plot_group.append(text_group)

display.show(plot_group)

def circlePlot(Theta,Vector,color):
    x=Vector*80*math.cos(Theta*3.14/180)
    y=Vector*80*math.sin(Theta*3.14/180)
    print('x:  {:.4f} y: {:.4f}'.format(x,y))
    circle.x=int(x)
    circle.y=int(y)

ble = BLERadio()
ble.name ="Feather nRF52840 Express"
connection = None

pwm10 = pwmio.PWMOut(board.D10, frequency = 240_000)
pwm11 = pwmio.PWMOut(board.D11, frequency = 240_000)
pwm10.duty_cycle = 1  # output level 3.3 to 0V
pwm11.duty_cycle = 1

while True:
    if not connection:
        print("Scanning")
        for adv in ble.start_scan(ProvideServicesAdvertisement):
            addr = adv.address
            s = ProvideServicesAdvertisement.matches
            #address = str(addr)[9:26]
            #print(address, adv)
            if SensorService in adv.services:
                connection = ble.connect(adv)
                print("Connected")
                break
            print(".")
        ble.stop_scan()
        print("stopped scan")
        if connection and connection.connected:
            service = connection[SensorService]
            while connection.connected:
                Theta = service.sensorsTheta
                Vector = service.sensorsVector
                if Vector>1:
                    Vector = 1.0
                #print('Theta:  {:.1f} Vector: {:.2f}'.format(Theta,Vector))
                Theta = Theta * -1
                circlePlot(Theta,Vector,0xff0000)
                #print('Theta::  {:.1f} Vector: {:.2f}'.format(Theta,Vector))

                if -180 <= Theta < -90:
                    Theta = -1*Theta -90
                elif -90 <= Theta < 0:
                    Theta = -1*Theta + 270
                elif 0 <= Theta < 180:
                    Theta = -1*Theta + 270

                print('Theta=  {:.1f} Vector= {:.2f}'.format(Theta,Vector))
                text_area1.text = 'Ve:'+str(round(Vector,2))
                text_area2.text = 'Th:'+str(round(Theta,2))
                print("")
                Theta = Theta*3.14/180  # dgree to radians
                print('radians Theta:  {:.1f}'.format(Theta))

                leftSpeed  = Vector * math.cos(Theta-3.14/4.0)  # 0 to 1.00
                text_area3.text = 'L:'+str(round(leftSpeed,2))
                if leftSpeed < 0:
                    leftSpeed = abs(leftSpeed)
                else:
                    leftSpeed = leftSpeed * 2
                text_area5.text = 'L:'+str(round(leftSpeed,2))
                #print(leftSpeed)

                #print(int(leftSpeed * 65000.0))
                pwm10.duty_cycle = int(leftSpeed * 65536.0/2.0)

                rightSpeed = Vector * math.sin(Theta-3.14/4.0)
                text_area4.text = 'R:'+str(round(rightSpeed,2))
                if rightSpeed < 0:
                    rightSpeed = abs(rightSpeed)
                else:
                    rightSpeed = rightSpeed * 2
                text_area6.text = 'R:'+str(round(rightSpeed,2))
                pwm11.duty_cycle = int(rightSpeed * 65536.0/2.0)
                #print(pwm11.duty_cycle)
                time.sleep(0.1)
                gc.collect()

ラズパイのプログラム（maxonライブラリ C++）

　駆動方法はCyclic Synchronous Position Mode (CSP)です。この駆動方法では、必要ならばPosition offset、Torque offsetを設定し、動作に必要なのは、Target positionを与えることです。
　　EPOS4 Firmware Specification
C++のライブラリ関数ではVCS_SetPositionMust()を利用します。
　いちぶProfile_decelerationなどが残っていますが、無視されます。
　引数は、0位置からのインクリメント量です。
　このインクリメント量は、アナログ入力に比例して累積していきます。プラス値の時は増え、マイナスの時は減少します。CSP駆動では、この値に追随してモータを回転させます。

　readADC()で、アナログ・ポートのデータを読んでいます。最大1500ぐらいのデータになり、これをVCS_SetPositionMust()に渡すとすごく回転量が大きくなるので、とりあえず1/10にしています。

メインのerobo04dBLE.cpp

#include <iostream>
#include "Definitions.h"
#include <unistd.h>

void* keyHandle = 0;
char* deviceName = (char*)"EPOS4";
char* protocolStackName = (char*)"CANopen";
char* interfaceName = (char*)"CAN_mcp251x 0";
char* portName = (char*)"CAN0";

uint32_t errorCode = 0;
uint16_t nodeId = 5;
int32_t PositionIs = 0;
long TargetPosition = 0;
uint32_t NbOfBytesWritten = 0;
uint32_t NbOfBytesRead = 0;
int32_t pPositionMust = 0;
void* p1Data = 0;
void* p2Data = 0;
uint32_t distance = 100;
int32_t ADCValue1;
int32_t ADCValue2;

uint32_t readADC(int channel){
    VCS_GetObject(keyHandle, nodeId, 0x3160, 0x01, &p1Data, 2, &NbOfBytesRead,&errorCode);
    VCS_GetObject(keyHandle, nodeId, 0x3160, 0x02, &p2Data, 2, &NbOfBytesRead,&errorCode);
    ADCValue1 = (int)p1Data;  // 3.3V = 3300
    ADCValue2 = (int)p2Data;  // 0 to 1800 is mainus data. 1801 to 3300 is plus data
    if (ADCValue1 > 3300) {
        ADCValue1 = 0;
    }
    if (ADCValue2 > 3300) {
        ADCValue2 = 0;
    }
    if (ADCValue1 > 1512) {
        ADCValue1 = ADCValue1 - 1800;
    }
    else if (ADCValue1 < 1512) {
        ADCValue1 = ADCValue1 * -1;
    }
    if (ADCValue2 > 1512) {
        ADCValue2 = ADCValue2 - 1800;
    }
    else if (ADCValue2< 1512) {
        ADCValue2 = ADCValue2 * -1;
    }
    if (channel ==1) {
        distance = ADCValue1 / 10;
    }
    else if (channel ==2) {
        distance = ADCValue2 / 10;
    }
    return distance;
}

uint32_t moves(uint16_t nodeId, int32_t pPositionMust){
    VCS_SetPositionMust(keyHandle, nodeId, pPositionMust, &errorCode);  
    return errorCode;
}

uint32_t homing(uint16_t nodeId){
    VCS_ActivateHomingMode(keyHandle, nodeId, &errorCode);
    VCS_FindHome(keyHandle, nodeId, 37, &errorCode);  // Actual position
    VCS_StopHoming(keyHandle, nodeId, &errorCode);
    return errorCode;
}

uint32_t printPosition(uint16_t nodeId){
    VCS_GetPositionIs(keyHandle, nodeId, &PositionIs, &errorCode);
    printf("\n    ID=%d positionIs--- %ld\n", nodeId,PositionIs);
    return errorCode;
}

uint32_t Initialisation(uint16_t nodeId){
    // enable_state
    VCS_SendNMTService(keyHandle, nodeId, 130, &errorCode); // RESET_COMMUNICATION
    VCS_ClearFault(keyHandle, nodeId, &errorCode); 
    VCS_SetEnableState(keyHandle, nodeId, &errorCode);

    // Initialisation
    VCS_SetDisableState(keyHandle, nodeId, &errorCode);
    VCS_SetOperationMode(keyHandle, nodeId, -1, &errorCode);  // -1 to 8 (Cyclic Synchronous Position Mode)

    long Max_motor_speed = 1000;  // inc
    VCS_SetObject(keyHandle, nodeId, 0x6080, 0x00, &Max_motor_speed, 4, &NbOfBytesWritten, &errorCode);
    // skip Max gear input speed
    long Profile_deceleration = 10000;  // rpm/s
    VCS_SetObject(keyHandle, nodeId, 0x6084, 0x00, &Profile_deceleration, 4, &NbOfBytesWritten, &errorCode);
    long Quick_stop_deceleration = 10000;  // rpm/s
    VCS_SetObject(keyHandle, nodeId, 0x6085, 0x00, &Quick_stop_deceleration, 4, &NbOfBytesWritten, &errorCode);
    uint8_t Interpolation_time_period = 100;  // 100ms
    VCS_SetObject(keyHandle, nodeId, 0x60c2, 0x01, &Interpolation_time_period, 1, &NbOfBytesWritten, &errorCode);
    // Nominal torque ;207 mNm µNm
    // Motor Rated Torque is mota tekaku toruku

    int16_t Torque_offset  = 74;  // read dictionary
    int32_t Position_offset  = 0; // inc
    //int32_t Max_position_range_limit = 10000;
    VCS_SetObject(keyHandle, nodeId, 0x60b2, 0x00, &Torque_offset, 2, &NbOfBytesWritten, &errorCode);
    VCS_SetObject(keyHandle, nodeId, 0x60b0, 0x00, &Position_offset, 4, &NbOfBytesWritten, &errorCode);
    //VCS_SetObject(keyHandle, nodeId, 0x607b, 0x02, &Max_position_range_limit, 4, &NbOfBytesWritten, &errorCode);

    VCS_SetDisableState(keyHandle, nodeId, &errorCode);  // Controlword (Shutdown) 0x0006
    VCS_SetEnableState(keyHandle, nodeId, &errorCode);   // Controlword (Switch on & Enable) 0x000F

    VCS_ActivateAnalogPositionSetpoint(keyHandle, nodeId, 1, 0, 32767, &errorCode);
    VCS_ActivateAnalogPositionSetpoint(keyHandle, nodeId, 2, 0, 32767, &errorCode);
    VCS_EnableAnalogPositionSetpoint(keyHandle, nodeId, &errorCode); 

    VCS_ActivatePositionMode(keyHandle, nodeId, &errorCode);

    return errorCode;
}

uint32_t Reset_state(uint16_t nodeId){
    VCS_ResetDevice(keyHandle, nodeId, &errorCode);
    VCS_SetDisableState(keyHandle, nodeId, &errorCode);
    VCS_SendNMTService(keyHandle, nodeId, 130, &errorCode); // RESET_COMMUNICATION
    return errorCode;
}

int main(){
    printf("start EPOS4 CSP\n");

    keyHandle = VCS_OpenDevice(deviceName, protocolStackName, interfaceName, portName, &errorCode);

    if (keyHandle!=0 && errorCode == 0) {

        homing(5);
        homing(6);

        Initialisation(5);
        Initialisation(6);

        int32_t oldValue1 = 0;
        int32_t oldValue2 = 0;
        int32_t currentADCValue1 = 0;
        int32_t currentADCValue2 = 0;
        int32_t addedValue1 = 50;
        int32_t addedValue2 = 50;

      printf("\n start\n");
      printPosition(5);  printPosition(6);

      for (int i=1; i<2000; i++){
          printf("%d===\n",i);
          addedValue1 = readADC(1);
          addedValue2 = readADC(2);
          currentADCValue1 = oldValue1 + addedValue1 ;
          currentADCValue2 = oldValue2 - addedValue2 ;
          printf(":: currentADCValue1 %d oldValue1 %d addedValue1 %d\n",currentADCValue1,oldValue1,addedValue1);
          printf(":: currentADCValue2 %d oldValue2 %d addedValue2 %d\n",currentADCValue2,oldValue2,addedValue2);
          moves(5,currentADCValue1);
          moves(6,currentADCValue2);
          oldValue1 = currentADCValue1;
          oldValue2 = currentADCValue2;
          sleep(0.1);
          printPosition(5);  printPosition(6);
      }

      sleep(1);
      printf("\n----\n");
      printPosition(5);  printPosition(6);

        printf("\nReset state\n");
        Reset_state(5);
        Reset_state(6);
    }
    VCS_CloseDevice(keyHandle, &errorCode);
}

Makefile

CC = g++
CFLAGS = -I.
TARGET = erobo04dBLE
LIBS = -lEposCmd

all: $(TARGET)

$(TARGET): $(TARGET).cpp
	$(CC) -o $(TARGET) $(TARGET).cpp $(CFLAGS) $(LIBS)

clean:
	$(RM) $(TARGET)