71歳の挑戦... ２足歩行ロボットをブラウザでリモートコントロールしてみる

　かなり前に購入したヴイストン株式会社の２足歩行ロボット「ピッコロボ」

https://www.vstone.co.jp/products/piccorobo/index.html

 をウェブブラウザ上からリモートコントロールしてみました。現在では、「XBee」や「IoT対応マイコンボード」などのオプションがあるようですが、私のは古くて、リモートコントロールが出来ないモノです。なのでこれまでは、別途購入したXBeeを２個を使い、そのモジュール間の無線通信でコントロールしていました。今回は更に進めて、マイコンボードを「Raspberry Pi Pico W」に変更し、
 
![pikkorobo.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3189926/bff7f368-a253-2bab-a8fe-e0e4ef928675.jpeg)

ブラウザから制御してみました。次の２点が今回の主題になります。

    １．「Pico W」で複数のサーボモーターを動作させる
    　　GPIOの14番、15番、16番、17番ピンを使用
      
        Servo servo[SERV_NUM];
        #define CENTER 14  // 腰
        #define RIGHT 15   // 右足
        #define LEFT 16    // 左足
        #define NECK 17    // 首
        //attach pins to each servo
        servo[0].attach(CENTER,544,2400);// 14
        servo[1].attach(RIGHT,544,2400); // 15
        servo[2].attach(LEFT),544,2400;  // 16
        servo[3].attach(NECK,544,2400);  // 17

    ２．ブラウザから１文字渡す
この２については、次の記事

https://karakuri-musha.com/inside-technology/arduino-raspberrypi-picow-httptest01/

が大変参考になりました。筆者に感謝です。これで表示されるページ内の入力欄にアルファベットの１文字（Wなど）を入力してPicoに送れば、
![pikkorobopage.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3189926/351ecd4d-1ba8-0baa-e198-6bb84b0389cd.jpeg)

