【MiniPupper入門】Atom_liteで動かしてみた♪

今回は、少しずつMiniPupperを動かす。
最終的には、Raspi４利用までやろうと思うが、完成版でなくリサーチ版入手なので、当初は、動きがわかっているAtom_liteを利用して動かそうと思う。
# 参考
今回は、以下の参考➀のAssemblyを見ながら組み立てました。
入手やRaspiのソフト、ロボット犬の知識のために以下リンクしておきます。
➀[初心者もプロも楽しめる！自作できるロボット犬『Mini Pupperミニぷぱ』](https://www.makuake.com/project/mini_pupper/)
➁[Welcome to Mini Pupper’s documentation](https://mangdang-minipupperdocs.readthedocs-hosted.com/en/latest/index.html)
➂[ロボット犬「Mini Pupper」で遊んでみた](https://note.com/_5884/n/n14fb0b4e0228)
➃[Mini Pupperを動かすまで（ソフトウェア準備編）](https://memoteki.net/archives/4772)
# Atom_liteで動かす準備
Atom_liteとPCA9685を利用します。
結線は、通常のI2Cです。
また、付属の電源LiPo2S(7.4v?;実測)からPCA9685に電源供給
Atom_liteは開発中は、USBでWindows上のArduinoIDEを利用しています。
必要なライブラリーは、atomとAdafruit_PWMServoDriverを使います。
# 最低限のコード
四足歩行ロボット犬は、キャリブレーションが重要です。
以下のコードで、実施しました。
※コード解説は省きますが、mangdang社サーボは、サーボテスターで動かすと180度サーボでした

キャリブレーションの足の配置は、二種類ありますがべたっと座った参考➃の「現行キャリブレーションツール」の角度が合わせやすいです。
以下のコードでは、当初の45度の立ち姿のキャリブレーションのパラメータになっています。

```calibrate.c
#include <M5Atom.h>
#include <Wire.h> 
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN 102 
     // 下で設定するPWM設定周波数を4096で分割したタイミングで制御する最小パルス幅　：4096/20×0.5＝102.4　（0.5ms：SG90、0°の時のパルス幅）
#define SERVOMAX 492 
     // 下で設定するPWM設定周波数を4096で分割したタイミングで制御する最大パルス幅　：4096/20×2.4＝491.5　（2.4ms：SG90、180°の時のパルス幅）

void servo_write_func(float p0,float p1,float p2,float p3,float p4,float p5,float p6,float p7,float p8,float p9,float p10,float p11){
  servo_write(0, p0);
  servo_write(1, p1);
  servo_write(2, p2);
  servo_write(3, p3);
  servo_write(4, p4);
  servo_write(5, p5);
  servo_write(6, p6);
  servo_write(7, p7);
  servo_write(8, p8);
  servo_write(9, p9);
  servo_write(10, p10);
  servo_write(11, p11);
}

float p0_=90,p1_=90, p2_=60;
float p3_=90,p4_=90, p5_=60;
float p6_=90, p7_=100, p8_=120;
float p9_=90, p10_=90, p11_=120;

int sk =0;

void calibrate(){
    servo_write_func(
    p0_,p1_, p2_,
    p3_,p4_, p5_, 
    p6_, p7_, p8_, 
    p9_, p10_, p11_);
}

void loop() {
  calibrate();
  delay(1);
  sk += 1;
}

void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
  ang = map(ang, 0, 180, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅（150～500）に変換
  pwm.setPWM(ch, 0, ang);
}
```
そして、以下のパラメータでべったり座ったキャリブレーションができます。

```calibrate_.c
void calibrate_(){
    p0_,p1_+45, p2_+45,
    p3_,p4_-45, p5_-45, 
    p6_, p7_+45, p8_+45, 
    p9_, p10_-45, p11_-45);
}
```
この二つの校正を連続すると校正１から２への動作をします。
つまり、こういうデータ列を連続して与えれば、ロボット犬が動いてくれます。
また、サーボ回転方向もわかると思います。
※ここで指示にサーボが追いつけるように、それぞれの関数に`delay(1000);`を追記しています。

ここで一番大切なことは、キャリブレーションの位置に合わせて足の位置を正しく取り付けることです。
**基準値で合わせることもできますが、必要に応じて取り付けなおすことをお勧めします。**

```loop.c
void loop() {
  calibrate();
  calibrate_();
  delay(1);
  sk += 1;
}
```
同じように、ポーズ系の動作として、以下でステイのポーズが実現できます。

```stay.c
void stay(){
  servo_write_func(
    p0_,p1_, p2_+45,
    p3_,p4_, p5_-45, 
    p6_, p7_, p8_+25, 
    p9_, p10_, p11_-25);
  delay(1000);
}
```

### 連続動作

ポーズの連続動作として、伸びやちんちんそしてお手があります、
こまどり写真の要領ですが、それぞれ以下で実現できました。
#### 伸び

```nobi.c
void nobi(){
  servo_write_func(
    p0_,p1_+45, p2_+45,
    p3_,p4_-45, p5_-45, 
    p6_, p7_+45, p8_+45, 
    p9_, p10_-45, p11_-45);
  delay(2000);
  servo_write_func(
    p0_,p1_+45, p2_+45,
    p3_,p4_-45, p5_-45, 
    p6_, p7_, p8_+25, 
    p9_, p10_, p11_-25);
  delay(1000);  
  servo_write_func(
    p0_,p1_-0, p2_+85,
    p3_,p4_+0, p5_-85, 
    p6_, p7_, p8_+25, 
    p9_, p10_, p11_-25);
  delay(2000);
}
```
#### ちんちん

```chinchin.c
void chinchin(){
  servo_write_func(
    p0_, p1_+45, p2_+45,
    p3_, p4_-45, p5_-45, 
    p6_, p7_+45, p8_+45, 
    p9_, p10_-45, p11_-45);
  delay(2000);
  servo_write_func(
    p0_,p1_-45, p2_+45,
    p3_,p4_+45, p5_-45, 
    p6_, p7_+45, p8_-0, 
    p9_, p10_-45, p11_+0);
  delay(1000);  
  servo_write_func(
    p0_,p1_+45, p2_+45,
    p3_,p4_-45, p5_-45, 
    p6_, p7_+60, p8_-30, 
    p9_, p10_-60, p11_+30);
  delay(2000);
}
```
#### お手
```ote.c
void ote(){
  servo_write_func(
    p0_,p1_, p2_+45,
    p3_,p4_, p5_-45, 
    p6_, p7_, p8_+25, 
    p9_, p10_, p11_-25);
  delay(1000);
  servo_write_func(
    p0_-10,p1_+90, p2_+85,
    p3_-10,p4_, p5_-45, 
    p6_, p7_, p8_+25, 
    p9_, p10_, p11_-25);
  delay(1000);
  servo_write_func(
    p0_-20,p1_+30, p2_+85,
    p3_-10,p4_, p5_-45, 
    p6_, p7_, p8_+25, 
    p9_, p10_, p11_-25);
  delay(2000);
}
```
# 時系列関数を利用する
連続動作を再現するために、時間に依存して数値を変える関数を導入する。

#### ジャンプ
単純にその場でジャンプする関数は、以下の通り。
前後左右の足がそろっているので、比較的簡単に実現できる。

```jump.c
void jump(){
  servo_write_func(
    p0_,pk*(sk%st-st2)+p1_, -pk*(sk%st-st2)+p2_,
    p3_,-pk*(sk%st-st2)+p4_, pk*(sk%st-st2)+p5_,
    p6_,-pk*(sk%st-st2)+p7_, pk*(sk%st-st2)+p8_,
    p9_,pk*(sk%st-st2)+p10_, -pk*(sk%st-st2)+p11_);
  delay(25);
}
```

#### 足踏み
関数で動かす場合、左右前後の足の位相を考慮して動かす。
一番簡単な足踏みは、以下のような関数で実現できる。

```asibumi.c
void asibumi(){
  st = 32;
  st2 =st*0.5;
  pk=0.5;
  servo_write_func(
    p0_,pk*(sk%st-st2)+p1_, -pk*(sk%st-st2)+p2_,
    p3_,pk*(st2-int(sk-st2)%st)+p4_, -pk*(st2-int(sk-st2)%st)+p5_,
    p6_,pk*(sk%st-st2)+p7_, -pk*(sk%st-st2)+p8_,
    p9_,pk*(st2-int(sk-st2)%st)+p10_, -pk*(st2-int(sk-st2)%st)+p11_);
  delay(50);
}
```

#### 歩行
歩行は、比較的難易度が高く、以下の関数で一応実現できる。

```walk.c
void walk(){
  st = 32;
  st2 =st*0.5;
  pk=1;
  servo_write_func(
    p0_,pk*(st2-int(sk-st2)%st)+p1_, 0.5*pk*(st2-int(sk-st2)%st)+p2_,
    p3_,pk*(sk%st-st2)+p4_, 0.5*pk*(sk%st-st2)+p5_,
    p6_,p7_, -0.8*pk*(sk%st-st2)+p8_,
    p9_,p10_, -0.8*pk*(st2-int(sk-st2)%st)+p11_);
  delay(10);
}
```
少し進化したものとして、以下のように後ろ脚も駆動させ、ゆっくり動かしてみる。
※ターゲットになるパラメーターが見えるのであとで最適解を探してみよう

```walk_.c
void walk(){
  st = 32;
  st2 =st*0.5;
  pk=1;
  servo_write_func(
    p0_,pk*(st2-int(sk-st2)%st)+p1_, 0.5*pk*(st2-int(sk-st2)%st)+p2_,
    p3_,pk*(sk%st-st2)+p4_, 0.5*pk*(sk%st-st2)+p5_,
    p6_,0.25*pk*(sk%st-st2)+p7_, -0.8*pk*(sk%st-st2)+p8_,
    p9_,0.25*pk*(st2-int(sk-st2)%st)+p10_, -0.8*pk*(st2-int(sk-st2)%st)+p11_);
  delay(50);
}
```
#### 左右旋回
旋回のもっとも簡単なコードは以下の通り

```LR_turn.c
void r_turn(){
  pk=0.25;
  servo_write_func(
    p0_+pk*(sk%st-st2),pk*(sk%st-st2)+p1_, p2_,
    p3_+pk*(sk%st-st2),pk*(sk%st-st2)+p4_, p5_,
    p6_+pk*(sk%st-st2),p7_, -pk*(sk%st-st2)+p8_,
    p9_+pk*(sk%st-st2),p10_, -pk*(sk%st-st2)+p11_);
  delay(2);
}

void l_turn(){
  pk=0.25;
  servo_write_func(
    p0_-pk*(sk%st-st2),-pk*(sk%st-st2)+p1_, p2_,
    p3_-pk*(sk%st-st2),-pk*(sk%st-st2)+p4_, p5_,
    p6_-pk*(sk%st-st2), p7_, pk*(sk%st-st2)+p8_,
    p9_-pk*(sk%st-st2), p10_, pk*(sk%st-st2)+p11_);
  delay(2);
}
```
#### 明日は、コントローラーでパラメーターを決めよう♪