【MiniPupper入門】PS3コントローラーでAtom_liteを制御してみた♪

前回に引き続いて、MiniPupperをAtom_liteで動かしつつ、それをPS3コントローラーで制御する。
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">ps3コントロール<a href="https://twitter.com/hashtag/mini_pupper?src=hash&amp;ref_src=twsrc%5Etfw">#mini_pupper</a> <a href="https://t.co/p61CmbzuJ2">pic.twitter.com/p61CmbzuJ2</a></p>&mdash; ウワン (@MuAuan) <a href="https://twitter.com/MuAuan/status/1499378183920963594?ref_src=twsrc%5Etfw">March 3, 2022</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

# PS3コントローラーの接続
Atom_liteなどESP32系の接続のスケッチ例は以下の通り
**これは、ArduinoIDEのライブラリマネジャーからPS3で検索すればPS3 Controller Hostをインストールできる。**
このコードをAtom_liteに書き込んで動かしてみると、それぞれのボタンやスティックを動かすとどのような反応をするか理解できる。

<details><summary>PS3Demoのコード</summary>

```PS3Demo.c
#include <Ps3Controller.h>

int player = 0;
int battery = 0;

void notify()
{
    //--- Digital cross/square/triangle/circle button events ---
    if( Ps3.event.button_down.cross )
        Serial.println("Started pressing the cross button");
    if( Ps3.event.button_up.cross )
        Serial.println("Released the cross button");

    if( Ps3.event.button_down.square )
        Serial.println("Started pressing the square button");
    if( Ps3.event.button_up.square )
        Serial.println("Released the square button");

    if( Ps3.event.button_down.triangle )
        Serial.println("Started pressing the triangle button");
    if( Ps3.event.button_up.triangle )
        Serial.println("Released the triangle button");

    if( Ps3.event.button_down.circle )
        Serial.println("Started pressing the circle button");
    if( Ps3.event.button_up.circle )
        Serial.println("Released the circle button");

    //--------------- Digital D-pad button events --------------
    if( Ps3.event.button_down.up )
        Serial.println("Started pressing the up button");
    if( Ps3.event.button_up.up )
        Serial.println("Released the up button");

    if( Ps3.event.button_down.right )
        Serial.println("Started pressing the right button");
    if( Ps3.event.button_up.right )
        Serial.println("Released the right button");

    if( Ps3.event.button_down.down )
        Serial.println("Started pressing the down button");
    if( Ps3.event.button_up.down )
        Serial.println("Released the down button");

    if( Ps3.event.button_down.left )
        Serial.println("Started pressing the left button");
    if( Ps3.event.button_up.left )
        Serial.println("Released the left button");

    //------------- Digital shoulder button events -------------
    if( Ps3.event.button_down.l1 )
        Serial.println("Started pressing the left shoulder button");
    if( Ps3.event.button_up.l1 )
        Serial.println("Released the left shoulder button");

    if( Ps3.event.button_down.r1 )
        Serial.println("Started pressing the right shoulder button");
    if( Ps3.event.button_up.r1 )
        Serial.println("Released the right shoulder button");

    //-------------- Digital trigger button events -------------
    if( Ps3.event.button_down.l2 )
        Serial.println("Started pressing the left trigger button");
    if( Ps3.event.button_up.l2 )
        Serial.println("Released the left trigger button");

    if( Ps3.event.button_down.r2 )
        Serial.println("Started pressing the right trigger button");
    if( Ps3.event.button_up.r2 )
        Serial.println("Released the right trigger button");

    //--------------- Digital stick button events --------------
    if( Ps3.event.button_down.l3 )
        Serial.println("Started pressing the left stick button");
    if( Ps3.event.button_up.l3 )
        Serial.println("Released the left stick button");

    if( Ps3.event.button_down.r3 )
        Serial.println("Started pressing the right stick button");
    if( Ps3.event.button_up.r3 )
        Serial.println("Released the right stick button");

    //---------- Digital select/start/ps button events ---------
    if( Ps3.event.button_down.select )
        Serial.println("Started pressing the select button");
    if( Ps3.event.button_up.select )
        Serial.println("Released the select button");

    if( Ps3.event.button_down.start )
        Serial.println("Started pressing the start button");
    if( Ps3.event.button_up.start )
        Serial.println("Released the start button");

    if( Ps3.event.button_down.ps )
        Serial.println("Started pressing the Playstation button");
    if( Ps3.event.button_up.ps )
        Serial.println("Released the Playstation button");


    //---------------- Analog stick value events ---------------
   if( abs(Ps3.event.analog_changed.stick.lx) + abs(Ps3.event.analog_changed.stick.ly) > 2 ){
       Serial.print("Moved the left stick:");
       Serial.print(" x="); Serial.print(Ps3.data.analog.stick.lx, DEC);
       Serial.print(" y="); Serial.print(Ps3.data.analog.stick.ly, DEC);
       Serial.println();
    }

   if( abs(Ps3.event.analog_changed.stick.rx) + abs(Ps3.event.analog_changed.stick.ry) > 2 ){
       Serial.print("Moved the right stick:");
       Serial.print(" x="); Serial.print(Ps3.data.analog.stick.rx, DEC);
       Serial.print(" y="); Serial.print(Ps3.data.analog.stick.ry, DEC);
       Serial.println();
   }

   //--------------- Analog D-pad button events ----------------
   if( abs(Ps3.event.analog_changed.button.up) ){
       Serial.print("Pressing the up button: ");
       Serial.println(Ps3.data.analog.button.up, DEC);
   }

   if( abs(Ps3.event.analog_changed.button.right) ){
       Serial.print("Pressing the right button: ");
       Serial.println(Ps3.data.analog.button.right, DEC);
   }

   if( abs(Ps3.event.analog_changed.button.down) ){
       Serial.print("Pressing the down button: ");
       Serial.println(Ps3.data.analog.button.down, DEC);
   }

   if( abs(Ps3.event.analog_changed.button.left) ){
       Serial.print("Pressing the left button: ");
       Serial.println(Ps3.data.analog.button.left, DEC);
   }

   //---------- Analog shoulder/trigger button events ----------
   if( abs(Ps3.event.analog_changed.button.l1)){
       Serial.print("Pressing the left shoulder button: ");
       Serial.println(Ps3.data.analog.button.l1, DEC);
   }

   if( abs(Ps3.event.analog_changed.button.r1) ){
       Serial.print("Pressing the right shoulder button: ");
       Serial.println(Ps3.data.analog.button.r1, DEC);
   }

   if( abs(Ps3.event.analog_changed.button.l2) ){
       Serial.print("Pressing the left trigger button: ");
       Serial.println(Ps3.data.analog.button.l2, DEC);
   }

   if( abs(Ps3.event.analog_changed.button.r2) ){
       Serial.print("Pressing the right trigger button: ");
       Serial.println(Ps3.data.analog.button.r2, DEC);
   }

   //---- Analog cross/square/triangle/circle button events ----
   if( abs(Ps3.event.analog_changed.button.triangle)){
       Serial.print("Pressing the triangle button: ");
       Serial.println(Ps3.data.analog.button.triangle, DEC);
   }

   if( abs(Ps3.event.analog_changed.button.circle) ){
       Serial.print("Pressing the circle button: ");
       Serial.println(Ps3.data.analog.button.circle, DEC);
   }

   if( abs(Ps3.event.analog_changed.button.cross) ){
       Serial.print("Pressing the cross button: ");
       Serial.println(Ps3.data.analog.button.cross, DEC);
   }

   if( abs(Ps3.event.analog_changed.button.square) ){
       Serial.print("Pressing the square button: ");
       Serial.println(Ps3.data.analog.button.square, DEC);
   }

   //---------------------- Battery events ---------------------
    if( battery != Ps3.data.status.battery ){
        battery = Ps3.data.status.battery;
        Serial.print("The controller battery is ");
        if( battery == ps3_status_battery_charging )      Serial.println("charging");
        else if( battery == ps3_status_battery_full )     Serial.println("FULL");
        else if( battery == ps3_status_battery_high )     Serial.println("HIGH");
        else if( battery == ps3_status_battery_low)       Serial.println("LOW");
        else if( battery == ps3_status_battery_dying )    Serial.println("DYING");
        else if( battery == ps3_status_battery_shutdown ) Serial.println("SHUTDOWN");
        else Serial.println("UNDEFINED");
    }

}

void onConnect(){
    Serial.println("Connected.");
}

void setup()
{
    Serial.begin(115200);

    Ps3.attach(notify);
    Ps3.attachOnConnect(onConnect);
    Ps3.begin("01:02:03:04:05:06");

    Serial.println("Ready.");
}

void loop()
{
    if(!Ps3.isConnected())
        return;

    //-------------------- Player LEDs -------------------
    Serial.print("Setting LEDs to player "); Serial.println(player, DEC);
    Ps3.setPlayer(player);

    player += 1;
    if(player > 10) player = 0;


    //------ Digital cross/square/triangle/circle buttons ------
    if( Ps3.data.button.cross && Ps3.data.button.down )
        Serial.println("Pressing both the down and cross buttons");
    if( Ps3.data.button.square && Ps3.data.button.left )
        Serial.println("Pressing both the square and left buttons");
    if( Ps3.data.button.triangle && Ps3.data.button.up )
        Serial.println("Pressing both the triangle and up buttons");
    if( Ps3.data.button.circle && Ps3.data.button.right )
        Serial.println("Pressing both the circle and right buttons");

    if( Ps3.data.button.l1 && Ps3.data.button.r1 )
        Serial.println("Pressing both the left and right bumper buttons");
    if( Ps3.data.button.l2 && Ps3.data.button.r2 )
        Serial.println("Pressing both the left and right trigger buttons");
    if( Ps3.data.button.l3 && Ps3.data.button.r3 )
        Serial.println("Pressing both the left and right stick buttons");
    if( Ps3.data.button.select && Ps3.data.button.start )
        Serial.println("Pressing both the select and start buttons");

    delay(2000);
}
```

