ラズパイからarduinoへのシリアル通信で複数のうち任意のサーボを制御してみる

PC関係

タイトル通り、マニアックでニッチなことをしてみたいと思います。

先々、ロボットを作りたいおっさんです。今目指してるのがロボットの頭部分の制作で、とりあえずこうなってほしいなっていうのが、

  1. ラズパイでOpenCVを使って人の顔を検知、
  2. カメラで取得した画面の中央からどれくらい離れているかを判定
  3. arduinoに知らせ、中央に捉えるように首を動かす

といった具合。まずとりかかろうと思ったのが、ラズパイからの指示で任意のサーボを動かすところ。色々調べてみたけど、1つだけつないだサーボを動かすか、複数のサーボを一斉に同じように動かす方法しか見つからず……。複数のうちどれかっていうのは案外ない。

人で言うと首の部分なので、一緒にグリグリ無秩序に動けばいいわけじゃない。ちょっと上を向きつつ右の方を見たい、っていう秩序だった動き方をしてほしい。と言うわけで、自分でどうにかするしかないようなのでその備忘録。

今回目指したのは、

  • pythonスクリプトを実行すると、Pan用サーボかTilt用サーボの選択を迫られる
  • 続いて角度を決めるように迫られる
  • 両方決まるとシリアル通信でaruinoに指令を送る
  • arduinoはだまって従う

的な流れ。完成後の実験が↓

ラズパイ側のpythonコード

ではラズパイ側のpythonコードの全体はこちら

こんな感じ。Pyserialを利用させてもらって、通信します。事前にインストールしといてね。今回はクラス分けの勉強も兼ねて3つに分けてみた。ぶっちゃけ無理に分けなくてもいいかもしれないけど、自分的にはかなり見やすいと思うのです。では偉そうに解説してみる。

#!/usr/bin/env python3
import serial
import serial.tools.list_ports
import os

class Main():
    def __init__(self):
        sp2a = Ser2ar()
        angle = Angle()
        while True:
            ang = angle.ptangle()
            print(ang)
            try:
                sp2a.senddata(ang)
            except:
                print("error")

USB接続されたarduino(と言う名の互換機)を自動検出するためにserial.tools.list_portsを使うのでインポートしとく。Mainクラスは全体のループ処理に専念します。

class Ser2ar():
    def __init__(self):
        self.ser = serial.Serial()
        self.ser.baudrate = 9600
        self.devices = serial.tools.list_ports.comports()
        for device in self.devices:
            a = str(device).split()
            #print(a,a[0],type(a[0])) //debug
            if a[0] == "/dev/ttyUSB0" or a[0] == "/dev/ttyUSB1":
                self.ser.port = a[0]
        self.ser.open()
    def senddata(self, ang):
        self.ang = ang
        self.ser.write(self.ang.encode())

Ser2arクラスはラズパイに接続されたarduinoを探し出して自動で接続し待機。その後指令を受けてシリアル送信するクラス。

arduino互換のHL-340の場合、self.devices = serial.tools.list_ports.comports()
で検出される内容が”arduino uno”とかではなく”/dev/ttyUSB○○/”の文字列が含まれるリストになるので、for文以下で分割して条件判定してます。その際str型に変換が必要です。

senddata関数は、どのサーボをどれだけ動かすかを受けてarduinoに送る関数だよ。

class Angle():
    def __init__(self):
        self.syutan = "\n"
    def ptangle(self):
        while True:
            PT = input("Pan or Tilt? Pan → p,Tilt → t:  ")
            if PT == "p" or PT == "t":
                break
            else:
                print("type it again. p or t.")
        while True:
            angle = input("Type angle. 10 ~ 170:  ")
            try:
                n = int(angle)
                if 10 <= n <= 170:
                    data = PT + angle + self.syutan
                    break
                else:
                    print("Type it again. 10 ~ 170")
            except:
                print("Type angle again. 10 ~ 170")
        return data

if __name__ == "__main__":
    Main()

Amgleクラスはarduinoに送る文字列を生成するためのクラス。

送る文字列は、最初にどのサーボか識別するための文字(今回は”p” or “t”) + 角度に当たる数字(10~170) + aruduinoが文字列の終端を判別しやすくするための”\n”。例えば「p120」とか「t40」とかになる。

角度の値が10~170なのは、今回使ったSG90が「0°」を指定すると何故か延々とグルグル回り続けるから。実際にロボの首に使う時はもっと狭い角度でいいと思う。

arduino側のsketch

arduinoに書き込むsketchのコード全体はこちらでも見れます。何となく関数がつくれるか実験してみたかったのでこんな構成

#include 
#include 
Servo sv1;
Servo sv2;
String PTangle, kari;
unsigned int ang;

void setup() {
  Serial.begin(9600);
  pinMode(13, OUTPUT);
  digitalWrite(13, LOW);
  sv1.attach(9);
  sv2.attach(10);
  sv1.write(85);
  sv2.write(90);
}

void loop() {
  if (Serial.available() > 0) {
    PTangle = Serial.readStringUntil("\n");
    kari = PTangle.substring(1);
    ang = kari.toInt();
    if (ang >= 10 && ang <= 180) {
      if (PTangle.startsWith("p")) {
        ptang(sv1, ang);
      } else if (PTangle.startsWith("t")) {
        ptang(sv2, ang);
      }
    }
  }
}

void ptang(Servo x, int y) {
  digitalWrite(13, HIGH);
  x.write(y);
  delay(300);
  digitalWrite(13, LOW);
}

loop内では、ラズパイから送られてきた信号をまず終端文字まで受信してPTangle内に全部入れる。Serial.readStringUntilは終端文字を省いた文字列を入れてくれるらしい。便利。

その後受け取った文字列を最初の文字とその後に分割。1文字目が”p”か”t”かを吟味、後に続く数字のを元にptang関数を使って、サーボを動かす。

とりあえず、これで首振りロボットの準備ができたので、じわじわとOpenCVから指示を出せるようにしてみたい。

おつかれっした!!

コメント

タイトルとURLをコピーしました