LoginSignup
0
0

More than 1 year has passed since last update.

@jamjam(吉田 伸三)

日記；回すだけⅥ ③ C++でプログラミング ＜BLEセントラルの座標表示＞

Posted at

　前回、BLEの受信をするセントラルを作りました。このデータをグラフィック・ディスプレイに表示をします。

AdafruitのST7789

　利用したのは、1.47インチ、高密度250 ppiの172 x 320ピクセル フルカラーTFTディスプレイです。
　　　ST7789搭載 Adafruit 1.47インチ 320x172 角丸 カラーIPS TFTディスプレイ
　Adafruitには、外形の異なるST7789コントローラを用いたTFTディスプレイはほかにもあります。

接続

名称未設定-a8618ae6.png

ST7789表示器 Feather nRF52840 Sense
Vin Vin(3.3V)
3V -
GND GND
SCK SCK
MISO -
MOSI MOSI
TFTCS 5
RST 6
DC 9
SDCS -
Lite -

座標

　左上が物理的に(0.0)です。

ファイル名

　ライブラリadafruit_display_textには、Groupクラスが含まれています。サブGroupを作って入れ子にできます。
　グループにアイテムを追加するのはappendです。
　最初のGroupを作ります。
plot_group = displayio.Group(scale=1, x=80, y=80)
　(80,80)の座標を原点にします。

ファイル名

参考；CircuitPython 10行プログラミング Step9 (6) SPIとグラフィック・ディスプレイ ＜その3＞

プログラム

初期設定
# 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

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)
座標をプロットするエリアの描画

　円や線分を描画するモジュールは、adafruit_display_shapesです。なぜかadafruit_display_textとは独立しています。
　plot_groupに円や線分を追加していきます。このような記述が正しいのかは不明です。なんかめんどいです。

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)

IMGP1526.png

　右側にデータ類を表示します。
　テキストを描画するグループを作り、ベースのグループplot_groupに追加していきます。テキストオブジェクトはadafruit_display_textモジュールに入っていて、関数Groupのもつ変数、x、y、textを操作できます。位置を指定するx,yは、初期のままで、text変数に測定値を代入しています。

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)

display.show(plot_group)

IMGP1526a.png

座標

　一般的なx-yの座標です。

ファイル名

　送ってくるThetaは、次のように変な座標（赤色）になっています。

ファイル名

　表示をするループです。

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

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)
                text_area1.text = 'Ve:'+str(round(Vector,2))
                text_area2.text = 'Th:'+str(round(Theta,2))
                print("")

                leftSpeed  = Vector * math.cos(Theta-3.14/4.0)
                text_area3.text = 'L:'+str(round(leftSpeed,2))
                rightSpeed = Vector * math.sin(Theta-3.14/4.0)
                text_area4.text = 'R:'+str(round(rightSpeed,2))
                time.sleep(0.1)
                gc.collect()
座標を変更

　0～180、-180～0の座標を0～360に修正します。90°の位置が車の進行方向で赤いぽっちを描画しています。
　修正した後、右と左のモータの推進力（0～1）を次の式で計算します。参考

$v_{R} = $sin$(θ - \frac{\pi}{4})$
$v_{L} = $cos$(θ - \frac{\pi}{4})$

x-y4a.png

　ひとつ前のプログラムの置き換えです。

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

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)
                text_area3.text = 'L:'+str(round(leftSpeed,2))
                rightSpeed = Vector * math.sin(Theta-3.14/4.0)
                text_area4.text = 'R:'+str(round(rightSpeed,2))
                time.sleep(0.1)
                gc.collect()

　ガベージ・コレクションは自動で行われますが、ここでは明示的に処理gc.collect()を組み込みました。ただ遅くなっているだけかもしれません。

ファイル名
0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0