</details>

ここで重要なことは、(以下の部分)通信相手のAtom_liteのMac AdrressをPC（Windows）上のSixaxisPairToolを利用してPS3コントローラーに書き込んでおく。

`Ps3.begin("01:02:03:04:05:06");`
Atom_liteのMac Adrressは、以下のコードをAtom_liteに書き込んで動かすとSerial出力から取得できる。

<details><summary>PS3Addressのコード</summary>

```PS3Address.c
#include <Ps3Controller.h>

void setup()
{
    Serial.begin(115200);
    Ps3.begin();

    String address = Ps3.getAddress();

    Serial.print("The ESP32's Bluetooth MAC address is: ");
    Serial.println(address);
}

void loop()
{

}
```

</details>

# PS3コントローラーでロボット犬を制御する
一番簡単なコードは、以下のようなものである。
すなわち、Digital cross/square/triangle/circle buttonを押すことによって、

* cross; お座り
* square; スタンドアップ
* triangle; 前進
* circle; 後退

をする。

<details><summary>PS3_MiniPupperのコード</summary>

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

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

//Servoのマップ関数の下限と上限の角度に相当するパルス幅相当digitの値
#define SERVOMIN 102
#define SERVOMAX 492

//時間の経過を示す変数
int sk =0;
float pk=1.; //pk=1 ; 歩幅初期値
int st=36; //st=36　；繰り返し初期値
float st2=st*0.5; //繰り返しの折り返し
float st4=st*0.25; //位相初期値
float D10=50; //delay

