【RasPi Pico入門】ServoとMPU6050センサーで追跡カメラを制御する♪

Raspberry Pi Picoの環境構築は、参考②に詳細がありますが、0は趣味だけど、二点注意を書いておきます。
0. ヘッドピンのハンダ付けされたものも売られているので、それを購入するのも便利
1．一度ダウンロードしてPicoにuf2ファイルをコピペするとそれ以後は、RasPi４のプログラミングからThonny Python IDEを立ち上げる
2．作成したコード名を、main.pyとしてPicoに保存すると次回電源に継げただけで自動的に起動する
###やったこと
・環境構築；Lチカ；省略
・ADCで内蔵温度計測
・サーボを動かす
・I2CでMPU6050加速度等計測
・カメラ追跡アプリを動かす
###・環境構築；Lチカ；省略
参考②に詳細に書かれているし、ググれば多くの同じ記述があるので、特にインストールは省略する。ただし、こんなに簡単にインストール出来るのはRaspberryPiのおかげだと思う。
そして、Thonny環境でのMicropythonのインストールまで参考になる。
一方、その言語仕様は普通のpythonとは少し異なるので、参考⑥⑦にリンクを張っておこうと思う。
数学関数などは、参考⑦の[Docs » MicroPython ライブラリ » math -- 数学関数](https://micropython-docs-ja.readthedocs.io/ja/latest/library/math.html)が便利です。
ただし、[制約条件](https://micropython-docs-ja.readthedocs.io/ja/latest/library/index.html#micropython-libraries)として「特定の MicroPython の亜種またはポートには、(リソースの制約またはその他の制限のために)この一般的なドキュメンテーションに記載されている機能/関数がサポートされていないことがあります。」ということです。

【参考】
①[【Raspberry Pi Pico】サーボモーターをPWMで動かす【MicroPython】](https://hellobreak.net/raspberry-pi-pico-pwm-servo-0211/)
②[Raspberry Pi PicoでMicroPythonでPWM信号でスピーカーでメロディー演奏](https://qiita.com/undo0530/items/7ce2ccc806668d911525)
③[Raspberry Pi PicoのmicroPythonでI2CとADCを使ってみる](https://qiita.com/SamAkada/items/07f4929a9fcfbfd942bc)
④[How to use I2C Pins in Raspberry Pi Pico | I2C Scanner Code](https://how2electronics.com/how-to-use-i2c-pins-in-raspberry-pi-pico-i2c-scanner/)
⑤[ラズパイ・ピコ(Raspberry Pi Pico)のmicropythonで3軸加速度センサー（ADXL345）を動作させる](https://qiita.com/tsuchima/items/cf6bd3232bf969c79ba0)
⑥[Getting started with Raspberry Pi Pico](https://projects.raspberrypi.org/en/projects/getting-started-with-the-pico)
⑦[MicroPython ライブラリ](https://micropython-docs-ja.readthedocs.io/ja/latest/library/index.html)
###・ADCで内蔵温度計測
最も簡単なアプリを動かしてみます。
####結線の仕方
内臓温度計は、単にUSBでRasPi4からPicoと結線するだけです。
####計測コード
参考③より、以下のコードで内臓温度計測ができます。
なお、ADCは温度計測の4以外に以下の3つのADCが使える
【参考】可変抵抗を利用して電圧変えてADCで読み取る簡単なPGが掲載されている
[How to use ADC in Raspberry Pi Pico | ADC Example Code](https://how2electronics.com/how-to-use-adc-in-raspberry-pi-pico-adc-example-code/)

|ADC Module|GPIO Pins|コード|
|:--:|:--:|:--:|
|ADC0|	GP26|machine.ADC(26)|
|ADC1|	GP27|machine.ADC(27)|
|ADC2|	GP28|machine.ADC(28)|
※`import machine`が必要です。

```py
# MicroPython for Rapperry Pi Pico
import machine
import utime

if __name__ == '__main__':
    sensor_temp = machine.ADC(4)    
    conversion_factor = 3.3 / (65535)

    while True:
        reading = sensor_temp.read_u16() * conversion_factor
        temperature = 27 - (reading - 0.706)/0.001721
        tempStr = "  {:5.1f}C".format(temperature)
        print(temperature)
        utime.sleep(1)
```
計測結果は以下のとおり、なお、「View-Plotter」をチェックいれておくと、Shellの右側の枠にグラフが現れます。

```
27.0444
27.51255
27.51255
27.0444
27.51255
27.98069
28.44883
...
```

###・サーボを動かす
次に、参考①のコード改変して二種類のサーボを動かします．
####結線の仕方
①USBでRasPi4からPicoに接続します。
②サーボモーターの３線の内、+を3.3v以上6v以下, －をGND、そして制御信号（PWM）を結線します。

|サーボ|＋|GND|PWM|コード|
|:-:|:-:|:-:|:-:|:-:|
|サーボ1|電池＋|電池－&GND|GP0|PWM(Pin(0))|   
|サーボ2|電池＋|電池－&GND|GP1|PWM(Pin(1))|   
※**電池－はGP38などのGNDと結線する。**電池は単三4本(6v)の電池ボックスやLIPO(4.2v)で可。
※電池供給で無いとpicoが不安定になる
※`from machine import PWM, Pin`が必要です
![PWMonpico.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/233744/ab77e4ba-9c6d-57d0-fa12-209b696ed232.png)
【参考】上図のPWM出力は以下のサイトから転載させていただきました
[Raspberry Pi Pico Pinout, specifications, datasheet in detail](https://www.etechnophiles.com/raspberry-pi-pico-pinout-specifications-datasheet-in-detail/)

通常よく見るピン配置は以下の通りです。
![RPI-Pico-Pins.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/233744/c5e511ad-edc0-82c3-1b04-af9b9d0c22ab.jpeg)


####計測コード
今回は、参考①のボードのPin　GP0,　GP1を使って、SG90を動かします。
(なお、結線は上の通り電池供給が安全です)
[データシートpdf](https://akizukidenshi.com/download/ds/towerpro/SG90_a.pdf)から、５０Hzで
「Position "0" (1.45 ms pulse) is middle, "90" (～2.4 ms pulse) is all the way to the right, "-90" (～ 0.5 ms pulse) is all the way lef」
つまり、
Duty；0．5ms pulse（−90 deg）〜1．45ms pulse(0 deg)〜2.4ms pulse(90 deg)
全幅：20ms（50Hz）なので、比は以下のように求められる。
0.5/20=0.025, 1.45/20=0.0725, 2.4/20=0.12
これに分解能の６５０２５をかけると、それぞれのdutyが得られる．

```py
from machine import PWM, Pin
import time

servo1 = PWM(Pin(0))
servo2 = PWM(Pin(1))
servo1.freq(50)
servo2.freq(50)
led_onboard = Pin(25, Pin.OUT)

max_duty = 65025
dig_0 = 0.0725    #0°
dig_90 = 0.12     #90°
dig__90 = 0.025     #-90°

servo1.duty_u16(int(max_duty*dig_0))
for i in range(10):
    servo1.duty_u16(int(max_duty*(dig_90-dig_0)*i/10))
    led_onboard.value(1) # LEDを光らせる
    time.sleep(0.5)
    led_onboard.value(0) # LEDを消す
    time.sleep(0.5)
    print(i)
    
servo1.duty_u16(int(max_duty*dig_0))

servo2.duty_u16(int(max_duty*dig_0))
for i in range(0,10):
    servo2.duty_u16(int(max_duty*(dig_90-dig_0)*i/10))
    led_onboard.value(1) # LEDを光らせる
    time.sleep(0.5)
    led_onboard.value(0) # LEDを消す
    time.sleep(0.5)
    print(i)
    
servo2.duty_u16(int(max_duty*dig_0))
```
###・I2CでMPU6050加速度等計測
最後の要素技術は、I2Cを利用して、MPU6050を使って加速度を読み取ります。
####結線の仕方
結線はpicoとMPU6050と以下のとおり結線します。

|MPU6050|GPIO Pins|コード|
|:-:|:-:|:-:|:-:|:-:|
|VCC|3V3(OUT)|-|
|GND|GND|-|
|SCL|GP13|scl = Pin(13)|
|SDA|GP12|sda = Pin(12)|
※`self.i2c = I2C(0,scl = self.scl, sda = self.sda, freq = 100000)`で初期化トリガーを掛けています
以下のように他のGPIO Pinsも使えます。

|I2C Controller|  	  GPIO Pins|
|:-:|:-:|
|I2C0 SDA|   	GP0/GP4/GP8/GP12/GP16/GP20|
|I2C0 SCL| 	GP1/GP5/GP9/GP13/GP17/GP21|
|I2C1 SDA|  	GP2/GP6/GP10/GP14/GP18/GP26|
|I2C1 SCL| 	GP3/GP7/GP11/GP15/GP19/GP27|


####計測コード
まず、以下のコードにより、アクティブなI2Cのアドレスを読み取ります。
参考④より

```py
#https://how2electronics.com/how-to-use-i2c-pins-in-raspberry-pi-pico-i2c-scanner/
import machine
sda=machine.Pin(12)
scl=machine.Pin(13)
i2c=machine.I2C(0,sda=sda, scl=scl, freq=400000)
 
print('Scan i2c bus...')
devices = i2c.scan()
 
if len(devices) == 0:
  print("No i2c device !")
else:
  print('i2c devices found:',len(devices))
 
  for device in devices:  
    print("Decimal address: ",device," | Hexa address: ",hex(device))
```
結果は、十進数で104と求められます。
そこで、以下のコードではslvAddr=104としています。
そして、レジスタリセットとクリアは[前回のコード](https://qiita.com/MuAuan/items/5ea096ea98e2c2ffcc53)やMPU6050のデータシート(前回記事参照)から
`self.writeByte(0x6B,0x80)`
`self.writeByte(0x6B,0x00)`
なお、writeByte関数は参考④から以下のコード内のように定義しています。
これで、自由にMPU6050で加速度、などが計測できます。
なお、角速度や温度なども読める完全版はおまけに掲載しています。

```py
from machine import Pin
from machine import I2C
import time
import ustruct

#unsignedを、signedに変換(16ビット限定）
def u2s(unsigneddata):
    if unsigneddata & (0x01 << 15) : 
        return -1 * ((unsigneddata ^ 0xffff) + 1)
    return unsigneddata

class mpu6050:
    def __init__(self, scl, sda):
        self.scl = scl
        self.sda = sda
        self.i2c = I2C(0,scl = self.scl, sda = self.sda, freq = 100000)
        #slv = self.i2c.scan()
        self.slvAddr = 104 #slv[0]
        # レジスタをリセットする
        self.writeByte(0x6B,0x80)   
        time.sleep(0.1)     

        # PWR_MGMT_1をクリア
        self.writeByte(0x6B,0x00) 
        time.sleep(0.1)


    def readXYZ(self):
        data    = self.readByte(0x3B ,6)
        x    = (2.0 / 0x8000) * u2s(data[0] << 8 | data[1])
        y    = (2.0 / 0x8000) * u2s(data[2] << 8 | data[3])
        z    = (2.0 / 0x8000) * u2s(data[4] << 8 | data[5])
        return (x,y,z)

    def writeByte(self, addr, data):
        d = bytearray([data])
        self.i2c.writeto_mem(self.slvAddr, addr, d)

    def readByte(self, addr, num):
        s = self.i2c.readfrom_mem(self.slvAddr, addr, num)
        return s

scl = Pin(13)
sda = Pin(12)
snsr = mpu6050(scl, sda)
while True:
    x,y,z = snsr.readXYZ()
    print('x:',x,'y:',y,'z:',z,'uint:mg')
    time.sleep(0.5)
```
###・カメラ追跡アプリを動かす
以上の要素をつなぎ合わせると、前回のカメラ追跡アプリが完成します。
ここで、面倒なのは前回記事ではnumpyで数学関数を利用しましたが、micropythonではそれが利用できないので、上述したmathというLibを利用しました。
また、matplotlibも使えないので、代わりにログ出力として以下でファイルに書き出しています。
これで、write1.txtというファイルにx,yzなどが書き込まれます。

```py
    f = open('write1.txt', 'wb')
    f.write('t:  x:   y:   z: /g ' + ' \n')
    f.write('{}'.format(t)+ ' ')
    f.write('{0:.2f} {1:.2f} {2:.2f}'.format(x,y,z)+'\n')
```
こうして、完全版は以下のようになります。
一応、前回と同様な動きでカメラとセンサーを同じ基板上に配置して、動かすとカメラが常に同一の方角を見てくれます。つまり、自分の顔を最初に入れると基盤を動かしても追跡してくれます。

```py
from machine import Pin
from machine import I2C
from machine import PWM
import time
import math as mt

class mpu6050:
    def __init__(self, scl, sda):
        self.scl = scl
        self.sda = sda
        self.i2c = I2C(0,scl = self.scl, sda = self.sda, freq = 100000)
        slv = self.i2c.scan()
        self.slvAddr = 104 #slv[0]
        # レジスタをリセットする
        self.writeByte(0x6B,0x80)   
        time.sleep(0.1)     
        
        self.writeByte(0x6B,0x00) 
        time.sleep(0.1)

    def readXYZ(self):
        data    = self.readByte(0x3B ,6)
        x    = (2.0 / 0x8000) * u2s(data[0] << 8 | data[1])
        y    = (2.0 / 0x8000) * u2s(data[2] << 8 | data[3])
        z    = (2.0 / 0x8000) * u2s(data[4] << 8 | data[5])
        return (x,y,z)
   
    def writeByte(self, addr, data):
        d = bytearray([data])
        self.i2c.writeto_mem(self.slvAddr, addr, d)

    def readByte(self, addr, num):
        #print(self.slvAddr, addr)
        #time.sleep(0.5)
        s = self.i2c.readfrom_mem(self.slvAddr, addr, num)
        #print(s)
        #time.sleep(0.5)
        return s
    
#unsignedを、signedに変換(16ビット限定）
def u2s(unsigneddata):
    if unsigneddata & (0x01 << 15) : 
        return -1 * ((unsigneddata ^ 0xffff) + 1)
    return unsigneddata

def get_angle(rawX, rawY, rawZ):
            if mt.fabs(rawZ) > 0.7:
                acc_angle_x = mt.degrees(mt.atan2(rawX, rawZ))
                acc_angle_y = mt.degrees(mt.atan2(rawY, rawZ))
                acc_angle_z = 0
            elif mt.fabs(rawX) > 0.7:
                acc_angle_x = mt.degrees(mt.atan2(rawX, rawZ)) 
                acc_angle_y = 0
                acc_angle_z = mt.degrees(mt.atan2(rawX, rawY))
            elif mt.fabs(rawY) >0.7:
                acc_angle_x = 0
                acc_angle_y = mt.degrees(mt.atan2(rawY, rawZ))
                acc_angle_z = mt.degrees(mt.atan2(rawX, rawY))
            else:
                acc_angle_x = mt.degrees(mt.atan2(rawX, rawZ))
                acc_angle_y = mt.degrees(mt.atan2(rawY, rawZ))
                acc_angle_z = mt.degrees(mt.atan2(rawX, rawY))
            return acc_angle_x,acc_angle_y,acc_angle_z

if __name__ == "__main__":
    start = time.time()
    scl = Pin(13)
    sda = Pin(12)
    servo1 = PWM(Pin(0))
    servo2 = PWM(Pin(2))
    servo1.freq(50)
    servo2.freq(50)
    max_duty = 65025
    dig_0 = 0.0725    #0°
    dig_90 = 0.12     #90°
    dig__90 = 0.0725-(0.12-0.0725)
    const_ = max_duty*(dig_90-dig_0)
    
    snsr = mpu6050(scl, sda)
    f = open('write1.txt', 'wb')
    f.write('t:  x:   y:   z: /g ' + ' \n')
    try:
        while True:
            t = time.time()- start
            print(t)
            x,y,z = snsr.readXYZ()
            acc_angle_x,acc_angle_y,acc_angle_z = get_angle(x,y,z)
            print(acc_angle_x,acc_angle_y,acc_angle_z)
            
            duty = int(max_duty*dig_0) + int((0-acc_angle_y)*const_/90)
            duty1 = int(max_duty*dig_0) + int((acc_angle_x)*const_/90)
            print(duty, duty1)
            servo1.duty_u16(duty)
            servo2.duty_u16(duty1)
            f.write('{}'.format(t)+ ' ')
            f.write('{0:.2f} {1:.2f} {2:.2f}'.format(x,y,z)+'\n')
            time.sleep(1)
    except KeyboardInterrupt:
        pass
    finally:
        f.close()
        pass
```
###・電源LIPO 1Sを利用した全体像
電源供給としてRaspberryPi4などを離れて、USBの代わりにLIPO 1S(3.7v 600mAh)を使ってみると、自律して動きました。
※保護回路としてdiodeやコンデンサを繋げていませんが、動作は確認できたレベル
上記のコードをmain.pyとして、RaspberryPi picoに格納すれば、電源が入った時点で自動的に動き始めます。

![pic2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/233744/b17afc3e-97fd-a3bd-552d-18b43bdc4f0a.png)

**電源つなぎ方：ここではV-Bus:5v（LIPO3.7v給電）GNDを利用しました；以下の各Raspberry Piの回路図や参考⑨を参考に自己責任でお願いします**
(上では5v-GNDを上記のLIPOから3.7vを供給し、サーボにもこの電源から給電してみました；picoは回路図が未掲載で試行的にその場のみでやったので、通常運用には堪えないと思います)
【参考】
⑧[Schematics for the various Raspberry Pi board versions:](https://www.raspberrypi.org/documentation/hardware/raspberrypi/schematics/README.md)
⑨[Raspberry Pi Picoの仕様書を読んでみる（５）](https://blog.boochow.com/article/rpi-pico-spec-5.html)

###おまけ
**このおまけが一番利用価値高そうです**
logは、以下のようにファイル出力が出来ました。
【参考】
[Docs » ESP8266 用クイックリファレンス » ESP8266用 MicroPythonチュートリアル » 3. 内部ファイルシステム](https://micropython-docs-ja.readthedocs.io/ja/latest/esp8266/tutorial/filesystem.html#creating-and-reading-files)
mpu6050のアドレスマップは以下のとおりです。

| Addr(Hex) |Addr(Dec.)|Register Name|R/W|Bit7 - Bit0|
|:-:|:-:|:-:|:-:|:-:|
|3B| 59| ACCEL_XOUT_H| R| ACCEL_XOUT[15:8]|
|3C| 60| ACCEL_XOUT_L| R| ACCEL_XOUT[7:0]|
|3D| 61| ACCEL_YOUT_H| R| ACCEL_YOUT[15:8]|
|3E| 62| ACCEL_YOUT_L| R| ACCEL_YOUT[7:0]|
|3F| 63| ACCEL_ZOUT_H| R| ACCEL_ZOUT[15:8]|
|40| 64| ACCEL_ZOUT_L| R| ACCEL_ZOUT[7:0]|
|41| 65| TEMP_OUT_H| R| TEMP_OUT[15:8]|
|42| 66| TEMP_OUT_L| R| TEMP_OUT[7:0]|
|43| 67| GYRO_XOUT_H| R| GYRO_XOUT[15:8]|
|44| 68| GYRO_XOUT_L| R| GYRO_XOUT[7:0]|
|45| 69| GYRO_YOUT_H| R| GYRO_YOUT[15:8]|
|46| 70| GYRO_YOUT_L| R| GYRO_YOUT[7:0]|
|47| 71| GYRO_ZOUT_H| R| GYRO_ZOUT[15:8]|
|48| 72| GYRO_ZOUT_L| R| GYRO_ZOUT[7:0]|


```mpu6050.py
from machine import Pin
from machine import I2C
import time

class mpu6050:
    def __init__(self, scl, sda):
        self.scl = scl
        self.sda = sda
        self.i2c = I2C(0,scl = self.scl, sda = self.sda, freq = 100000)
        slv = self.i2c.scan()

        self.slvAddr = 104
        # レジスタをリセットする
        self.writeByte(0x6B,0x80)   
        time.sleep(0.1)     

        # PWR_MGMT_1をクリア
        self.writeByte(0x6B,0x00) 
        time.sleep(0.1)


    def readXYZ(self):
        data    = self.readByte(0x3B ,6)
        x    = (2.0 / 0x8000) * u2s(data[0] << 8 | data[1])
        y    = (2.0 / 0x8000) * u2s(data[2] << 8 | data[3])
        z    = (2.0 / 0x8000) * u2s(data[4] << 8 | data[5])
        return (x,y,z)

    def readTemp(self):
        data    = self.readByte(0x41 ,2)
        raw    = data[0] << 8 | data[1]    # 上位ビットが先
        # Temperature in degrees C = (TEMP_OUT Register Value as a signed quantity)/340 + 36.53
        temp= u2s(raw)/340 + 36.53
        return temp
    
    def readGyro(self):
        data    = self.readByte(0x43 ,6)
        x    = (250 / 0x8000) * u2s(data[0] << 8 | data[1])
        y    = (250 / 0x8000) * u2s(data[2] << 8 | data[3])
        z    = (250 / 0x8000) * u2s(data[4] << 8 | data[5])
        return (x,y,z)
    
    def writeByte(self, addr, data):
        d = bytearray([data])
        self.i2c.writeto_mem(self.slvAddr, addr, d)

    def readByte(self, addr, num):
        s = self.i2c.readfrom_mem(self.slvAddr, addr, num)
        return s
    
#unsignedを、signedに変換(16ビット限定）
def u2s(unsigneddata):
    if unsigneddata & (0x01 << 15) : 
        return -1 * ((unsigneddata ^ 0xffff) + 1)
    return unsigneddata

if __name__ == "__main__":
    scl = Pin(13)
    sda = Pin(12)
    snsr = mpu6050(scl, sda)
    f = open('write1.txt', 'wb')
    f.write('x:   y:   z: /g temp gyrox: gyroy: gyroz: deg/s' + ' \n')
    try:
        while True:
            x,y,z = snsr.readXYZ()
            print('x:',x,'y:',y,'z:',z,'unit:mg')
            temp = snsr.readTemp()
            print('temp = {0:.2f} C'.format(temp))
            gyrox,gyroy,gyroz = snsr.readGyro()
            print('gyroX:',gyrox,'gyroy:',gyroy,'gyroz:',gyroz,'unit:deg/s')
            f.write('{0:.2f} {1:.2f} {2:.2f} {3:.2f} {4:.2f} {5:.2f} {6:.2f} '.format(x,y,z,temp,gyrox,gyroy,gyroz)+' '+' \n')
            time.sleep(0.5)
    except KeyboardInterrupt:
        pass
    finally:
        f.close()
        pass
```

```write1.txt
x:   y:   z: /g temp gyroX: gyroy: gyroz: deg/s 
-0.21 0.05 1.10 27.49 -6.09 0.70 -0.80   
-0.21 0.05 1.10 27.54 -5.94 1.01 -0.64   
-0.21 0.04 1.09 27.59 -5.99 1.02 -0.82   
-0.21 0.05 1.09 27.54 -6.16 0.61 -0.75   
-0.21 0.05 1.09 27.49 -6.00 0.79 -0.69   
-0.21 0.05 1.09 27.59 -6.07 0.65 -0.69   
```

```Thonny.Shell
>>> %Run -c $EDITOR_CONTENT
49
x: -0.2084961 y: 0.04760742 z: 1.0979 unit:mg
temp = 27.49 C
gyroX: -6.088257 gyroy: 0.7019043 gyroz: -0.8010864 unit:deg/s
42
x: -0.2116699 y: 0.0480957 z: 1.099609 unit:mg
temp = 27.54 C
gyroX: -5.943298 gyroy: 1.014709 gyroz: -0.6408691 unit:deg/s
42
x: -0.2121582 y: 0.0402832 z: 1.088623 unit:mg
temp = 27.59 C
gyroX: -5.989075 gyroy: 1.022339 gyroz: -0.8239746 unit:deg/s
42
x: -0.2133789 y: 0.04833984 z: 1.092529 unit:mg
temp = 27.54 C
gyroX: -6.164551 gyroy: 0.6103516 gyroz: -0.7476807 unit:deg/s
42
x: -0.2126465 y: 0.04541016 z: 1.09375 unit:mg
temp = 27.49 C
gyroX: -6.004333 gyroy: 0.7858276 gyroz: -0.6866455 unit:deg/s
42
```