タイムラグはありますが、ピッコロボ君はそれに応じた動きをします。
　次に、ArduinoIDE 2.3.2でのスケッチを示します。ただし、ピッコロボ本来のスケッチと「karakuri-musha」さんのものを単純にくっつけただけです。もし、著作権等の問題があればご指摘ください。

 ```c:Pikkorobo_WebControll.ino
#include <WiFi.h>   // Wifi制御用ライブラリ
#include <stdio.h>  // 標準入出力処理ライブラリ
#include <Servo.h>

//====================================================================================================================
//Servo Parameter
//====================================================================================================================
#define SERV_NUM 4  //number of servo for array
#define FRAME 20    //interval time from current step to next step: 20msecc

int current_angle[SERV_NUM];
int target_angle[SERV_NUM + 1];
float rotating_angle[SERV_NUM];  //rotating angle on each frame: calcurated by (target_angle - current_angle)/number of steps
int servo_trim[SERV_NUM];        //trim to adjust each servo motors's angle to center

Servo servo[SERV_NUM];
#define CENTER 14  // 腰
#define RIGHT 15   // 右足
#define LEFT 16    // 左足
#define NECK 17    // 首
#define DELAY 50

//====================================================================================================================
//Motion
//====================================================================================================================
//motion1 look around
int look_around[5][SERV_NUM + 1] = {
  { 0, 0, 0, 0, 200 },
  { 0, 0, 0, 60, 300 },
  { 0, 0, 0, 0, 300 },
  { 0, 0, 0, -60, 300 },
  { 0, 0, 0, 0, 300 }
};

//motion2 move forward
int move_forward[][SERV_NUM + 1] = {
  { 35, 0, 0, 0, 400 },
  { 35, -18, -18, 0, 200 },
  { 0, -18, -18, 0, 400 },
  { -35, -18, -18, 0, 400 },
  { -35, 18, 18, 0, 400 },
  { 0, 18, 18, 0, 400 },
  { 35, 18, 18, 0, 400 },
  { 35, -18, -18, 0, 400 },
  { 0, -18, -18, 0, 400 },
  { -35, -18, -18, 0, 400 },
  { -35, 18, 18, 0, 400 },
  { 0, 18, 18, 0, 400 }
};

//motion3 move back
int move_back[6][SERV_NUM + 1] = {
  { -35, 0, 0, 0, 200 },
  { -35, -15, -15, 0, 200 },
  { 0, -15, -15, 0, 600 },
  { 35, -15, -15, 0, 200 },
  { 35, 15, 15, 0, 400 },
  { 0, 15, 15, 0, 600 }
};

//motion4 home position
int home_position[1][SERV_NUM + 1] = { { 0, 0, 0, 0, 300 } };

//motion5 turn right
int turn_right[7][SERV_NUM + 1] = {
  { 35, 0, 0, 0, 200 },
  { 35, -15, 0, 0, 200 },
  { 0, -15, 0, 0, 600 },
  { -35, -15, 0, 0, 200 },
  { -35, -15, -15, 0, 200 },
  { -35, 0, 0, 0, 200 },
  { 0, 0, 0, 0, 600 }
};

//motion6 turn left
int turn_left[7][SERV_NUM + 1] = {
  { -35, 0, 0, 0, 200 },
  { -35, 0, 15, 0, 200 },
  { 0, 0, 15, 0, 600 },
  { 35, 0, 15, 0, 200 },
  { 35, 15, 15, 0, 200 },
  { 35, 0, 0, 0, 200 },
  { 0, 0, 0, 0, 600 }
};

//====================================================================================================================
//Servo Method
//====================================================================================================================
void initServo() {
  //set trim
  int tmp_trim[SERV_NUM] = { 0, 2, 12, -8 };  //腰 右 左 首trim to each servo
  for (int i = 0; i < SERV_NUM; i++) {
    servo_trim[i] = tmp_trim[i];
  }
  //attach pins to each servo
  servo[0].attach(CENTER, 544, 2400);  // 14
  servo[1].attach(RIGHT, 544, 2400);   // 15
  servo[2].attach(LEFT), 544, 2400;    // 16
  servo[3].attach(NECK, 544, 2400);    // 17
  //rotate all servos to center position
  for (int i = 0; i < SERV_NUM; i++) {  // ajast 90 degree
    servo[i].write(90 + servo_trim[i]);
    current_angle[i] = 0;
    target_angle[i] = 0;
  }
}

//move to next position
void moveToNextPosition() {
  //check limit
  for (int i = 0; i < SERV_NUM; i++) {
    if (target_angle[i] > 90) {
      target_angle[i] = 90;
    } else if (target_angle[i] < -90) {
      target_angle[i] = -90;
    }
  }
  //total number of steps to move to next position. i.e. 300ms/20
  int numberOfStep = target_angle[SERV_NUM] / FRAME;
  for (int i = 0; i < SERV_NUM; i++) {
    rotating_angle[i] = ((float)target_angle[i] - (float)current_angle[i]) / (float)numberOfStep;
  }

  int next_timing = millis() + FRAME;
  int current_time;
  float tmp_angle[SERV_NUM];

  for (int i = 0; i < SERV_NUM; i++) {
    tmp_angle[i] = (float)current_angle[i];
  }

  while (numberOfStep) {
    current_time = millis();
    if (current_time > next_timing) {
      for (int i = 0; i < SERV_NUM; i++) {
        tmp_angle[i] += rotating_angle[i];

        if (rotating_angle[i] < 0) {
          if (current_angle[i] > target_angle[i]) {
            current_angle[i] = (int)tmp_angle[i];
          }
        } else if (rotating_angle[i] > 0) {
          if (current_angle[i] < target_angle[i]) {
            current_angle[i] = (int)tmp_angle[i];
          } else {
            current_angle[i] = target_angle[i];
          }
        }
        servo[i].write(current_angle[i] + servo_trim[i] + 90);
      }
      next_timing = next_timing + FRAME;
      numberOfStep--;
    }
  }

  //adjust current_angle
  for (int i = 0; i < SERV_NUM; i++) {
    if (rotating_angle[i] < 0 && current_angle[i] > target_angle[i]) {
      current_angle[i] = target_angle[i];
      servo[i].write(current_angle[i] + servo_trim[i] + 90);
    } else if (rotating_angle[i] > 0 && current_angle[i] < target_angle[i]) {
      current_angle[i] = target_angle[i];
      servo[i].write(current_angle[i] + servo_trim[i] + 90);
    } else if (rotating_angle[i] < 0 && current_angle[i] < target_angle[i]) {
      current_angle[i] = target_angle[i];
      servo[i].write(current_angle[i] + servo_trim[i] + 90);
    } else if (rotating_angle[i] > 0 && current_angle[i] > target_angle[i]) {
      current_angle[i] = target_angle[i];
      servo[i].write(current_angle[i] + servo_trim[i] + 90);
    }
  }
}

//call moveToNextPosition() method continuously
void playMotion(int motion[][SERV_NUM + 1], int numberOfMotion) {

  for (int i = 0; i < numberOfMotion; i++) {
    for (int j = 0; j < SERV_NUM + 1; j++) {
      target_angle[j] = motion[i][j];
    }
    moveToNextPosition();
  }
}

// ------------------------------------------------------------
// 定数/変数　定義部　Constant / variable definition section.
// ------------------------------------------------------------
const char* ssid = "your-ssid";
const char* pass = "your-password";

int port = 80;
WiFiServer server(port);  // WiFiサーバーの生成(Http接続に使うデフォルトのポート番号)

const long TIMEOUT_TIME = 2000;        // タイムアウト時間の定義（2s=2000ms) Definition of timeout time (2s = 2000ms).
String Header;                         // HTTPリクエストの格納用 For storing HTTP requests
unsigned long CurrentTime = millis();  // 現在時刻格納用 For storing the current time.
unsigned long PreviousTime = 0;        // 前の時刻格納用 For storing the previous time.

char cmd;
// ------------------------------------------------------------
// Webリクエスト応答関数
// Run_WEB()
// ------------------------------------------------------------
void Run_WEB() {
  WiFiClient client = server.available();  // Listen for incoming clients
  if (client) {                            // new client がある場合に以下を実行
    CurrentTime = millis();                // 現在時刻の取得
    PreviousTime = CurrentTime;            // 前回時刻への現在時刻の格納

    String _CurrentLine = "";                                                   // クライアントからの受信データを保持する文字列の生成
    while (client.connected() && CurrentTime - PreviousTime <= TIMEOUT_TIME) {  // クライアントが接続している間繰り返し実行する
      CurrentTime = millis();                                                   // 現在時刻の更新
      if (client.available()) {                                                 // クライアントから読み取る情報（バイト）がある場合
        char _c = client.read();                                                // 値（バイト）を読み取り
        // Serial.write(_c);                                  // デバッグ用シリアルに表示

        Header += _c;      // リクエストに値（バイト）を格納
        if (_c == '\n') {  // 値が改行文字の場合に以下を実行
          //現在の行が空白の場合、2つの改行文字が連続して表示
          //クライアントのHTTPリクエストは終了であるため、応答を送信
          if (_CurrentLine.length() == 0) {
            int pos1 = 0;
            int pos2 = 0;

            // Header String から各閾値を取得し、変数に格納
            pos1 = Header.indexOf("massaget=", pos2);
            pos2 = Header.indexOf(" ", pos1);

            // 受信したメッセージをシリアルモニタに表示する
            String mes_str = Header.substring(pos1 + 9, pos2);  // 受信したテキストからメッセージを抽出
            mes_str.replace("+", " ");
            if (mes_str.length() <= 32) {  // テキストの長さがcharの大きさよりも小さい場合に実行
              char buf[64];
              char mes_text[32];
              mes_str.toCharArray(mes_text, 32);
              sprintf(buf, "Send Message : %s", mes_text);
              Serial.println(mes_text);
              cmd = mes_text[0];
            }

            // HTTPヘッダーは常に応答コードで始まる（例：HTTP/1.1 200 OK)
            // Content-typeでクライアントが何が来るのか知ることができその後空白行になる
            client.println("HTTP/1.1 200 OK");
            client.println("Content-type:text/html");
            client.println("Connection: close");
            client.println();

            // HTML Webページの表示
            client.println("<!DOCTYPE html><html>");
            client.println("<html lang=\"ja\">");
            client.println("<head><meta charset=\"UTF-8\" name=\"viewport\" content=\"width=device-width, initial-scale=1\">");
            client.println("<title>KARAKURI-MUSHA -Http connect test- </title>");
            client.println("<link rel=\"icon\" href=\"data:,\">");

            // CSSのスタイルの指定
            client.println("<style>");
            client.println("html { font-family: Helvetica; display: inline-block; margin: 0px auto; text-align: center;}");
            client.println(".btn_1 {background-color: #e64e20; border: none; color: white; padding: 16px 40px;text-decoration: none; font-size: 15px; margin: 2px; cursor: pointer;border-radius: 100vh;width: 150px;}");
            client.println("</style></head>");

            // Webページ本体(タイトルと説明)
            client.println("<body><h3>KARAKURI-MUSHA</h3>");
            client.println("<h3>- Http 接続テスト-</h3>");
            client.println("<form action=\"MESSAGEQ\" name=\"massageq\" method=\"get\">");
            client.println("<input type=\"text\" name=\"massaget\">");
            client.println("<p ><input type=\"submit\" value=\"SEND\" class=\"btn_1\"></a></p></form>");
            client.println("</body></html>");  // 本文末
            client.println();                  // HTTP応答は別の空白行で終了
            break;                             // Break out of the while loop
          } else {                             // if you got a newline, then clear currentLine
            _CurrentLine = "";
          }
        } else if (_c != '\r') {  // if you got anything else but a carriage return character,
          _CurrentLine += _c;     // add it to the end of the currentLine
        }
      }
    }
    Header = "";  // Clear the header variable
    client.flush();
  }
}

void setup() {
  initServo();
  // シリアルコンソールの開始　Start serial console.
  Serial.begin(9600);
  delay(1000);

  WiFi.mode(WIFI_STA);
  Serial.printf("Connecting to '%s' \n", ssid);
  WiFi.begin(ssid, pass);
  while (WiFi.status() != WL_CONNECTED) {
    Serial.print(".");
    delay(100);
  }
  server.begin();
  Serial.printf("\nConnected to WiFi\nConnect to server at %s:%d\n", WiFi.localIP().toString().c_str(), port);
}

void loop() {
  Run_WEB();  // Getting cmd from WEB
  Serial.println(cmd);
  switch (cmd) {
    case 'w':
      playMotion(move_forward, sizeof(move_forward)
                                 / ((SERV_NUM + 1) * sizeof(move_forward[0][0])));
      break;
    case 's':
      playMotion(move_back, 6);
      break;
    case 'a':
      playMotion(turn_left, 7);
      break;
    case 'd':
      playMotion(turn_right, 7);
      break;
    case 'j':
      playMotion(look_around, 5);
      break;
    case 'k':
      playMotion(home_position, 1);
      break;
  }

  cmd = 'k';
  delay(100);
}
 ```
　ただ単純にくっつけただけと言いましたが、少しの変更はしてあります。最後まで見ていただきありがとうございました。