//paramは、動作に紐ついた変数
int param=0; 
/* 0: calibrate
 * 1; calibrate_ 
 * 2; walk
 * 3; back
 */

#include <Ps3Controller.h>
int player = 0;
int battery = 0;

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(12, p0);
  servo_write(13, p1);
  servo_write(14, 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_=70;
float p3_=95,p4_=90, p5_=60;
float p6_=85, p7_=100, p8_=110;
float p9_=86, p10_=100, p11_=110;

void notify()
{
    //--- Digital cross/square/triangle/circle button events ---
    if( Ps3.event.button_down.triangle ) {
      param = 0;
      Serial.println(param);
      Serial.println("stay");
    }
    if( Ps3.event.button_down.square ) {
      param = 1;
      Serial.println(param);
      Serial.println("standup");
    }    
    if( Ps3.event.button_down.cross ) {
      param = 2;
      Serial.println(param);
      Serial.println("walk");
    }
    if( Ps3.event.button_down.circle ) {
      param = 3;
      Serial.println(param);
      Serial.println("back");
    }
}

void onConnect(){
    Serial.println("Connected.");
}

void setup() {
  Wire.begin(26,32); //i2cのpin宣言が重要
  pwm.begin();
  pwm.setPWMFreq(50); // SG90は 50 Hz で動く（PWM周波数設定）  

  Serial.begin(115200);     //シリアル通信を115200bpsに設定

  Ps3.attach(notify);
  Ps3.attachOnConnect(onConnect);
  Ps3.begin("**:**:**:**:**:**"); //PS3コントロール　Atom　Mac
  Serial.println("Ready.");

  param = 0; 
}

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

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

void walk(){
  st = 32;
  st2 =st*0.5;
  st4 = st*0.05;
  pk=1;
  servo_write_func(
    p0_-10-0.5*pk*(st2-int(sk-st2)%st),2*pk*(st2-int(sk-st2)%st)+p1_, 1*pk*(st2-int(sk-st2)%st)+p2_,
    p3_+10-0.5*pk*(st2-int(sk-st2)%st),2*pk*(sk%st-st2)+p4_, 1*pk*(sk%st-st2)+p5_,
    p6_,-2*pk*(int(sk-st4)%st-st2)+p7_, -1*pk*(int(sk-st4)%st-st2)+p8_,
    p9_,-2*pk*(st2-int(sk-st2-st4)%st)+p10_, -1*pk*(st2-int(sk-st2-st4)%st)+p11_);
  delay(D10);
}

void back(){
  st = 32;
  st2 =st*0.5;
  st4 = st*0.05;
  pk=-1;
  servo_write_func(
    p0_,-2*pk*(int(sk-st4)%st-st2)+p1_, -1*pk*(int(sk-st4)%st-st2)+p2_,
    p3_,-2*pk*(st2-int(sk-st2-st4)%st)+p4_, -1*pk*(st2-int(sk-st2-st4)%st)+p5_,
    p6_+10-0.5*pk*(st2-int(sk-st2)%st),2*pk*(st2-int(sk-st2)%st)+p7_, 1*pk*(st2-int(sk-st2)%st)+p8_,
    p9_-10-0.5*pk*(st2-int(sk-st2)%st),2*pk*(sk%st-st2)+p10_, 1*pk*(sk%st-st2)+p11_);
  delay(D10);
}

void loop() {
  if(!Ps3.isConnected())
    return;
  Ps3.setPlayer(player);
  
  param = constrain(param,0,4);
  if(param ==0){
    calibrate_();
    delay(0);
  }
  if(param ==1){
    calibrate();
    delay(0);
  }
  if(param ==2){
    walk();
    delay(0);
  }
  if(param ==3){
    back();
    delay(0);
  }
  sk += 1;
}

//MangDang社サーボ利用の出力
void servo_write(int ch, int ang){
  ang = map(ang, 0, 180, SERVOMIN, SERVOMAX); 
  pwm.setPWM(ch, 0, ang);
}
```

</details>
次回は、もっと複雑な動作に挑戦しよう♪