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

Raspi Picoで先日同様な記事を書いたが、IoTをググっていると、esp8266が非常に多い印象を受ける。
そして、元々このあたりの計測にはesp8266などで十分らしいことがうすうす分かってきた。
しかも、今回は実施しないがWifiなども利用できるらしい。
ということで、今回esp8266でpicoでやった同じことをやってみたので、記録しておこうと思う。
※因みに、esp32も同様なboard（より機能的）ということでどちらを使うべきかは以下の参考⓪を参照
　また、ウワンも同じようにesp32をインストールしようとしたが、2個とも[flash read error 1000](https://github.com/espressif/esp-idf/issues/113)が出てここでは比較できないのが残念です。
※esp32においても、MicroPythonのインストールに成功したのでおまけに追記しました

ちなみに、参考⓪は分かり易いサイトで初心者でも参考になるが、日本語サイトは「ほんとに初めてesp8266を触る人」にやさしいサイトが無い。
とはいえ、この記事は以下のサイトを参照した。
普通は、esp8266でプログラミングする場合は、参考①のようにArdiunoIDEを利用して、Ardiuno(C,C++like)言語で実装するようだ。
しかし、今回はPythonでプログラミングしたいので、Raspi４に参考②の方法でestoolをインストールし、直にbinを利用して書き込む方法でMicroPythonをインストールした。
※この方法は[MicroPython本家でもこの方法を示している](https://docs.micropython.org/en/latest/esp8266/tutorial/intro.html#deploying-the-firmware)ので、正統派と考えていいと思います
【参考】
⓪[ESP32 vs ESP8266 – Pros and Cons(長所と短所)＠May 18, 2020 By Sara Santos](https://makeradvisor.com/esp32-vs-esp8266/)
①[Installing ESP8266 Board in Arduino IDE (Windows, Mac OS X, Linux)](https://randomnerdtutorials.com/how-to-install-esp8266-board-arduino-ide/)
[Arduino 日本語リファレンス](http://www.musashinodenpa.com/arduino/ref/)
②[esp8266でMicroPythonを動かす](https://qiita.com/koji14/items/25e2415234085565d4d5)
###やったこと
・環境構築
・MPU6050センサー利用とServo利用
・追跡カメラを制御する
###・環境構築
####RasPi4環境
ESP8266に新しい環境を構築するためにbinをダウンロードしてきてesptoolでファームウェアを書き込みます。
そのために、esptoolを以下でインストールします。

```
$ pip install esptool
```
※esptoolはpython3環境でも動きました。
次に、ESP8266の古いファームウェアを消去します。

```
$ sudo esptool.py --port /dev/ttyUSB0 erase_flash
esptool.py v3.0
Serial port /dev/ttyUSB0
Connecting....
Detecting chip type... ESP8266
Chip is ESP8266EX
Features: WiFi
Crystal is 26MHz
MAC: **************
Uploading stub...
Running stub...
Stub running...
Erasing flash (this may take a while)...
Chip erase completed successfully in 2.5s
Hard resetting via RTS pin...
```
以下のMicroPython.orgのサイトから、esp8266-20210418-v1.15.bin (elf, map)（03/05/21現在の安定最新MicroPythonバージョン）をダウンロードしてpiディレクトリ配下に配置します。
【参考】
③[Firmware for Generic ESP8266 module](https://micropython.org/download/esp8266/)
そして、以下のコマンドでesp8266に書き込みます。

```
 $ sudo esptool.py --port /dev/ttyUSB0 --baud 460800 write_flash --flash_size=32m 0 esp8266-20210418-v1.15.bin
WARNING: Flash size arguments in megabits like '32m' are deprecated.
Please use the equivalent size '4MB'.
Megabit arguments may be removed in a future release.
esptool.py v3.0
Serial port /dev/ttyUSB0
Connecting....
Detecting chip type... ESP8266
Chip is ESP8266EX
Features: WiFi
Crystal is 26MHz
MAC: ***************
Uploading stub...
Running stub...
Stub running...
Changing baud rate to 460800
Changed.
Configuring flash size...
Flash params set to 0x0040
Compressed 632632 bytes to 415633...
Wrote 632632 bytes (415633 compressed) at 0x00000000 in 9.6 seconds (effective 525.9 kbit/s)...
Hash of data verified.

Leaving...
Hard resetting via RTS pin...
```
※単位をmegabitsじゃなくて、4MBとしてねとおこられていますが、無事に書き込めています
シリアルモニタを開いて確認する。

```
$ sudo screen /dev/ttyUSB0 115200
```
esp8266のresetボタンを押すと、以下が表示されます。

```
...
MicroPython v1.15 on 2021-04-18; ESP module with ESP8266
Type "help()" for more information.
>>> 
```
これで、通常のpythonと同じようにコマンドを入力すれば、逐次実行ができます。

```
>>> import time
>>> print(time.time())
310
>>> 
```
####micropythonの開発環境であるThonnyをinstall
Thonnyの開発環境は、いろいろなOs上で動くようにそれぞれ提供されています。
※[インストール方法は先日の記事のとおり](https://thonny.org/)からダウンロードしてインストールします（今回はWindows版を利用してみました）。
インストールして立ち上げたら、右下の言語選択からMicroPython(esp8266)を選択します。これでMicroPython deviceとして認識されて、deviceのディレクトリにアクセスできるようになります。（図の背景はご容赦下さい）
![thonny2021-05-05 214444.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/233744/28c82480-c7ff-70b1-bbb9-b6a69a6567c0.png)
※ここで、ウワンがやった範囲だと、接続は以下の手順が確実なようです。
①Thonnyを立ち上げる
②USBでboardを接続する
③openする、または新たなＰＧを作成して、saveする
これで、接続確認と動くことが確認できると思います。
なお、installの状況により、Raspi4上のThonnyでは動くけど、Windowsでは動かないesp8266がありました。

Lチカをやってみます。
14番PinとLEDのプラス側、GNDをマイナス側に結線し、以下のコードを実行します。

```py
import machine
import time
pin=machine.Pin(14, machine.Pin.OUT)
while True:
    pin.on()
    time.sleep(1)
    pin.off()
    time.sleep(1)
```
###・MPU6050センサー利用とServo利用
ここまで来るとあとは、picoでやったこととほとんど同じです。
異なるのは、以下のI2Cの定義でidが不要なようです。
`self.i2c = I2C(scl = self.scl, sda = self.sda, freq = 100000)`
一方、Servoモータも同じようなコードですが、本家に以下のような解説があります。

```
PWM can be enabled on all pins except Pin(16). There is a single frequency for all channels, with range between 1 and 1000 (measured in Hz). The duty cycle is between 0 and 1023 inclusive.
```
【参考】
[PWM (pulse width modulation)@Quick reference for the ESP8266@MicroPython1.15](https://docs.micropython.org/en/latest/esp8266/quickref.html#pwm-pulse-width-modulation)

ということで、周波数は100Hzとし、動作範囲を見て、以下のように
`duty = int(150 - pitch)`
`duty1 = int(150 - yaw)`
としています。pitchとyawは台座のx-軸とz-軸周りの回転角度です。
###・追跡カメラを制御する
今回は、回転角pitch,rollを加速度センサー、yaw（ｚ軸周りの回転角）を角速度センサーから算出することとしました。
※今回はpitchのみ利用（roll側の回転では動かない）し、yawの補正は原点補正のみとして、それ以外の積分補正も無く、動くかな程度のものです。
【参考】
[6軸センサーから3軸回転の傾き角度算出とドリフト補正方法](https://garchiving.com/calculate-angle-of-3axis-rotation-in-6axis-sensor/)

<details><summary>コード</summary><div>

```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(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

def get_angle(rawX, rawY, rawZ):
    pitch = mt.degrees(mt.atan2(rawX, mt.sqrt(rawY*rawY+rawZ*rawZ)))
    roll = mt.degrees(mt.atan2(rawY, rawZ))
    return pitch, roll

if __name__ == "__main__":
    start = time.time()
    scl = Pin(5)
    sda = Pin(4)
    servo1 = PWM(Pin(0))
    servo2 = PWM(Pin(12))
    servo1.freq(100)
    servo2.freq(100)
    led_onboard = Pin(14, Pin.OUT)

    snsr = mpu6050(scl, sda)
    f = open('write1.txt', 'wb')
    f.write('t:  pitch:   zg_:  ' + ' \n')
    xg_,yg_,zg_ = 0,0,0
    x0,y0,z0 = snsr.readXYZ()
    z0 = z0 - 1
    try:
        while True:
            t = time.time()- start
            #print(t)
            x1,y1,z1 = snsr.readXYZ()
            x, y, z = x1-x0,y1-y0,z1-z0
            xg,yg,zg = snsr.readGyro()
            pitch, roll = get_angle(x,y,z)
            xg_ += xg
            yg_ += yg
            zg_ += zg
            #print('xg_,yg_,zg_=',xg_,yg_,zg_)
            print(zg,zg_,pitch)
            duty = int(150 - pitch)
            duty1 = int(150 - zg_) #int(roll)
            #print(duty, duty1)
            servo1.duty(duty1)
            servo2.duty(duty)
            led_onboard.on()
            f.write('{}'.format(t)+ ' ')
            f.write('{0:.2f} {1:.2f}'.format(pitch,zg_)+'\n')
            time.sleep(0.8)
            led_onboard.off()
            time.sleep(0.2)
    except KeyboardInterrupt:
        pass
    finally:
        f.close()
        pass
```
</div></details>
実行結果は以下のようになります。

![camera_servo_mpu6050-05-07.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/233744/a795250b-0afd-1f5e-16ca-1a5bbd1a11f7.png)
そして、標準出力は以下のような数値になっています。
※このうち、'write1.txt'に't:  pitch:   zg_:  'を保存しています。

<details><summary>標準出力</summary><div>

```
zg,　　　　zg_,　　　pitch
-0.160217 -0.160217 9.06498
0.556946 0.396729 7.57075
0.183105 0.579834 8.03654
0.213623 0.793457 8.50789
0.518799 1.31226 8.20365
0.457764 1.77002 7.50442
-0.0305176 1.7395 8.26609
-5.77545 -4.03595 8.01375
-0.167847 -4.2038 7.727
-6.23322 -10.437 6.7226
-0.816345 -11.2534 8.37546
0.236511 -11.0168 11.0136
0.259399 -10.7574 7.455
10.9787 0.221252 6.81365
12.085 12.3062 8.67728
7.18689 19.4931 8.86063
16.6397 36.1328 6.88407
15.7547 51.8875 6.88363
-13.0081 38.8794 7.73767
-11.8866 26.9928 7.83815
0.0457764 27.0386 8.67745
-0.0915527 26.947 27.1415
0.976563 27.9236 25.942
1.97601 29.8996 39.4848
-0.816345 29.0833 32.9303
0.709534 29.7928 19.0487
-4.15039 25.6424 14.0403
-0.411987 25.2304 9.2747
-1.51825 23.7122 -6.38299
-0.37384 23.3383 7.11945
-3.31879 20.0195 7.52914
-0.50354 19.516 1.76985
28.4348 47.9507 2.62209
7.71332 55.6641 2.18427
-14.7858 40.8783 3.15388
-1.30463 39.5737 5.20048
-9.89532 29.6783 5.69588
0.144958 29.8233 7.59914
0.25177 30.0751 6.01024
0.434875 30.5099 5.92602
0.274658 30.7846 5.6169
0.236511 31.0211 6.05784
-0.114441 30.9067 6.0632
0.579834 31.4865 6.41652
0.221252 31.7078 6.27217
0.328064 32.0358 6.28518
0.434875 32.4707 6.14895
0.152588 32.6233 6.21468
0.62561 33.2489 6.30875
0.350952 33.5999 6.10206
0.350952 33.9508 6.30138
```
</div></details>
この程度のものでも、一応ゆっくり動かせば、どうにかカメラが追跡して同一方向を向いてくれました。
###まとめ
・esp8266にMicroPythonをインストールして環境構築を実施した
・PWMでServoを動かせた
・MPU6050を利用できた
・カメラの乗っている基盤のpitchとyawに基いて、追跡カメラが出来た

・RasPi4, RasPi Pico, esp8266が出来たので、esp32とRasPi zeroでも同様に実施してそれぞれの特徴を明らかにしようと思う
・倒立振り子を完成したいと思う

###おまけ
ESP32のMicroPythonインストールもできました。
[本家コマンドのとおり](https://docs.micropython.org/en/latest/esp32/tutorial/intro.html#getting-started-with-micropython-on-the-esp32)です．

```
$ esptool.py --port /dev/ttyUSB0 erase_flash
esptool.py v3.0
Serial port /dev/ttyUSB0
Connecting....
Detecting chip type... ESP32
Chip is ESP32-D0WDQ6 (revision 1)
Features: WiFi, BT, Dual Core, 240MHz, VRef calibration in efuse, Coding Scheme None
Crystal is 40MHz
MAC: **************
Uploading stub...
Running stub...
Stub running...
Erasing flash (this may take a while)...
Chip erase completed successfully in 15.7s
Hard resetting via RTS pin...
$ esptool.py --chip esp32 --port /dev/ttyUSB0 write_flash -z 0x1000 esp32-20210418-v1.15.bin
esptool.py v3.0
Serial port /dev/ttyUSB0
Connecting......
Chip is ESP32-D0WDQ6 (revision 1)
Features: WiFi, BT, Dual Core, 240MHz, VRef calibration in efuse, Coding Scheme None
Crystal is 40MHz
MAC: **************
Uploading stub...
Running stub...
Stub running...
Configuring flash size...
Compressed 1469216 bytes to 953244...
Wrote 1469216 bytes (953244 compressed) at 0x00001000 in 84.9 seconds (effective 138.5 kbit/s)...
Hash of data verified.

Leaving...
Hard resetting via RTS pin...
```
Thonny-MicroPython上で以下が出てきます．

```
MicroPython v1.15 on 2021-04-18; ESP32 module with ESP32
Type "help()" for more information.
>>> 
```