本記事にはアフィリエイト広告が含まれます。

はじめに

以前、低価格なのに高トルクで高精度のPWMサーボ PTK 7465 MG を入手して大騒ぎさせていただきました。

低価格のPWMサーボのスペックがゲキ高だったー PTK 7465 MG ー　

この時に検証で製作した4脚ロボットを作ってみたいという声をチラホラいただいたので、ここで製法をまとめたいと思います。

PTK 7465 MG

PTKというメーカーによる 7465 MG というマイクロサーボ、
7465W MGというモデルもありこちらは耐水性

ここでは耐水なしの7465 MGを使用しました。

以下が驚きの PTK 7465 MG のスペック

低価格なのに電源 7.4Vで速度が 0.075 sec/60°、トルクが5.5 kg・cm !!
ヤバいって

PWM信号のパルス長 500～2500usec で180°動きます。
つまり中間点(1500usec)から±90°可動します。

構成

マイコンにはIMU内蔵のATOM Matrixを使用します。
2セルのLiPoバッテリを使用してマイコンへは5Vに降圧して供給しています。

PWMサーボ PTK 7465 MG は PCA9685 で制御します。

ここでは筐体パーツは3Dプリント品で、コントローラは専用基板を製作し用いています。

回路図

以下が本4脚ロボットのコントロール基板の回路図です。

部品

ここで使用した部品は以下のとおりです。

ATOM Matrix
　
PWMサーボ PTK 7465 MG
PCA9685
5V 降圧DCDC M78AR05-0.5

超高効率DCDCコンバーター(5V0.5A) M78AR05-0.5: 半導体秋月電子通商-電子部品・ネット通販
akizukidenshi.com
2セル 7.4V LiPoバッテリ
LiPoバッテリ用配線コネクタ
電源用スライドスイッチ

基板搭載 1608チップ部品は以下のとおり

4.7k ohm：2個 (R9, R10)
220 ohm：8個 (R1～8)
1uF：1個 (C2)

ロボット組立てに使用したネジは以下のとおりです。

M2×6mmネジ：1本　(ATOMと基板固定用)
20mm M3ネジスペーサ：4個　(基板固定用)
M3×6mmネジ：2個　(電源スイッチ固定用)
なべタッピングネジ 2×8mm：16本　(ボディとサーボ固定用)
M2×8mmネジ：8本　(サーボホーンと足筐体固定用)
M2×10mmネジ：4本　(サーボホーン固定用)
なべタッピングネジ 2×10mm：16本　(足パーツ固定用)

専用基板

以下で専用基板を販売しておりますので宜しければご購入のほどよろしくお願いいたします。
構成がシンプルであるためユニバーサル基板やPCA9685ボードの組合せなどでの自作も容易かと思います。

[電子工作キット] PTK7465MG 4脚ロボット専用基板

部品実装済みの基板も販売しております。ご検討ください。

[電子工作キット] PTK7465MG 4脚ロボット専用基板 (部品実装済み)

基板のサイズは以下のとおりで、筐体３Dモデルもこの基板サイズを元に製作しています。

3Dモデル

3Dモデルは以下よりダウンロードできます。zipファイルがダウンロードされます。
　４脚ロボット筐体用3Dモデル

ここではshoe.stlのみTPUフィラメントで出力し、残りはPLAフィラメントで出力しました。

筐体組み立てキット

3Dプリント品も販売しております。
よろしければご検討ください。

[電子工作キット] PTK7465MG 4脚ロボット筐体組み立てキット

コントロール基板

前述の回路と部品で制御基板を構築します。

コントロール基板の電源入力にスライドスイッチを介してバッテリコネクタをはんだ付けします。

ATOM Matrixと基板はピンヘッダで接続して、基板間にM2×6mmネジでspacer.stlを通して固定します。

Arduinoコード

コントロール基板に載せたATOM matrixに4脚ロボット用コードを書き込みます。

このコードでスマホなどのブラウザやPS4コントローラで4脚ロボットをコントロールできます。
ブラウザアプリにはサーボモータのオフセット調整モードも用意してあります。

ここでのコード開発環境のバージョンは以下の通りです。
　Arduino IDEバージョン：ver. 1.8.19
　ESP32ライブラリ：ver. 2.0.13
　M5Atomライブラリ：ver. 0.1.3
　Kalman Filter Library ：ver. 1.0.2
　PS4Controllerライブラリ：ver. 1.1.0
　PCA9685：秋月電子通商作成のPCA9685ライブラリを使用

#include <M5Atom.h>
#include <Kalman.h>
#include <WiFi.h>
#include <WebServer.h>
#include <Preferences.h>
#include <Kalman.h>
#include <Wire.h>
#include <PCA9685.h>            //PCA9685用ヘッダーファイル（秋月電子通商作成）
#include <PS4Controller.h>


WebServer server(80);
const char ssid[] = "4legRoboPWM";  // SSID
const char pass[] = "password";   // password
const IPAddress ip(192, 168, 55, 29);      //　IP address
const IPAddress subnet(255, 255, 255, 0);

int Mode;

float Ly = 0.0, Ry = 0.0;

Preferences preferences;



PCA9685 pwm = PCA9685(0x40);    //PCA9685のアドレス指定（アドレスジャンパ未接続時）

#define SERVOMIN 123   //最小パルス幅 (標準的なサーボパルスに設定)
#define SERVOMAX 614   //最大パルス幅 (標準的なサーボパルスに設定)

int offset[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int id[] = {-1, 7, 6, 0, 1, 8, 9, 15, 14};
int iniPos = 0;

//leg length
float L1 = 50.0, L2 = 65.0;//unit:mm

Kalman kalmanX, kalmanY;
float kalAngleX, kalAngleDotX, kalAngleY, kalAngleDotY;
unsigned long oldTime = 0, loopTime, nowTime;
float dt;
float accX = 0, accY = 0, accZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;
float theta_X = 0.0, theta_Y = 0.0;
float theta_Xdot = 0.0, theta_Ydot = 0.0;

int Jump = 0, Step = 0, Imu = 0, Stretch = 0, L = 0, R = 0, Roll = 0;
int Advance =0, Back = 0;
int period = 80, x = 60, height = 50, upHeight = 20, stride = 20;

float hX, hY, Kp = 0.4, Kd = 0.4;
float hs, hd;

int Dj1 = 120, Dj2 = 150;


//加速度センサから傾きデータ取得 [deg]
void get_theta() {
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  //傾斜角導出 単位はdeg
  theta_X =  atan2(accY, -accZ) * 57.29578f;
  theta_Y  = -atan2(-accX, -accZ) * 57.29578f;
}

//Y軸 角速度取得
void get_gyro_data() {
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  theta_Xdot = -gyroX;
  theta_Ydot = gyroY;
}


String modeBtJump = "off";
String modeBtStep = "off";
String modeBtImu = "off";
String modeBtStretch = "off";
String modeBtAd = "off";
String modeBtBack = "off";
String modeBtL = "off";
String modeBtR = "off";
String modeBtRoll = "off";


//browser
void handleRoot() {
  String temp ="<!DOCTYPE html> \n<html lang=\"ja\">";
  temp +="<head>";
  temp +="<meta charset=\"utf-8\">";
  temp +="<title>4legRoboPWM</title>";
  temp +="<meta name=\"viewport\" content=\"width=device-width, initial-scale=1\">";
  temp +="<style>";
  temp +=".container{";
  temp +="  margin: auto;";
  temp +="  text-align: center;";
  temp +="  font-size: 1.2rem;";
  temp +="}";
  temp +="span,.pm{";
  temp +="  display: inline-block;";
  temp +="  border: 1px solid #ccc;";
  temp +="  width: 50px;";
  temp +="  height: 30px;";
  temp +="  vertical-align: middle;";
  temp +="  margin-bottom: 8px;";
  temp +="}";
  temp +="span{";
  temp +="  width: 120px;";
  temp +="}";
  temp +="button{";
  temp +="  width: 100px;";
  temp +="  height: 40px;";
  temp +="  font-weight: bold;";
  temp +="  margin-bottom: 8px;";
  temp +="}";
  temp +="button.on{ background:lime; color:white; }";
  temp +=".column-3{ max-width:330px; margin:auto; text-align:center; display:flex; justify-content:space-between; flex-wrap:wrap; }";
  temp +="</style>";
  temp +="</head>";
  
  temp +="<body>";
  temp +="<div class=\"container\">";

  temp += "<a class='button' href='/IniPos'>サーボ Offset調整</a><br><br>";

  //button
  temp +="<div class=\"column-3\">";
  temp +="<button class=\"" + modeBtImu + "\" type=\"button\" ><a href=\"/imu\">IMU</a></button><br>";
  temp +="<button class=\"" + modeBtAd + "\" type=\"button\" ><a href=\"/ad\">Advance</a></button><br>";
  temp +="<button class=\"" + modeBtJump + "\" type=\"button\" ><a href=\"/Jump\">Jump</a></button><br>";
  temp +="<button class=\"" + modeBtL + "\" type=\"button\" ><a href=\"/L\">L</a></button><br>";
  temp +="<button class=\"" + modeBtStep + "\" type=\"button\" ><a href=\"/step\">Step</a></button><br>";
  temp +="<button class=\"" + modeBtR + "\" type=\"button\" ><a href=\"/R\">R</a></button><br>";
  temp +="<button class=\"" + modeBtStretch + "\" type=\"button\" ><a href=\"/stretch\">Stretch</a></button><br>";
  temp +="<button class=\"" + modeBtBack + "\" type=\"button\" ><a href=\"/back\">Back</a></button><br>";
  temp +="<button class=\"" + modeBtRoll + "\" type=\"button\" ><a href=\"/Roll\">Roll</a></button><br>";
  temp +="</div>";


  //Dj1
  temp +="Dj1 (msec)<br>";
  temp +="<a class=\"pm\" href=\"/Dj1M\">-</a>";
  temp +="<span>" + String(Dj1) + "</span>";
  temp +="<a class=\"pm\" href=\"/Dj1P\">+</a><br>";
  //Dj2
  temp +="Dj2 (msec)<br>";
  temp +="<a class=\"pm\" href=\"/Dj2M\">-</a>";
  temp +="<span>" + String(Dj2) + "</span>";
  temp +="<a class=\"pm\" href=\"/Dj2P\">+</a><br>";

  //period
  temp +="period (msec)<br>";
  temp +="<a class=\"pm\" href=\"/periodM\">-</a>";
  temp +="<span>" + String(period) + "</span>";
  temp +="<a class=\"pm\" href=\"/periodP\">+</a><br>";

  //x
  temp +="x (mm)<br>";
  temp +="<a class=\"pm\" href=\"/xM\">-</a>";
  temp +="<span>" + String(x) + "</span>";
  temp +="<a class=\"pm\" href=\"/xP\">+</a><br>";
  
  //height
  temp +="height (mm)<br>";
  temp +="<a class=\"pm\" href=\"/heightM\">-</a>";
  temp +="<span>" + String(height) + "</span>";
  temp +="<a class=\"pm\" href=\"/heightP\">+</a><br>";

  //upHeight
  temp +="upHeight (mm)<br>";
  temp +="<a class=\"pm\" href=\"/upHeightM\">-</a>";
  temp +="<span>" + String(upHeight) + "</span>";
  temp +="<a class=\"pm\" href=\"/upHeightP\">+</a><br>";

  //stride
  temp +="stride (mm)<br>";
  temp +="<a class=\"pm\" href=\"/strideM\">-</a>";
  temp +="<span>" + String(stride) + "</span>";
  temp +="<a class=\"pm\" href=\"/strideP\">+</a><br>";


  //Kp
  temp +="Kp<br>";
  temp +="<a class=\"pm\" href=\"/KpM\">-</a>";
  temp +="<span>" + String(Kp) + "</span>";
  temp +="<a class=\"pm\" href=\"/KpP\">+</a><br>";

  //Kd
  temp +="Kd<br>";
  temp +="<a class=\"pm\" href=\"/KdM\">-</a>";
  temp +="<span>" + String(Kd) + "</span>";
  temp +="<a class=\"pm\" href=\"/KdP\">+</a><br>";
   
  temp +="</div>";
  temp +="</body>";
  server.send(200, "text/HTML", temp);
}


void handleIniPos() {
  iniPos = 1;
  
  String html ="<!DOCTYPE html><html lang='ja'><head><meta charset='utf-8'>";
  html +="<meta name='viewport' content='width=device-width, initial-scale=1'>";
  html +="<title>IniPos</title>";
  html +="<style>";
  html +=".container{margin:auto;text-align:center;font-size:1.2rem;}";
  html +="span,.pm{display:inline-block;border:1px solid #ccc;width:50px;height:30px;vertical-align:middle;margin-bottom:8px;}";
  html +="span{width:120px;}button{width:100px;height:40px;font-weight:bold;margin-bottom:8px;}";
  html +=".column-3{max-width:330px;margin:auto;text-align:center;display:flex;justify-content:space-between;flex-wrap:wrap;}";
  html +="a.btn{display:inline-block;margin-top:20px;padding:10px 20px;background:#007bff;color:#fff;text-decoration:none;border-radius:6px;}";
  html +="</style></head><body><div class='container'>";
  html +="<h3>サーボ Offset調整</h3>";

  // --- offset UI ---
  for (int i = 1; i <= 8; i++) {
    html += "Offset S" + String(i) + "<br>";
    html += "<a class='pm' href='/offset" + String(i) + "M'>-</a>";
    html += "<span>" + String(offset[id[i]]) + "</span>";
    html += "<a class='pm' href='/offset" + String(i) + "P'>+</a><br>";
  }

  // --- 戻るボタン ---
  html += "<a class='btn' href='/backToMain'>戻る</a>";

  html += "</div></body></html>";
  server.send(200, "text/html", html);
}

void handleBackToMain() {
  iniPos = 0;  // ← 戻る時に0に戻す
  handleRoot(); // トップページを再表示
}


void handleL() {
  if(modeBtL == "off"){
    modeBtL = "on";
    modeBtR = "off";
    modeBtJump = "off";
    modeBtStep = "off";
    modeBtStretch = "off";
    modeBtAd = "off";
    modeBtBack = "off";
    modeBtImu = "off";
    modeBtRoll = "off";

    L = 1;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance =0;
    Back = 0;
    Roll = 0;
  }else{
    modeBtL = "off";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance =0;
    Back = 0;
    Roll = 0;
  }
  handleRoot();
}

void handleR() {
  if(modeBtR == "off"){
    modeBtL = "off";
    modeBtR = "on";
    modeBtJump = "off";
    modeBtStep = "off";
    modeBtStretch = "off";
    modeBtAd = "off";
    modeBtBack = "off";
    modeBtImu = "off";
    modeBtRoll = "off";

    L = 0;
    R = 1;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance =0;
    Back = 0;
    Roll = 0;
  }else{
    modeBtR = "off";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance =0;
    Back = 0;
    Roll = 0;
  }
  handleRoot();
}

void handleJump() {
  if(modeBtJump == "off"){
    modeBtL = "off";
    modeBtR = "off";
    modeBtJump = "on";
    modeBtStep = "off";
    modeBtStretch = "off";
    modeBtAd = "off";
    modeBtBack = "off";
    modeBtImu = "off";
    modeBtRoll = "off";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 1;
    Step = 0;
    Stretch = 0;
    Advance =0;
    Back = 0;
    Roll = 0;
  }else{
    modeBtJump = "off";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance =0;
    Back = 0;
    Roll = 0;
  }
  handleRoot();
}

void handleStep() {
  if(modeBtStep == "off"){
    modeBtL = "off";
    modeBtR = "off";
    modeBtJump = "off";
    modeBtStep = "on";
    modeBtStretch = "off";
    modeBtAd = "off";
    modeBtBack = "off";
    modeBtImu = "off";
    modeBtRoll = "off";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 1;
    Stretch = 0;
    Advance =0;
    Back = 0;
    Roll = 0;
  }else{
    modeBtStep = "off";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance =0;
    Back = 0;
    Roll = 0;
  }
  handleRoot();
}


void handleImu() {
  if(modeBtImu == "off"){
    modeBtL = "off";
    modeBtR = "off";
    modeBtJump = "off";
    modeBtStep = "off";
    modeBtStretch = "off";
    modeBtAd = "off";
    modeBtBack = "off";
    modeBtImu = "on";
    modeBtRoll = "off";

    L = 0;
    R = 0;
    Imu = 1;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance =0;
    Back = 0;
    Roll = 0;
  }else{
    modeBtImu = "off";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance =0;
    Back = 0;
    Roll = 0;
  }
  handleRoot();
}

void handleStretch() {
  if(modeBtStretch == "off"){
    modeBtL = "off";
    modeBtR = "off";
    modeBtJump = "off";
    modeBtStep = "off";
    modeBtStretch = "on";
    modeBtAd = "off";
    modeBtBack = "off";
    modeBtImu = "off";
    modeBtRoll = "off";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 1;
    Advance =0;
    Back = 0;
    Roll = 0;
  }else{
    modeBtStretch = "off";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance =0;
    Back = 0;
    Roll = 0;
  }
  handleRoot();
}

void handleAd() {
  if(modeBtAd == "off"){
    modeBtL = "off";
    modeBtR = "off";
    modeBtJump = "off";
    modeBtStep = "off";
    modeBtStretch = "off";
    modeBtAd = "on";
    modeBtBack = "off";
    modeBtImu = "off";
    modeBtRoll = "off";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance = 1;
    Back = 0;
    Roll = 0;
  }else{
    modeBtAd = "off";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance =0;
    Back = 0;
    Roll = 0;
  }
  handleRoot();
}

void handleBack() {
  if(modeBtBack == "off"){
    modeBtL = "off";
    modeBtR = "off";
    modeBtJump = "off";
    modeBtStep = "off";
    modeBtStretch = "off";
    modeBtAd = "off";
    modeBtBack = "on";
    modeBtImu = "off";
    modeBtRoll = "off";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance = 0;
    Back = 1;
    Roll = 0;
  }else{
    modeBtBack = "off";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance =0;
    Back = 0;
    Roll = 0;
  }
  handleRoot();
}

void handleRoll() {
  if(modeBtRoll == "off"){
    modeBtL = "off";
    modeBtR = "off";
    modeBtJump = "off";
    modeBtStep = "off";
    modeBtStretch = "off";
    modeBtAd = "off";
    modeBtBack = "off";
    modeBtImu = "off";
    modeBtRoll = "on";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance = 0;
    Back = 0;
    Roll = 1;
  }else{
    modeBtRoll = "off";

    L = 0;
    R = 0;
    Imu = 0;
    Jump = 0;
    Step = 0;
    Stretch = 0;
    Advance =0;
    Back = 0;
    Roll = 0;
  }
  handleRoot();
}


void handleKpM() {
  if(Kp > 0){
    Kp -= 0.1;
    preferences.putFloat("Kp", Kp);
  }
  handleRoot();
}
void handleKpP() {
  if(Kp <= 10){
    Kp += 0.1;
    preferences.putFloat("Kp", Kp);
  }
  handleRoot();
}

void handleKdM() {
  if(Kd > 0){
    Kd -= 0.1;
    preferences.putFloat("Kd", Kd);
  }
  handleRoot();
}
void handleKdP() {
  if(Kd <= 10){
    Kd += 0.1;
    preferences.putFloat("Kd", Kd);
  }
  handleRoot();
}




void handlePeriodM() {
  if(period > 5){
    period -= 5;
    preferences.putInt("period", period);
  }
  handleRoot();
}
void handlePeriodP() {
  if(period <= 1000){
    period += 5;
    preferences.putInt("period", period);
  }
  handleRoot();
}


void handlexM() {
  if(x > -115){
    x -= 5;
    preferences.putInt("x", x);
  }
  handleRoot();
}
void handlexP() {
  if(x < 115){
    x += 5;
    preferences.putInt("x", x);
  }
  handleRoot();
}


void handleHeightM() {
  if(height > -120){
    height -= 5;
    preferences.putInt("height", height);
  }
  handleRoot();
}
void handleHeightP() {
  if(height <= 120){
    height += 5;
    preferences.putInt("height", height);
  }
  handleRoot();
}

void handleUpHeightM() {
  if(upHeight > 0){
    upHeight -= 2;
    preferences.putInt("upHeight", upHeight);
  }
  handleRoot();
}
void handleUpHeightP() {
  if(upHeight <= 50){
    upHeight += 2;
    preferences.putInt("upHeight", upHeight);
  }
  handleRoot();
}

void handleStrideM() {
  if(stride > 0){
    stride -= 2;
    preferences.putInt("stride", stride);
  }
  handleRoot();
}
void handleStrideP() {
  if(stride <= 40){
    stride += 2;
    preferences.putInt("stride", stride);
  }
  handleRoot();
}


void Dj1M() {
  if(Dj1 > 10){
    Dj1 -= 10;
    preferences.putInt("Dj1", Dj1);
  }
  handleRoot();
}
void Dj1P() {
  if(Dj1 <= 1000){
    Dj1 += 10;
    preferences.putInt("Dj1", Dj1);
  }
  handleRoot();
}

void Dj2M() {
  if(Dj2 > 10){
    Dj2 -= 10;
    preferences.putInt("Dj2", Dj2);
  }
  handleRoot();
}
void Dj2P() {
  if(Dj2 <= 1000){
    Dj2 += 10;
    preferences.putInt("Dj2", Dj2);
  }
  handleRoot();
}


void offset1M() {
  if(offset[id[1]] > -300){
    offset[id[1]] -= 1;
    preferences.putInt("offset1", offset[id[1]]);
  }
  handleIniPos();
}
void offset1P() {
  if(offset[id[1]] <= 300){
    offset[id[1]] += 1;
    preferences.putInt("offset1", offset[id[1]]);
  }
  handleIniPos();
}

void offset2M() {
  if(offset[id[2]] > -300){
    offset[id[2]] -= 1;
    preferences.putInt("offset2", offset[id[2]]);
  }
  handleIniPos();
}
void offset2P() {
  if(offset[id[2]] <= 300){
    offset[id[2]] += 1;
    preferences.putInt("offset2", offset[id[2]]);
  }
  handleIniPos();
}

void offset3M() {
  if(offset[id[3]] > -300){
    offset[id[3]] -= 1;
    preferences.putInt("offset3", offset[id[3]]);
  }
  handleIniPos();
}
void offset3P() {
  if(offset[id[3]] <= 300){
    offset[id[3]] += 1;
    preferences.putInt("offset3", offset[id[3]]);
  }
  handleIniPos();
}

void offset4M() {
  if(offset[id[4]] > -300){
    offset[id[4]] -= 1;
    preferences.putInt("offset4", offset[id[4]]);
  }
  handleIniPos();
}
void offset4P() {
  if(offset[id[4]] <= 300){
    offset[id[4]] += 1;
    preferences.putInt("offset4", offset[id[4]]);
  }
  handleIniPos();
}

void offset5M() {
  if(offset[id[5]] > -300){
    offset[id[5]] -= 1;
    preferences.putInt("offset5", offset[id[5]]);
  }
  handleIniPos();
}
void offset5P() {
  if(offset[id[5]] <= 300){
    offset[id[5]] += 1;
    preferences.putInt("offset5", offset[id[5]]);
  }
  handleIniPos();
}

void offset6M() {
  if(offset[id[6]] > -300){
    offset[id[6]] -= 1;
    preferences.putInt("offset6", offset[id[6]]);
  }
  handleIniPos();
}
void offset6P() {
  if(offset[id[6]] <= 300){
    offset[id[6]] += 1;
    preferences.putInt("offset6", offset[id[6]]);
  }
  handleIniPos();
}

void offset7M() {
  if(offset[id[7]] > -300){
    offset[id[7]] -= 1;
    preferences.putInt("offset7", offset[id[7]]);
  }
  handleIniPos();
}
void offset7P() {
  if(offset[id[7]] <= 300){
    offset[id[7]] += 1;
    preferences.putInt("offset7", offset[id[7]]);
  }
  handleIniPos();
}

void offset8M() {
  if(offset[id[8]] > -300){
    offset[id[8]] -= 1;
    preferences.putInt("offset8", offset[id[8]]);
  }
  handleIniPos();
}
void offset8P() {
  if(offset[id[8]] <= 300){
    offset[id[8]] += 1;
    preferences.putInt("offset8", offset[id[8]]);
  }
  handleIniPos();
}


//Core0
void browser(void *pvParameters) {
  M5.IMU.Init();

  //フルスケールレンジ
  M5.IMU.SetAccelFsr(M5.IMU.AFS_2G);
  M5.IMU.SetGyroFsr(M5.IMU.GFS_250DPS);

  get_theta();
  kalmanX.setAngle(theta_X);
  kalmanY.setAngle(theta_Y);

  
  M5.dis.setBrightness(20);


  WiFi.softAP(ssid, pass);           
  delay(100);
  WiFi.softAPConfig(ip, ip, subnet);
  
  IPAddress myIP = WiFi.softAPIP();
  

  server.on("/", handleRoot); 

  server.on("/IniPos", handleIniPos);
  server.on("/backToMain", handleBackToMain);
  
  server.on("/Jump", handleJump);
  server.on("/step", handleStep);
  server.on("/imu", handleImu);
  server.on("/stretch", handleStretch);
  server.on("/ad", handleAd);
  server.on("/back", handleBack);
  server.on("/L", handleL);
  server.on("/R", handleR);
  server.on("/Roll", handleRoll);
  

  server.on("/KpM", handleKpM);
  server.on("/KpP", handleKpP);
  server.on("/KdM", handleKdM);
  server.on("/KdP", handleKdP);
  
  server.on("/periodM", handlePeriodM);
  server.on("/periodP", handlePeriodP);
  server.on("/xM", handlexM);
  server.on("/xP", handlexP);
  server.on("/heightM", handleHeightM);
  server.on("/heightP", handleHeightP);
  server.on("/upHeightM", handleUpHeightM);
  server.on("/upHeightP", handleUpHeightP);
  server.on("/strideM", handleStrideM);
  server.on("/strideP", handleStrideP);

  server.on("/Dj1M", Dj1M);
  server.on("/Dj1P", Dj1P);
  server.on("/Dj2M", Dj2M);
  server.on("/Dj2P", Dj2P);

  server.on("/offset1M", offset1M);
  server.on("/offset1P", offset1P);
  server.on("/offset2M", offset2M);
  server.on("/offset2P", offset2P);
  server.on("/offset3M", offset3M);
  server.on("/offset3P", offset3P);
  server.on("/offset4M", offset4M);
  server.on("/offset4P", offset4P);
  server.on("/offset5M", offset5M);
  server.on("/offset5P", offset5P);
  server.on("/offset6M", offset6M);
  server.on("/offset6P", offset6P);
  server.on("/offset7M", offset7M);
  server.on("/offset7P", offset7P);
  server.on("/offset8M", offset8M);
  server.on("/offset8P", offset8P);
  
  server.begin();
  
  for (;;){
    server.handleClient();

    //LED表示
    M5.dis.clear();
      
    if(kalAngleX > 20.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i, 0xff0000);
      }
    }else if(kalAngleX <= 20.0 && kalAngleX > 12.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i, 0x0000ff);
      }
    }else if(kalAngleX <= 12.0 && kalAngleX > 4.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i + 5, 0x0000ff);
      }
    }else if(kalAngleX <= 4.0 && kalAngleX > 1.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i + 10, 0x0000ff);
      }
    }else if(abs(kalAngleX) <= 1.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i + 10, 0x00ff00);
      }
    }else if(kalAngleX >= -4.0 && kalAngleX < -1.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i + 10, 0x0000ff);
      }
    }else if(kalAngleX >= -12.0 && kalAngleX < -4.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i + 15, 0x0000ff);
      }
    }else if(kalAngleX >= -20.0 && kalAngleX < -12.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i + 20, 0x0000ff);
      }
    }else if(kalAngleX < -20.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i + 20, 0xff0000);
      }
    }
      
    if(kalAngleY > 20.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i * 5, 0xff0000);
      }
    }else if(kalAngleY <= 20.0 && kalAngleY > 12.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i * 5, 0x0000ff);
      }
    }else if(kalAngleY <= 12.0 && kalAngleY > 4.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i * 5 + 1, 0x0000ff);
      }
    }else if(kalAngleY <= 4.0 && kalAngleY > 1.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i * 5 + 2, 0x0000ff);
      }
    }else if(abs(kalAngleY) <= 1.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i * 5 + 2, 0x00ff00);
      }
    }else if(kalAngleY >= -4.0 && kalAngleY < -1.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i * 5 + 2, 0x0000ff);
      }
    }else if(kalAngleY >= -12.0 && kalAngleY < -4.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i * 5 + 3, 0x0000ff);
      }
    }else if(kalAngleY >= -20.0 && kalAngleY < -12.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i * 5 + 4, 0x0000ff);
      }
    }else if(kalAngleY < -20.0){
      for(int i = 0; i < 5; i++){
        M5.dis.drawpix(i * 5 + 4, 0xff0000);
      }
    }


    nowTime = micros();
    loopTime = nowTime - oldTime;
    oldTime = nowTime;
    
    dt = (float)loopTime / 1000000.0; //sec
  
    get_theta();
    get_gyro_data();
    
    //カルマンフィルタ 姿勢 傾き
    kalAngleX = kalmanX.getAngle(theta_X, theta_Xdot, dt);
    kalAngleY = kalmanY.getAngle(theta_Y, theta_Ydot, dt);
    
    //カルマンフィルタ 姿勢 角速度
    kalAngleDotX = kalmanX.getRate();
    kalAngleDotY = kalmanY.getRate();

    
    disableCore0WDT();
  }
}


void setup() {
  M5.begin(true, false, true); //SerialEnable, bool I2CEnable, DisplayEnable

  Wire.begin(22, 23);  //SDA, SCL

  pwm.begin();
  pwm.setPWMFreq(60);            //PWM周期を60Hzに設定
  
  delay(200);


  Serial.println("-----------------------------");
  uint8_t btmac[6];
  esp_read_mac(btmac, ESP_MAC_BT);
  Serial.printf("[Bluetooth] Mac Address = %02X:%02X:%02X:%02X:%02X:%02X\r\n", btmac[0], btmac[1], btmac[2], btmac[3], btmac[4], btmac[5]);

  PS4.begin();

  delay(200);
  

  //parameter memory
  preferences.begin("parameter", false);

  Kp = preferences.getFloat("Kp", Kp);
  Kd = preferences.getFloat("Kd", Kd);
  
  period = preferences.getInt("period", period);
  x = preferences.getInt("x", x);
  height = preferences.getInt("height", height);
  upHeight = preferences.getInt("upHeight", upHeight);
  stride = preferences.getInt("stride", stride);

  Dj1 = preferences.getInt("Dj1", Dj1);
  Dj2 = preferences.getInt("Dj2", Dj2);

  offset[id[1]] = preferences.getInt("offset1", offset[id[1]]);
  offset[id[2]] = preferences.getInt("offset2", offset[id[2]]);
  offset[id[3]] = preferences.getInt("offset3", offset[id[3]]);
  offset[id[4]] = preferences.getInt("offset4", offset[id[4]]);
  offset[id[5]] = preferences.getInt("offset5", offset[id[5]]);
  offset[id[6]] = preferences.getInt("offset6", offset[id[6]]);
  offset[id[7]] = preferences.getInt("offset7", offset[id[7]]);
  offset[id[8]] = preferences.getInt("offset8", offset[id[8]]);
 

  //browser task
  xTaskCreatePinnedToCore(
    browser
    ,  "browser"   // A name just for humans
    ,  4096  // This stack size can be checked & adjusted by reading the Stack Highwater
    ,  NULL
    ,  1  // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
    ,  NULL 
    ,  0);
}


void loop() {
  if (PS4.isConnected()) { //PS4コントローラ
    //ジョイスティック
    Ly = PS4.LStickY() / 5.0;
    Ry = PS4.RStickY() / 5.0;


    //ボタン
    if (PS4.Triangle()) Jump = 1;
    if (PS4.Cross()) Roll = 1;


    //トリガー
    if (PS4.L1()){
      if(height > 20){
        height--;
        delay(10);
      }
    }
    if (PS4.R1()){
      if(height < 100){
        height++;
        delay(10);
      }
    }
    
    if (PS4.L2()){
      x--;
      if(x < 0) x = 0;
      delay(10);
    }
    if (PS4.R2()){
      x++;
      if(x > 100) x = 99;
      delay(10);
    }
  }

  if(fabs(Ly) > 1.0 || fabs(Ry) > 1.0){
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x - Ry * cos(tt), height - upHeight * sin(tt));
      fLIK(x + Ly * cos(tt), height);
      rLIK(x + Ly * cos(tt), height - upHeight * sin(tt));
      rRIK(x - Ry * cos(tt), height);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x + Ry * sin(tt), height - upHeight * cos(tt));
      fLIK(x - Ly * sin(tt), height);
      rLIK(x - Ly * sin(tt), height - upHeight * cos(tt));
      rRIK(x + Ry * sin(tt), height);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x + Ry * cos(tt), height);
      fLIK(x - Ly * cos(tt), height - upHeight * sin(tt));
      rLIK(x - Ly * cos(tt), height);
      rRIK(x + Ry * cos(tt), height - upHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x - Ry * sin(tt), height);
      fLIK(x + Ly * sin(tt), height - upHeight * cos(tt));
      rLIK(x + Ly * sin(tt), height);
      rRIK(x - Ry * sin(tt), height - upHeight * cos(tt));
    }
  }
  

  if(Jump){ //ジャンプ
    fRIK(20, 60);
    fLIK(20, 60);
    rLIK(-30, 80);
    rRIK(-30, 80);
    fRIK(20, 60);
    fLIK(20, 60);
    rLIK(-30, 80);
    rRIK(-30, 80);
    delay(500);

    fRIK(-10, 78);
    fLIK(-10, 78);
    rLIK(-40, 75);
    rRIK(-40, 75);
    fRIK(-10, 78);
    fLIK(-10, 78);
    rLIK(-40, 75);
    rRIK(-40, 75);
    delay(Dj1);

    fRIK(44, -35);
    fLIK(44, -35);
    rLIK(100, -50);
    rRIK(100, -50);
    fRIK(44, -35);
    fLIK(44, -35);
    rLIK(100, -50);
    rRIK(100, -50);
    delay(Dj2);

    rLIK(60, 50);
    rRIK(60, 50);
    rLIK(60, 50);
    rRIK(60, 50);
    delay(400);

    Jump = 0;
  }else if(Roll){ //起き上がり
    fLIK(65, -50);
    rLIK(65, -50);
    fRIK(65, -50);
    rRIK(65, -50);
    fLIK(65, -50);
    rLIK(65, -50);
    fRIK(65, -50);
    rRIK(65, -50);
    delay(150);
    
    fLIK(0, 115);
    rLIK(50, -20);
    fRIK(0, -115);
    rRIK(50, -20);
    fLIK(0, 115);
    rLIK(50, -20);
    fRIK(0, -115);
    rRIK(50, -20);
    delay(150);

    fLIK(0, 115);
    rLIK(50, 20);
    fRIK(0, -115);
    rRIK(50, 20);
    fLIK(0, 115);
    rLIK(50, 20);
    fRIK(0, -115);
    rRIK(50, 20);
    delay(150);

    fLIK(60, 50);
    rLIK(-5, 60);
    fRIK(60, 50);
    rRIK(-5, 60);
    fLIK(60, 50);
    rLIK(-5, 60);
    fRIK(60, 50);
    rRIK(-5, 60);
    delay(400);
    
    Roll = 0;
  }else if(Stretch){ //屈伸
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period * 8){
      tim=millis()-time_mSt;
      tt=float(tim * 2 * PI / (period * 8));
      fRIK(x, height + upHeight * sin(tt));
      fLIK(x, height + upHeight * sin(tt));
      rLIK(x, height + upHeight * sin(tt));
      rRIK(x, height + upHeight * sin(tt));
    }
  }else if(Step){ //足踏み
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x, height - upHeight * sin(tt));
      rLIK(x, height - upHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x, height - upHeight * cos(tt));
      rLIK(x, height - upHeight * cos(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      rRIK(x, height - upHeight * sin(tt));
      fLIK(x, height - upHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      rRIK(x, height - upHeight * cos(tt));
      fLIK(x, height - upHeight * cos(tt));
    }
  }else if(Advance){ //前進
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x - stride * cos(tt), height - upHeight * sin(tt));
      fLIK(x + stride * cos(tt), height);
      rLIK(x + stride * cos(tt), height - upHeight * sin(tt));
      rRIK(x - stride * cos(tt), height);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x + stride * sin(tt), height - upHeight * cos(tt));
      fLIK(x - stride * sin(tt), height);
      rLIK(x - stride * sin(tt), height - upHeight * cos(tt));
      rRIK(x + stride * sin(tt), height);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x + stride * cos(tt), height);
      fLIK(x - stride * cos(tt), height - upHeight * sin(tt));
      rLIK(x - stride * cos(tt), height);
      rRIK(x + stride * cos(tt), height - upHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x - stride * sin(tt), height);
      fLIK(x + stride * sin(tt), height - upHeight * cos(tt));
      rLIK(x + stride * sin(tt), height);
      rRIK(x - stride * sin(tt), height - upHeight * cos(tt));
    }
  }else if(Back){ //後進
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x+ stride * cos(tt), height - upHeight * sin(tt));
      fLIK(x - stride * cos(tt), height);
      rLIK(x - stride * cos(tt), height - upHeight * sin(tt));
      rRIK(x + stride * cos(tt), height);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x - stride * sin(tt), height - upHeight * cos(tt));
      fLIK(x + stride * sin(tt), height);
      rLIK(x + stride * sin(tt), height - upHeight * cos(tt));
      rRIK(x - stride * sin(tt), height);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x - stride * cos(tt), height);
      fLIK(x + stride * cos(tt), height - upHeight * sin(tt));
      rLIK(x + stride * cos(tt), height);
      rRIK(x - stride * cos(tt), height - upHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x + stride * sin(tt), height);
      fLIK(x - stride * sin(tt), height - upHeight * cos(tt));
      rLIK(x - stride * sin(tt), height);
      rRIK(x + stride * sin(tt), height - upHeight * cos(tt));
    }
  }else if(R){ //R
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x + stride * cos(tt), height - upHeight * sin(tt));
      fLIK(x + stride * cos(tt), height);
      rLIK(x + stride * cos(tt), height - upHeight * sin(tt));
      rRIK(x + stride * cos(tt), height);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x - stride * sin(tt), height - upHeight * cos(tt));
      fLIK(x - stride * sin(tt), height);
      rLIK(x - stride * sin(tt), height - upHeight * cos(tt));
      rRIK(x - stride * sin(tt), height);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x -+ stride * cos(tt), height);
      fLIK(x - stride * cos(tt), height - upHeight * sin(tt));
      rLIK(x - stride * cos(tt), height);
      rRIK(x - stride * cos(tt), height - upHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x + stride * sin(tt), height);
      fLIK(x + stride * sin(tt), height - upHeight * cos(tt));
      rLIK(x + stride * sin(tt), height);
      rRIK(x + stride * sin(tt), height - upHeight * cos(tt));
    }
  }else if(L){ //L
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x - stride * cos(tt), height - upHeight * sin(tt));
      fLIK(x - stride * cos(tt), height);
      rLIK(x - stride * cos(tt), height - upHeight * sin(tt));
      rRIK(x - stride * cos(tt), height);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x + stride * sin(tt), height - upHeight * cos(tt));
      fLIK(x + stride * sin(tt), height);
      rLIK(x + stride * sin(tt), height - upHeight * cos(tt));
      rRIK(x + stride * sin(tt), height);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x + stride * cos(tt), height);
      fLIK(x + stride * cos(tt), height - upHeight * sin(tt));
      rLIK(x + stride * cos(tt), height);
      rRIK(x + stride * cos(tt), height - upHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x - stride * sin(tt), height);
      fLIK(x - stride * sin(tt), height - upHeight * cos(tt));
      rLIK(x - stride * sin(tt), height);
      rRIK(x - stride * sin(tt), height - upHeight * cos(tt));
    }
  }else if(Imu){
    hX = hX + Kp * kalAngleX / 90.0 + Kd * kalAngleDotX / 100.0;
    hY = hY + Kp * kalAngleY / 90.0 + Kd * kalAngleDotY / 100.0;

    hs = hX + hY; 
    hd = hX - hY;

    fRIK(x, height - hd);
    fLIK(x, height - hs);
    rRIK(x, height + hs);
    rLIK(x, height + hd);
  }else if(iniPos){
    for(int i = 1; i <= 8; i++){
      servo_write(id[i], 0);
    }
  }else{ //初期姿勢
    fRIK(x, height);
    fLIK(x, height);
    rLIK(x, height);
    rRIK(x, height);
  }
}



void servo_write(int ch, float ang){ 
  ang = fmap(ang, -90.0, 90.0, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅に変換
  ang = constrain(ang + offset[ch], SERVOMIN, SERVOMAX);
  pwm.setPWM(ch, 0, ang);
}

// float対応版 map関数
float fmap(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}


//-------------------------------------------
//IK
//-------------------------------------------
void fRIK(float x, float z){
  float phi,ld, th1, th2;
  
  ld = sqrt(x*x + z*z);

  if(ld <= L1 + L2){
    phi = atan2(x, z);
  
    th1 = 90.0 - (phi + acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld))) * RAD_TO_DEG;
    th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) * RAD_TO_DEG - 90.0;

    th1 = constrain(th1, -90.0, 90.0);
    th2 = constrain(th2, -90.0, 90.0);
  
    servo_write(id[8], int(-th2)); //ヒザ
    servo_write(id[7], int(-th1));
  }
}

void fLIK(float x, float z){
  float phi,ld, th1, th2;
  
  ld = sqrt(x*x + z*z);

  if(ld <= L1 + L2){
    phi = atan2(x, z);
  
    th1 = 90.0 - (phi + acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld))) * RAD_TO_DEG;
    th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) * RAD_TO_DEG - 90.0;

    th1 = constrain(th1, -90.0, 90.0);
    th2 = constrain(th2, -90.0, 90.0);
  
    servo_write(id[6], int(th2)); //ヒザ
    servo_write(id[5], int(th1));
  }
}

void rRIK(float x, float z){
  float phi,ld, th1, th2;
  
  ld = sqrt(x*x + z*z);

  if(ld <= L1 + L2){
    phi = atan2(x, z);
  
    th1 = 90.0 - (phi + acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld))) * RAD_TO_DEG;
    th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) * RAD_TO_DEG - 90.0;

    th1 = constrain(th1, -90.0, 90.0);
    th2 = constrain(th2, -90.0, 90.0);

    servo_write(id[4], int(th2)); //ヒザ
    servo_write(id[3], int(th1));
  }
}

void rLIK(float x, float z){
  float phi,ld, th1, th2;
  
  ld = sqrt(x*x + z*z);
  
  if(ld <= L1 + L2){
    phi = atan2(x, z);
  
    th1 = 90.0 - (phi + acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld))) * RAD_TO_DEG;
    th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) * RAD_TO_DEG - 90.0;

    th1 = constrain(th1, -90.0, 90.0);
    th2 = constrain(th2, -90.0, 90.0);
  
    servo_write(id[2], int(-th2)); //ヒザ
    servo_write(id[1], int(-th1));
  }
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

608

609

610

611

612

613

614

615

616

617

618

619

620

621

622

623

624

625

626

627

628

629

630

631

632

633

634

635

636

637

638

639

640

641

642

643

644

645

646

647

648

649

650

651

652

653

654

655

656

657

658

659

660

661

662

663

664

665

666

667

668

669

670

671

672

673

674

675

676

677

678

679

680

681

682

683

684

685

686

687

688

689

690

691

692

693

694

695

696

697

698

699

700

701

702

703

704

705

706

707

708

709

710

711

712

713

714

715

716

717

718

719

720

721

722

723

724

725

726

727

728

729

730

731

732

733

734

735

736

737

738

739

740

741

742

743

744

745

746

747

748

749

750

751

752

753

754

755

756

757

758

759

760

761

762

763

764

765

766

767

768

769

770

771

772

773

774

775

776

777

778

779

780

781

782

783

784

785

786

787

788

789

790

791

792

793

794

795

796

797

798

799

800

801

802

803

804

805

806

807

808

809

810

811

812

813

814

815

816

817

818

819

820

821

822

823

824

825

826

827

828

829

830

831

832

833

834

835

836

837

838

839

840

841

842

843

844

845

846

847

848

849

850

851

852

853

854

855

856

857

858

859

860

861

862

863

864

865

866

867

868

869

870

871

872

873

874

875

876

877

878

879

880

881

882

883

884

885

886

887

888

889

890

891

892

893

894

895

896

897

898

899

900

901

902

903

904

905

906

907

908

909

910

911

912

913

914

915

916

917

918

919

920

921

922

923

924

925

926

927

928

929

930

931

932

933

934

935

936

937

938

939

940

941

942

943

944

945

946

947

948

949

950

951

952

953

954

955

956

957

958

959

960

961

962

963

964

965

966

967

968

969

970

971

972

973

974

975

976

977

978

979

980

981

982

983

984

985

986

987

988

989

990

991

992

993

994

995

996

997

998

999

1000

1001

1002

1003

1004

1005

1006

1007

1008

1009

1010

1011

1012

1013

1014

1015

1016

1017

1018

1019

1020

1021

1022

1023

1024

1025

1026

1027

1028

1029

1030

1031

1032

1033

1034

1035

1036

1037

1038

1039

1040

1041

1042

1043

1044

1045

1046

1047

1048

1049

1050

1051

1052

1053

1054

1055

1056

1057

1058

1059

1060

1061

1062

1063

1064

1065

1066

1067

1068

1069

1070

1071

1072

1073

1074

1075

1076

1077

1078

1079

1080

1081

1082

1083

1084

1085

1086

1087

1088

1089

1090

1091

1092

1093

1094

1095

1096

1097

1098

1099

1100

1101

1102

1103

1104

1105

1106

1107

1108

1109

1110

1111

1112

1113

1114

1115

1116

1117

1118

1119

1120

1121

1122

1123

1124

1125

1126

1127

1128

1129

1130

1131

1132

1133

1134

1135

1136

1137

1138

1139

1140

1141

1142

1143

1144

1145

1146

1147

1148

1149

1150

1151

1152

1153

1154

1155

1156

1157

1158

1159

1160

1161

1162

1163

1164

1165

1166

1167

1168

1169

1170

1171

1172

1173

1174

1175

1176

1177

1178

1179

1180

1181

1182

1183

1184

1185

1186

1187

1188

1189

1190

1191

1192

1193

1194

1195

1196

1197

1198

1199

1200

1201

1202

1203

1204

1205

1206

1207

1208

1209

1210

1211

1212

1213

1214

1215

1216

1217

1218

1219

1220

1221

1222

1223

1224

1225

1226

1227

1228

1229

1230

1231

1232

1233

1234

1235

1236

1237

1238

1239

1240

1241

1242

1243

1244

1245

1246

1247

1248

1249

1250

1251

1252

1253

1254

1255

1256

1257

1258

1259

1260

1261

1262

1263

1264

1265

1266

1267

1268

1269

1270

1271

1272

1273

1274

1275

1276

1277

1278

1279

1280

1281

1282

1283

1284

1285

1286

1287

1288

1289

1290

1291

1292

1293

1294

1295

1296

1297

1298

1299

1300

1301

1302

1303

1304

1305

1306

1307

1308

1309

1310

1311

1312

1313

1314

1315

1316

1317

1318

1319

1320

1321

1322

1323

1324

1325

1326

1327

1328

1329

1330

1331

1332

1333

1334

1335

1336

1337

1338

1339

1340

1341

1342

1343

1344

1345

1346

1347

1348

1349

1350

1351

1352

1353

1354

1355

1356

1357

1358

1359

1360

1361

1362

1363

1364

1365

1366

1367

1368

1369

1370

1371

1372

1373

1374

1375

1376

1377

1378

1379

1380

1381

1382

1383

1384

1385

1386

1387

1388

1389

1390

1391

1392

1393

1394

1395

1396

1397

1398

1399

1400

1401

1402

1403

1404

1405

1406

1407

1408

1409

1410

1411

1412

1413

1414

1415

1416

1417

1418

1419

1420

1421

1422

1423

1424

1425

1426

1427

1428

1429

1430

1431

1432

1433

1434

1435

1436

1437

1438

1439

1440

1441

1442

1443

1444

1445

1446

1447

1448

1449

1450

1451

1452

1453

1454

1455

1456

1457

1458

1459

1460

1461

1462

1463

1464

1465

1466

1467

1468

1469

1470

1471

1472

1473

1474

1475

1476

1477

1478

1479

1480

1481

1482

1483

1484

1485

1486

1487

1488

1489

1490

1491

1492

1493

1494

1495

1496

1497

1498

1499

1500

1501

1502

1503

1504

1505

1506

1507

1508

1509

1510

1511

1512

1513

1514

1515

1516

1517

1518

1519

1520

1521

1522

1523

1524

1525

1526

1527

1528

1529

1530

1531

1532

1533

1534

1535

1536

1537

1538

1539

1540

1541

1542

1543

1544

1545

1546

1547

1548

1549

1550

1551

1552

1553

1554

1555

1556

1557

1558

1559

1560

1561

1562

1563

1564

1565

1566

1567

1568

1569

1570

1571

1572

1573

1574

1575

1576

1577

1578

1579

1580

1581

1582

1583

1584

1585

1586

1587

1588

1589

1590

1591

#include <M5Atom.h>

#include <Kalman.h>

#include <WiFi.h>

#include <WebServer.h>

#include <Preferences.h>

#include <Kalman.h>

#include <Wire.h>

#include <PCA9685.h> //PCA9685用ヘッダーファイル（秋月電子通商作成）

#include <PS4Controller.h>

WebServer server(80);

const char ssid[] = "4legRoboPWM"; // SSID

const char pass[] = "password"; // password

const IPAddress ip(192, 168, 55, 29); //　IP address

const IPAddress subnet(255, 255, 255, 0);

int Mode;

float Ly = 0.0, Ry = 0.0;

Preferences preferences;

PCA9685 pwm = PCA9685(0x40); //PCA9685のアドレス指定（アドレスジャンパ未接続時）

#define SERVOMIN 123 //最小パルス幅 (標準的なサーボパルスに設定)

#define SERVOMAX 614 //最大パルス幅 (標準的なサーボパルスに設定)

int offset[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

int id[] = {-1, 7, 6, 0, 1, 8, 9, 15, 14};

int iniPos = 0;

//leg length

float L1 = 50.0, L2 = 65.0;//unit:mm

Kalman kalmanX, kalmanY;

float kalAngleX, kalAngleDotX, kalAngleY, kalAngleDotY;

unsigned long oldTime = 0, loopTime, nowTime;

float dt;

float accX = 0, accY = 0, accZ = 0;

float gyroX = 0, gyroY = 0, gyroZ = 0;

float theta_X = 0.0, theta_Y = 0.0;

float theta_Xdot = 0.0, theta_Ydot = 0.0;

int Jump = 0, Step = 0, Imu = 0, Stretch = 0, L = 0, R = 0, Roll = 0;

int Advance =0, Back = 0;

int period = 80, x = 60, height = 50, upHeight = 20, stride = 20;

float hX, hY, Kp = 0.4, Kd = 0.4;

float hs, hd;

int Dj1 = 120, Dj2 = 150;

//加速度センサから傾きデータ取得 [deg]

void get_theta() {

M5.IMU.getAccelData(&accX,&accY,&accZ);

//傾斜角導出単位はdeg

theta_X = atan2(accY, -accZ) * 57.29578f;

theta_Y = -atan2(-accX, -accZ) * 57.29578f;

}

//Y軸角速度取得

void get_gyro_data() {

M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);

theta_Xdot = -gyroX;

theta_Ydot = gyroY;

}

String modeBtJump = "off";

String modeBtStep = "off";

String modeBtImu = "off";

String modeBtStretch = "off";

String modeBtAd = "off";

String modeBtBack = "off";

String modeBtL = "off";

String modeBtR = "off";

String modeBtRoll = "off";

//browser

void handleRoot() {

String temp ="<!DOCTYPE html> \n<html lang=\"ja\">";

temp +="<head>";

temp +="<meta charset=\"utf-8\">";

temp +="<title>4legRoboPWM</title>";

temp +="<meta name=\"viewport\" content=\"width=device-width, initial-scale=1\">";

temp +="<style>";

temp +=".container{";

temp +=" margin: auto;";

temp +=" text-align: center;";

temp +=" font-size: 1.2rem;";

temp +="}";

temp +="span,.pm{";

temp +=" display: inline-block;";

temp +=" border: 1px solid #ccc;";

temp +=" width: 50px;";

temp +=" height: 30px;";

temp +=" vertical-align: middle;";

temp +=" margin-bottom: 8px;";

temp +="}";

temp +="span{";

temp +=" width: 120px;";

temp +="}";

temp +="button{";

temp +=" width: 100px;";

temp +=" height: 40px;";

temp +=" font-weight: bold;";

temp +=" margin-bottom: 8px;";

temp +="}";

temp +="button.on{ background:lime; color:white; }";

temp +=".column-3{ max-width:330px; margin:auto; text-align:center; display:flex; justify-content:space-between; flex-wrap:wrap; }";

temp +="</style>";

temp +="</head>";

temp +="<body>";

temp +="<div class=\"container\">";

temp += "<a class='button' href='/IniPos'>サーボ Offset調整</a><br><br>";

//button

temp +="<div class=\"column-3\">";

temp +="<button class=\"" + modeBtImu + "\" type=\"button\" ><a href=\"/imu\">IMU</a></button><br>";

temp +="<button class=\"" + modeBtAd + "\" type=\"button\" ><a href=\"/ad\">Advance</a></button><br>";

temp +="<button class=\"" + modeBtJump + "\" type=\"button\" ><a href=\"/Jump\">Jump</a></button><br>";

temp +="<button class=\"" + modeBtL + "\" type=\"button\" ><a href=\"/L\">L</a></button><br>";

temp +="<button class=\"" + modeBtStep + "\" type=\"button\" ><a href=\"/step\">Step</a></button><br>";

temp +="<button class=\"" + modeBtR + "\" type=\"button\" ><a href=\"/R\">R</a></button><br>";

temp +="<button class=\"" + modeBtStretch + "\" type=\"button\" ><a href=\"/stretch\">Stretch</a></button><br>";

temp +="<button class=\"" + modeBtBack + "\" type=\"button\" ><a href=\"/back\">Back</a></button><br>";

temp +="<button class=\"" + modeBtRoll + "\" type=\"button\" ><a href=\"/Roll\">Roll</a></button><br>";

temp +="</div>";

//Dj1

temp +="Dj1 (msec)<br>";

temp +="<a class=\"pm\" href=\"/Dj1M\">-</a>";

temp +="<span>" + String(Dj1) + "</span>";

temp +="<a class=\"pm\" href=\"/Dj1P\">+</a><br>";

//Dj2

temp +="Dj2 (msec)<br>";

temp +="<a class=\"pm\" href=\"/Dj2M\">-</a>";

temp +="<span>" + String(Dj2) + "</span>";

temp +="<a class=\"pm\" href=\"/Dj2P\">+</a><br>";

//period

temp +="period (msec)<br>";

temp +="<a class=\"pm\" href=\"/periodM\">-</a>";

temp +="<span>" + String(period) + "</span>";

temp +="<a class=\"pm\" href=\"/periodP\">+</a><br>";

//x

temp +="x (mm)<br>";

temp +="<a class=\"pm\" href=\"/xM\">-</a>";

temp +="<span>" + String(x) + "</span>";

temp +="<a class=\"pm\" href=\"/xP\">+</a><br>";

//height

temp +="height (mm)<br>";

temp +="<a class=\"pm\" href=\"/heightM\">-</a>";

temp +="<span>" + String(height) + "</span>";

temp +="<a class=\"pm\" href=\"/heightP\">+</a><br>";

//upHeight

temp +="upHeight (mm)<br>";

temp +="<a class=\"pm\" href=\"/upHeightM\">-</a>";

temp +="<span>" + String(upHeight) + "</span>";

temp +="<a class=\"pm\" href=\"/upHeightP\">+</a><br>";

//stride

temp +="stride (mm)<br>";

temp +="<a class=\"pm\" href=\"/strideM\">-</a>";

temp +="<span>" + String(stride) + "</span>";

temp +="<a class=\"pm\" href=\"/strideP\">+</a><br>";

//Kp

temp +="Kp<br>";

temp +="<a class=\"pm\" href=\"/KpM\">-</a>";

temp +="<span>" + String(Kp) + "</span>";

temp +="<a class=\"pm\" href=\"/KpP\">+</a><br>";

//Kd

temp +="Kd<br>";

temp +="<a class=\"pm\" href=\"/KdM\">-</a>";

temp +="<span>" + String(Kd) + "</span>";

temp +="<a class=\"pm\" href=\"/KdP\">+</a><br>";

temp +="</div>";

temp +="</body>";

server.send(200, "text/HTML", temp);

}

void handleIniPos() {

iniPos = 1;

String html ="<!DOCTYPE html><html lang='ja'><head><meta charset='utf-8'>";

html +="<meta name='viewport' content='width=device-width, initial-scale=1'>";

html +="<title>IniPos</title>";

html +="<style>";

html +=".container{margin:auto;text-align:center;font-size:1.2rem;}";

html +="span,.pm{display:inline-block;border:1px solid #ccc;width:50px;height:30px;vertical-align:middle;margin-bottom:8px;}";

html +="span{width:120px;}button{width:100px;height:40px;font-weight:bold;margin-bottom:8px;}";

html +=".column-3{max-width:330px;margin:auto;text-align:center;display:flex;justify-content:space-between;flex-wrap:wrap;}";

html +="a.btn{display:inline-block;margin-top:20px;padding:10px 20px;background:#007bff;color:#fff;text-decoration:none;border-radius:6px;}";

html +="</style></head><body><div class='container'>";

html +="<h3>サーボ Offset調整</h3>";

// --- offset UI ---

for (int i = 1; i <= 8; i++) {

html += "Offset S" + String(i) + "<br>";

html += "<a class='pm' href='/offset" + String(i) + "M'>-</a>";

html += "<span>" + String(offset[id[i]]) + "</span>";

html += "<a class='pm' href='/offset" + String(i) + "P'>+</a><br>";

}

// --- 戻るボタン ---

html += "<a class='btn' href='/backToMain'>戻る</a>";

html += "</div></body></html>";

server.send(200, "text/html", html);

}

void handleBackToMain() {

iniPos = 0; // ← 戻る時に0に戻す

handleRoot(); // トップページを再表示

}

void handleL() {

if(modeBtL == "off"){

modeBtL = "on";

modeBtR = "off";

modeBtJump = "off";

modeBtStep = "off";

modeBtStretch = "off";

modeBtAd = "off";

modeBtBack = "off";

modeBtImu = "off";

modeBtRoll = "off";

L = 1;

R = 0;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 0;

Advance =0;

Back = 0;

Roll = 0;

}else{

modeBtL = "off";

L = 0;

R = 0;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 0;

Advance =0;

Back = 0;

Roll = 0;

}

handleRoot();

}

void handleR() {

if(modeBtR == "off"){

modeBtL = "off";

modeBtR = "on";

modeBtJump = "off";

modeBtStep = "off";

modeBtStretch = "off";

modeBtAd = "off";

modeBtBack = "off";

modeBtImu = "off";

modeBtRoll = "off";

L = 0;

R = 1;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 0;

Advance =0;

Back = 0;

Roll = 0;

}else{

modeBtR = "off";

L = 0;

R = 0;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 0;

Advance =0;

Back = 0;

Roll = 0;

}

handleRoot();

}

void handleJump() {

if(modeBtJump == "off"){

modeBtL = "off";

modeBtR = "off";

modeBtJump = "on";

modeBtStep = "off";

modeBtStretch = "off";

modeBtAd = "off";

modeBtBack = "off";

modeBtImu = "off";

modeBtRoll = "off";

L = 0;

R = 0;

Imu = 0;

Jump = 1;

Step = 0;

Stretch = 0;

Advance =0;

Back = 0;

Roll = 0;

}else{

modeBtJump = "off";

L = 0;

R = 0;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 0;

Advance =0;

Back = 0;

Roll = 0;

}

handleRoot();

}

void handleStep() {

if(modeBtStep == "off"){

modeBtL = "off";

modeBtR = "off";

modeBtJump = "off";

modeBtStep = "on";

modeBtStretch = "off";

modeBtAd = "off";

modeBtBack = "off";

modeBtImu = "off";

modeBtRoll = "off";

L = 0;

R = 0;

Imu = 0;

Jump = 0;

Step = 1;

Stretch = 0;

Advance =0;

Back = 0;

Roll = 0;

}else{

modeBtStep = "off";

L = 0;

R = 0;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 0;

Advance =0;

Back = 0;

Roll = 0;

}

handleRoot();

}

void handleImu() {

if(modeBtImu == "off"){

modeBtL = "off";

modeBtR = "off";

modeBtJump = "off";

modeBtStep = "off";

modeBtStretch = "off";

modeBtAd = "off";

modeBtBack = "off";

modeBtImu = "on";

modeBtRoll = "off";

L = 0;

R = 0;

Imu = 1;

Jump = 0;

Step = 0;

Stretch = 0;

Advance =0;

Back = 0;

Roll = 0;

}else{

modeBtImu = "off";

L = 0;

R = 0;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 0;

Advance =0;

Back = 0;

Roll = 0;

}

handleRoot();

}

void handleStretch() {

if(modeBtStretch == "off"){

modeBtL = "off";

modeBtR = "off";

modeBtJump = "off";

modeBtStep = "off";

modeBtStretch = "on";

modeBtAd = "off";

modeBtBack = "off";

modeBtImu = "off";

modeBtRoll = "off";

L = 0;

R = 0;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 1;

Advance =0;

Back = 0;

Roll = 0;

}else{

modeBtStretch = "off";

L = 0;

R = 0;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 0;

Advance =0;

Back = 0;

Roll = 0;

}

handleRoot();

}

void handleAd() {

if(modeBtAd == "off"){

modeBtL = "off";

modeBtR = "off";

modeBtJump = "off";

modeBtStep = "off";

modeBtStretch = "off";

modeBtAd = "on";

modeBtBack = "off";

modeBtImu = "off";

modeBtRoll = "off";

L = 0;

R = 0;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 0;

Advance = 1;

Back = 0;

Roll = 0;

}else{

modeBtAd = "off";

L = 0;

R = 0;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 0;

Advance =0;

Back = 0;

Roll = 0;

}

handleRoot();

}

void handleBack() {

if(modeBtBack == "off"){

modeBtL = "off";

modeBtR = "off";

modeBtJump = "off";

modeBtStep = "off";

modeBtStretch = "off";

modeBtAd = "off";

modeBtBack = "on";

modeBtImu = "off";

modeBtRoll = "off";

L = 0;

R = 0;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 0;

Advance = 0;

Back = 1;

Roll = 0;

}else{

modeBtBack = "off";

L = 0;

R = 0;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 0;

Advance =0;

Back = 0;

Roll = 0;

}

handleRoot();

}

void handleRoll() {

if(modeBtRoll == "off"){

modeBtL = "off";

modeBtR = "off";

modeBtJump = "off";

modeBtStep = "off";

modeBtStretch = "off";

modeBtAd = "off";

modeBtBack = "off";

modeBtImu = "off";

modeBtRoll = "on";

L = 0;

R = 0;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 0;

Advance = 0;

Back = 0;

Roll = 1;

}else{

modeBtRoll = "off";

L = 0;

R = 0;

Imu = 0;

Jump = 0;

Step = 0;

Stretch = 0;

Advance =0;

Back = 0;

Roll = 0;

}

handleRoot();

}

void handleKpM() {

if(Kp > 0){

Kp -= 0.1;

preferences.putFloat("Kp", Kp);

}

handleRoot();

}

void handleKpP() {

if(Kp <= 10){

Kp += 0.1;

preferences.putFloat("Kp", Kp);

}

handleRoot();

}

void handleKdM() {

if(Kd > 0){

Kd -= 0.1;

preferences.putFloat("Kd", Kd);

}

handleRoot();

}

void handleKdP() {

if(Kd <= 10){

Kd += 0.1;

preferences.putFloat("Kd", Kd);

}

handleRoot();

}

void handlePeriodM() {

if(period > 5){

period -= 5;

preferences.putInt("period", period);

}

handleRoot();

}

void handlePeriodP() {

if(period <= 1000){

period += 5;

preferences.putInt("period", period);

}

handleRoot();

}

void handlexM() {

if(x > -115){

x -= 5;

preferences.putInt("x", x);

}

handleRoot();

}

void handlexP() {

if(x < 115){

x += 5;

preferences.putInt("x", x);

}

handleRoot();

}

void handleHeightM() {

if(height > -120){

height -= 5;

preferences.putInt("height", height);

}

handleRoot();

}

void handleHeightP() {

if(height <= 120){

height += 5;

preferences.putInt("height", height);

}

handleRoot();

}

void handleUpHeightM() {

if(upHeight > 0){

upHeight -= 2;

preferences.putInt("upHeight", upHeight);

}

handleRoot();

}

void handleUpHeightP() {

if(upHeight <= 50){

upHeight += 2;

preferences.putInt("upHeight", upHeight);

}

handleRoot();

}

void handleStrideM() {

if(stride > 0){

stride -= 2;

preferences.putInt("stride", stride);

}

handleRoot();

}

void handleStrideP() {

if(stride <= 40){

stride += 2;

preferences.putInt("stride", stride);

}

handleRoot();

}

void Dj1M() {

if(Dj1 > 10){

Dj1 -= 10;

preferences.putInt("Dj1", Dj1);

}

handleRoot();

}

void Dj1P() {

if(Dj1 <= 1000){

Dj1 += 10;

preferences.putInt("Dj1", Dj1);

}

handleRoot();

}

void Dj2M() {

if(Dj2 > 10){

Dj2 -= 10;

preferences.putInt("Dj2", Dj2);

}

handleRoot();

}

void Dj2P() {

if(Dj2 <= 1000){

Dj2 += 10;

preferences.putInt("Dj2", Dj2);

}

handleRoot();

}

void offset1M() {

if(offset[id[1]] > -300){

offset[id[1]] -= 1;

preferences.putInt("offset1", offset[id[1]]);

}

handleIniPos();

}

void offset1P() {

if(offset[id[1]] <= 300){

offset[id[1]] += 1;

preferences.putInt("offset1", offset[id[1]]);

}

handleIniPos();

}

void offset2M() {

if(offset[id[2]] > -300){

offset[id[2]] -= 1;

preferences.putInt("offset2", offset[id[2]]);

}

handleIniPos();

}

void offset2P() {

if(offset[id[2]] <= 300){

offset[id[2]] += 1;

preferences.putInt("offset2", offset[id[2]]);

}

handleIniPos();

}

void offset3M() {

if(offset[id[3]] > -300){

offset[id[3]] -= 1;

preferences.putInt("offset3", offset[id[3]]);

}

handleIniPos();

}

void offset3P() {

if(offset[id[3]] <= 300){

offset[id[3]] += 1;

preferences.putInt("offset3", offset[id[3]]);

}

handleIniPos();

}

void offset4M() {

if(offset[id[4]] > -300){

offset[id[4]] -= 1;

preferences.putInt("offset4", offset[id[4]]);

}

handleIniPos();

}

void offset4P() {

if(offset[id[4]] <= 300){

offset[id[4]] += 1;

preferences.putInt("offset4", offset[id[4]]);

}

handleIniPos();

}

void offset5M() {

if(offset[id[5]] > -300){

offset[id[5]] -= 1;

preferences.putInt("offset5", offset[id[5]]);

}

handleIniPos();

}

void offset5P() {

if(offset[id[5]] <= 300){

offset[id[5]] += 1;

preferences.putInt("offset5", offset[id[5]]);

}

handleIniPos();

}

void offset6M() {

if(offset[id[6]] > -300){

offset[id[6]] -= 1;

preferences.putInt("offset6", offset[id[6]]);

}

handleIniPos();

}

void offset6P() {

if(offset[id[6]] <= 300){

offset[id[6]] += 1;

preferences.putInt("offset6", offset[id[6]]);

}

handleIniPos();

}

void offset7M() {

if(offset[id[7]] > -300){

offset[id[7]] -= 1;

preferences.putInt("offset7", offset[id[7]]);

}

handleIniPos();

}

void offset7P() {

if(offset[id[7]] <= 300){

offset[id[7]] += 1;

preferences.putInt("offset7", offset[id[7]]);

}

handleIniPos();

}

void offset8M() {

if(offset[id[8]] > -300){

offset[id[8]] -= 1;

preferences.putInt("offset8", offset[id[8]]);

}

handleIniPos();

}

void offset8P() {

if(offset[id[8]] <= 300){

offset[id[8]] += 1;

preferences.putInt("offset8", offset[id[8]]);

}

handleIniPos();

}

//Core0

void browser(void *pvParameters) {

M5.IMU.Init();

//フルスケールレンジ

M5.IMU.SetAccelFsr(M5.IMU.AFS_2G);

M5.IMU.SetGyroFsr(M5.IMU.GFS_250DPS);

get_theta();

kalmanX.setAngle(theta_X);

kalmanY.setAngle(theta_Y);

M5.dis.setBrightness(20);

WiFi.softAP(ssid, pass);

delay(100);

WiFi.softAPConfig(ip, ip, subnet);

IPAddress myIP = WiFi.softAPIP();

server.on("/", handleRoot);

server.on("/IniPos", handleIniPos);

server.on("/backToMain", handleBackToMain);

server.on("/Jump", handleJump);

server.on("/step", handleStep);

server.on("/imu", handleImu);

server.on("/stretch", handleStretch);

server.on("/ad", handleAd);

server.on("/back", handleBack);

server.on("/L", handleL);

server.on("/R", handleR);

server.on("/Roll", handleRoll);

server.on("/KpM", handleKpM);

server.on("/KpP", handleKpP);

server.on("/KdM", handleKdM);

server.on("/KdP", handleKdP);

server.on("/periodM", handlePeriodM);

server.on("/periodP", handlePeriodP);

server.on("/xM", handlexM);

server.on("/xP", handlexP);

server.on("/heightM", handleHeightM);

server.on("/heightP", handleHeightP);

server.on("/upHeightM", handleUpHeightM);

server.on("/upHeightP", handleUpHeightP);

server.on("/strideM", handleStrideM);

server.on("/strideP", handleStrideP);

server.on("/Dj1M", Dj1M);

server.on("/Dj1P", Dj1P);

server.on("/Dj2M", Dj2M);

server.on("/Dj2P", Dj2P);

server.on("/offset1M", offset1M);

server.on("/offset1P", offset1P);

server.on("/offset2M", offset2M);

server.on("/offset2P", offset2P);

server.on("/offset3M", offset3M);

server.on("/offset3P", offset3P);

server.on("/offset4M", offset4M);

server.on("/offset4P", offset4P);

server.on("/offset5M", offset5M);

server.on("/offset5P", offset5P);

server.on("/offset6M", offset6M);

server.on("/offset6P", offset6P);

server.on("/offset7M", offset7M);

server.on("/offset7P", offset7P);

server.on("/offset8M", offset8M);

server.on("/offset8P", offset8P);

server.begin();

for (;;){

server.handleClient();

//LED表示

M5.dis.clear();

if(kalAngleX > 20.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i, 0xff0000);

}

}else if(kalAngleX <= 20.0 && kalAngleX > 12.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i, 0x0000ff);

}

}else if(kalAngleX <= 12.0 && kalAngleX > 4.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i + 5, 0x0000ff);

}

}else if(kalAngleX <= 4.0 && kalAngleX > 1.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i + 10, 0x0000ff);

}

}else if(abs(kalAngleX) <= 1.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i + 10, 0x00ff00);

}

}else if(kalAngleX >= -4.0 && kalAngleX < -1.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i + 10, 0x0000ff);

}

}else if(kalAngleX >= -12.0 && kalAngleX < -4.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i + 15, 0x0000ff);

}

}else if(kalAngleX >= -20.0 && kalAngleX < -12.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i + 20, 0x0000ff);

}

}else if(kalAngleX < -20.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i + 20, 0xff0000);

}

if(kalAngleY > 20.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i * 5, 0xff0000);

}

}else if(kalAngleY <= 20.0 && kalAngleY > 12.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i * 5, 0x0000ff);

}

}else if(kalAngleY <= 12.0 && kalAngleY > 4.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i * 5 + 1, 0x0000ff);

}

}else if(kalAngleY <= 4.0 && kalAngleY > 1.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i * 5 + 2, 0x0000ff);

}

}else if(abs(kalAngleY) <= 1.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i * 5 + 2, 0x00ff00);

}

}else if(kalAngleY >= -4.0 && kalAngleY < -1.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i * 5 + 2, 0x0000ff);

}

}else if(kalAngleY >= -12.0 && kalAngleY < -4.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i * 5 + 3, 0x0000ff);

}

}else if(kalAngleY >= -20.0 && kalAngleY < -12.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i * 5 + 4, 0x0000ff);

}

}else if(kalAngleY < -20.0){

for(int i = 0; i < 5; i++){

M5.dis.drawpix(i * 5 + 4, 0xff0000);

}

nowTime = micros();

loopTime = nowTime - oldTime;

oldTime = nowTime;

dt = (float)loopTime / 1000000.0; //sec

get_theta();

get_gyro_data();

//カルマンフィルタ姿勢傾き

kalAngleX = kalmanX.getAngle(theta_X, theta_Xdot, dt);

kalAngleY = kalmanY.getAngle(theta_Y, theta_Ydot, dt);

//カルマンフィルタ姿勢角速度

kalAngleDotX = kalmanX.getRate();

kalAngleDotY = kalmanY.getRate();

disableCore0WDT();

}

void setup() {

M5.begin(true, false, true); //SerialEnable, bool I2CEnable, DisplayEnable

Wire.begin(22, 23); //SDA, SCL

pwm.begin();

pwm.setPWMFreq(60); //PWM周期を60Hzに設定

delay(200);

Serial.println("-----------------------------");

uint8_t btmac[6];

esp_read_mac(btmac, ESP_MAC_BT);

Serial.printf("[Bluetooth] Mac Address = %02X:%02X:%02X:%02X:%02X:%02X\r\n", btmac[0], btmac[1], btmac[2], btmac[3], btmac[4], btmac[5]);

PS4.begin();

delay(200);

//parameter memory

preferences.begin("parameter", false);

Kp = preferences.getFloat("Kp", Kp);

Kd = preferences.getFloat("Kd", Kd);

period = preferences.getInt("period", period);

x = preferences.getInt("x", x);

height = preferences.getInt("height", height);

upHeight = preferences.getInt("upHeight", upHeight);

stride = preferences.getInt("stride", stride);

Dj1 = preferences.getInt("Dj1", Dj1);

Dj2 = preferences.getInt("Dj2", Dj2);

offset[id[1]] = preferences.getInt("offset1", offset[id[1]]);

offset[id[2]] = preferences.getInt("offset2", offset[id[2]]);

offset[id[3]] = preferences.getInt("offset3", offset[id[3]]);

offset[id[4]] = preferences.getInt("offset4", offset[id[4]]);

offset[id[5]] = preferences.getInt("offset5", offset[id[5]]);

offset[id[6]] = preferences.getInt("offset6", offset[id[6]]);

offset[id[7]] = preferences.getInt("offset7", offset[id[7]]);

offset[id[8]] = preferences.getInt("offset8", offset[id[8]]);

//browser task

xTaskCreatePinnedToCore(

browser

, "browser" // A name just for humans

, 4096 // This stack size can be checked & adjusted by reading the Stack Highwater

, NULL

, 1 // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.

, NULL

, 0);

}

void loop() {

if (PS4.isConnected()) { //PS4コントローラ

//ジョイスティック

Ly = PS4.LStickY() / 5.0;

Ry = PS4.RStickY() / 5.0;

//ボタン

if (PS4.Triangle()) Jump = 1;

if (PS4.Cross()) Roll = 1;

//トリガー

if (PS4.L1()){

if(height > 20){

height--;

delay(10);

}

if (PS4.R1()){

if(height < 100){

height++;

delay(10);

}

if (PS4.L2()){

x--;

if(x < 0) x = 0;

delay(10);

}

if (PS4.R2()){

x++;

if(x > 100) x = 99;

delay(10);

}

if(fabs(Ly) > 1.0 || fabs(Ry) > 1.0){

float tim,tt;

unsigned long time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x - Ry * cos(tt), height - upHeight * sin(tt));

fLIK(x + Ly * cos(tt), height);

rLIK(x + Ly * cos(tt), height - upHeight * sin(tt));

rRIK(x - Ry * cos(tt), height);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x + Ry * sin(tt), height - upHeight * cos(tt));

fLIK(x - Ly * sin(tt), height);

rLIK(x - Ly * sin(tt), height - upHeight * cos(tt));

rRIK(x + Ry * sin(tt), height);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x + Ry * cos(tt), height);

fLIK(x - Ly * cos(tt), height - upHeight * sin(tt));

rLIK(x - Ly * cos(tt), height);

rRIK(x + Ry * cos(tt), height - upHeight * sin(tt));

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x - Ry * sin(tt), height);

fLIK(x + Ly * sin(tt), height - upHeight * cos(tt));

rLIK(x + Ly * sin(tt), height);

rRIK(x - Ry * sin(tt), height - upHeight * cos(tt));

}

if(Jump){ //ジャンプ

fRIK(20, 60);

fLIK(20, 60);

rLIK(-30, 80);

rRIK(-30, 80);

fRIK(20, 60);

fLIK(20, 60);

rLIK(-30, 80);

rRIK(-30, 80);

delay(500);

fRIK(-10, 78);

fLIK(-10, 78);

rLIK(-40, 75);

rRIK(-40, 75);

fRIK(-10, 78);

fLIK(-10, 78);

rLIK(-40, 75);

rRIK(-40, 75);

delay(Dj1);

fRIK(44, -35);

fLIK(44, -35);

rLIK(100, -50);

rRIK(100, -50);

fRIK(44, -35);

fLIK(44, -35);

rLIK(100, -50);

rRIK(100, -50);

delay(Dj2);

rLIK(60, 50);

rRIK(60, 50);

rLIK(60, 50);

rRIK(60, 50);

delay(400);

Jump = 0;

}else if(Roll){ //起き上がり

fLIK(65, -50);

rLIK(65, -50);

fRIK(65, -50);

rRIK(65, -50);

fLIK(65, -50);

rLIK(65, -50);

fRIK(65, -50);

rRIK(65, -50);

delay(150);

fLIK(0, 115);

rLIK(50, -20);

fRIK(0, -115);

rRIK(50, -20);

fLIK(0, 115);

rLIK(50, -20);

fRIK(0, -115);

rRIK(50, -20);

delay(150);

fLIK(0, 115);

rLIK(50, 20);

fRIK(0, -115);

rRIK(50, 20);

fLIK(0, 115);

rLIK(50, 20);

fRIK(0, -115);

rRIK(50, 20);

delay(150);

fLIK(60, 50);

rLIK(-5, 60);

fRIK(60, 50);

rRIK(-5, 60);

fLIK(60, 50);

rLIK(-5, 60);

fRIK(60, 50);

rRIK(-5, 60);

delay(400);

Roll = 0;

}else if(Stretch){ //屈伸

float tim,tt;

unsigned long time_mSt= millis();

tim=0;

while(tim<period * 8){

tim=millis()-time_mSt;

tt=float(tim * 2 * PI / (period * 8));

fRIK(x, height + upHeight * sin(tt));

fLIK(x, height + upHeight * sin(tt));

rLIK(x, height + upHeight * sin(tt));

rRIK(x, height + upHeight * sin(tt));

}

}else if(Step){ //足踏み

float tim,tt;

unsigned long time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x, height - upHeight * sin(tt));

rLIK(x, height - upHeight * sin(tt));

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x, height - upHeight * cos(tt));

rLIK(x, height - upHeight * cos(tt));

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

rRIK(x, height - upHeight * sin(tt));

fLIK(x, height - upHeight * sin(tt));

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

rRIK(x, height - upHeight * cos(tt));

fLIK(x, height - upHeight * cos(tt));

}

}else if(Advance){ //前進

float tim,tt;

unsigned long time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x - stride * cos(tt), height - upHeight * sin(tt));

fLIK(x + stride * cos(tt), height);

rLIK(x + stride * cos(tt), height - upHeight * sin(tt));

rRIK(x - stride * cos(tt), height);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x + stride * sin(tt), height - upHeight * cos(tt));

fLIK(x - stride * sin(tt), height);

rLIK(x - stride * sin(tt), height - upHeight * cos(tt));

rRIK(x + stride * sin(tt), height);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x + stride * cos(tt), height);

fLIK(x - stride * cos(tt), height - upHeight * sin(tt));

rLIK(x - stride * cos(tt), height);

rRIK(x + stride * cos(tt), height - upHeight * sin(tt));

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x - stride * sin(tt), height);

fLIK(x + stride * sin(tt), height - upHeight * cos(tt));

rLIK(x + stride * sin(tt), height);

rRIK(x - stride * sin(tt), height - upHeight * cos(tt));

}

}else if(Back){ //後進

float tim,tt;

unsigned long time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x+ stride * cos(tt), height - upHeight * sin(tt));

fLIK(x - stride * cos(tt), height);

rLIK(x - stride * cos(tt), height - upHeight * sin(tt));

rRIK(x + stride * cos(tt), height);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x - stride * sin(tt), height - upHeight * cos(tt));

fLIK(x + stride * sin(tt), height);

rLIK(x + stride * sin(tt), height - upHeight * cos(tt));

rRIK(x - stride * sin(tt), height);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x - stride * cos(tt), height);

fLIK(x + stride * cos(tt), height - upHeight * sin(tt));

rLIK(x + stride * cos(tt), height);

rRIK(x - stride * cos(tt), height - upHeight * sin(tt));

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x + stride * sin(tt), height);

fLIK(x - stride * sin(tt), height - upHeight * cos(tt));

rLIK(x - stride * sin(tt), height);

rRIK(x + stride * sin(tt), height - upHeight * cos(tt));

}

}else if(R){ //R

float tim,tt;

unsigned long time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x + stride * cos(tt), height - upHeight * sin(tt));

fLIK(x + stride * cos(tt), height);

rLIK(x + stride * cos(tt), height - upHeight * sin(tt));

rRIK(x + stride * cos(tt), height);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x - stride * sin(tt), height - upHeight * cos(tt));

fLIK(x - stride * sin(tt), height);

rLIK(x - stride * sin(tt), height - upHeight * cos(tt));

rRIK(x - stride * sin(tt), height);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x -+ stride * cos(tt), height);

fLIK(x - stride * cos(tt), height - upHeight * sin(tt));

rLIK(x - stride * cos(tt), height);

rRIK(x - stride * cos(tt), height - upHeight * sin(tt));

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x + stride * sin(tt), height);

fLIK(x + stride * sin(tt), height - upHeight * cos(tt));

rLIK(x + stride * sin(tt), height);

rRIK(x + stride * sin(tt), height - upHeight * cos(tt));

}

}else if(L){ //L

float tim,tt;

unsigned long time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x - stride * cos(tt), height - upHeight * sin(tt));

fLIK(x - stride * cos(tt), height);

rLIK(x - stride * cos(tt), height - upHeight * sin(tt));

rRIK(x - stride * cos(tt), height);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x + stride * sin(tt), height - upHeight * cos(tt));

fLIK(x + stride * sin(tt), height);

rLIK(x + stride * sin(tt), height - upHeight * cos(tt));

rRIK(x + stride * sin(tt), height);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x + stride * cos(tt), height);

fLIK(x + stride * cos(tt), height - upHeight * sin(tt));

rLIK(x + stride * cos(tt), height);

rRIK(x + stride * cos(tt), height - upHeight * sin(tt));

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

tt=float(tim * PI / 2 / period);

fRIK(x - stride * sin(tt), height);

fLIK(x - stride * sin(tt), height - upHeight * cos(tt));

rLIK(x - stride * sin(tt), height);

rRIK(x - stride * sin(tt), height - upHeight * cos(tt));

}

}else if(Imu){

hX = hX + Kp * kalAngleX / 90.0 + Kd * kalAngleDotX / 100.0;

hY = hY + Kp * kalAngleY / 90.0 + Kd * kalAngleDotY / 100.0;

hs = hX + hY;

hd = hX - hY;

fRIK(x, height - hd);

fLIK(x, height - hs);

rRIK(x, height + hs);

rLIK(x, height + hd);

}else if(iniPos){

for(int i = 1; i <= 8; i++){

servo_write(id[i], 0);

}

}else{ //初期姿勢

fRIK(x, height);

fLIK(x, height);

rLIK(x, height);

rRIK(x, height);

}

void servo_write(int ch, float ang){

ang = fmap(ang, -90.0, 90.0, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅に変換

ang = constrain(ang + offset[ch], SERVOMIN, SERVOMAX);

pwm.setPWM(ch, 0, ang);

}

// float対応版 map関数

float fmap(float x, float in_min, float in_max, float out_min, float out_max) {

return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;

}

//-------------------------------------------

//IK

//-------------------------------------------

void fRIK(float x, float z){

float phi,ld, th1, th2;

ld = sqrt(x*x + z*z);

if(ld <= L1 + L2){

phi = atan2(x, z);

th1 = 90.0 - (phi + acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld))) * RAD_TO_DEG;

th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) * RAD_TO_DEG - 90.0;

th1 = constrain(th1, -90.0, 90.0);

th2 = constrain(th2, -90.0, 90.0);

servo_write(id[8], int(-th2)); //ヒザ

servo_write(id[7], int(-th1));

}

void fLIK(float x, float z){

float phi,ld, th1, th2;

ld = sqrt(x*x + z*z);

if(ld <= L1 + L2){

phi = atan2(x, z);

th1 = 90.0 - (phi + acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld))) * RAD_TO_DEG;

th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) * RAD_TO_DEG - 90.0;

th1 = constrain(th1, -90.0, 90.0);

th2 = constrain(th2, -90.0, 90.0);

servo_write(id[6], int(th2)); //ヒザ

servo_write(id[5], int(th1));

}

void rRIK(float x, float z){

float phi,ld, th1, th2;

ld = sqrt(x*x + z*z);

if(ld <= L1 + L2){

phi = atan2(x, z);

th1 = 90.0 - (phi + acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld))) * RAD_TO_DEG;

th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) * RAD_TO_DEG - 90.0;

th1 = constrain(th1, -90.0, 90.0);

th2 = constrain(th2, -90.0, 90.0);

servo_write(id[4], int(th2)); //ヒザ

servo_write(id[3], int(th1));

}

void rLIK(float x, float z){

float phi,ld, th1, th2;

ld = sqrt(x*x + z*z);

if(ld <= L1 + L2){

phi = atan2(x, z);

th1 = 90.0 - (phi + acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld))) * RAD_TO_DEG;

th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) * RAD_TO_DEG - 90.0;

th1 = constrain(th1, -90.0, 90.0);

th2 = constrain(th2, -90.0, 90.0);

servo_write(id[2], int(-th2)); //ヒザ

servo_write(id[1], int(-th1));

}

コード説明

L. 12～16 WiFi APモード用設定。SSIDやパスワード、IPを指定 (それぞれ任意)。

L. 22 Preferencesライブラリを使用して調整値をフラッシュに記憶して電源OFF後も保持します。

L. 26～32 PCA9685のアドレスとサーボパルス幅とIOピンを指定
　サーボパルスは500～2500usecになるように設定

L. 36 本ロボットの足の長さです。逆運動学算出時に使用
　大腿長さL1=50mm、下腿長さL2=65mm

L. 49～54 ロボット動作パラメータ初期値。アプリで可変

L. 58～63加速度センサから傾斜角を取得する関数
　ATOM Matrix内蔵のIMUセンサ(MPU6886)で検出した加速度からX軸とY軸の傾斜角を算出

　
L. 66～70 角速度取得関数
　IMUのX軸とY軸の角速度を使用

L. 85～829 ブラウザアプリ表示設定
　APモード接続時の動作モード選択ボタンや各パラメータ設定用の表示
　およびサーボモータオフセット調整ページの記述をしています。　

L. 833～1014 core0動作
　ATOM Matrix デュアルコアのcore0でLEDマトリクス表示とブラウザ表示とIMU処理を実行します。

　慣性センサMPU6886のフルスケールレンジは以下で指定 (L. 837, 838)
　・加速度：16bit　±2 g
　・ジャイロ(角速度)：16bit ±250 deg/sec

　機体のX軸とY軸の角度を以下の通りLEDインジケータ表示しています。
　インジケータは動作モード実行時は機能しません。
　・±1°以下：中心で緑表示
　・±20°以上：赤表示
　・その他：青表示

　カルマンフィルタライブラリの関数を用いてセンサの姿勢角と角速度を算出 (L. 994~1009)
　

L. 1017～1072 setup関数
　PCA9685用I2C　SDA：IO22、SCL：IO23 (L. 1020)

　アプリで設定した各オフセット値はPreferences機能でフラッシュに記録します。
　電源起動時にフラッシュに記録されたオフセット値を読み出します (L. 1039～1060)
　

L. 1075～1498 loop関数
　PS4コントローラ設定 (L. 1076~1111)
　　コントローラ接続時に各種動作や足位置を制御 (詳細は後述)

　各種動作モード (L. 1162~1497)
　　逆運動学関数で足先の座標を指定して各動作を実現しています。

　　ジャンプ動作モード (L. 1162~1199)
　　　各足の逆運動学関数で足先の座標を指定してバク転動作を実現
　　　前足を伸ばすタイミングと後ろ足を伸ばすタイミングを
　　　パラメータDj1, Dj2の時間(msec)で調整可能

　　反転(Roll)動作モード (L. 1200~1241)
　　　ひっくり返った状態から起上って反転動作します。

　　屈伸、足踏み、前後進、左右旋回動作モード (L. 1242~1476)
　　以下のパラメータで動作を調整可能です。
　　・period：動作の周期時間を指定
　　・x：各足先の前後位置指定
　　・height：各足先の付け根からの高さ位置指定
　　・upHeight：足上げ動作時の上げ量を指定
　　・stride：歩行時の歩幅指定

　　IMU動作モード (L. 1477~1487)
　　　機体の傾きと角速度を検知して足の高さを制御し機体の水平を保ちます。
　　　それぞれの足の高さをセンサのX軸、Y軸の角度と角速度に応じていわゆるPD制御しています。

　　　応答はパラメータKp、Kdで調整できます。

　　初期姿勢 (L. 1492~1497)
　　　動作モード以外は全足がパラメータx, heightで指定した位置で固定されます。

L. 1502～1511 servo_write() 関数
　サーボIDと角度を指定

L. 1517~1591 逆運動学
　関数 fRIK()、fLIK()、rRIK()、rLIK()でそれぞれの足先座標を指定して、サーボの角度を算出して駆動しています。

　各足の付け根を原点として、高さと前後の座標を指定して以下の式で運動させます。

　各サーボ角度は他のパーツなどに干渉しないようにconstrain関数で動作範囲角度を制限しております。

アプリ画面

コードをATOM Matrixに書き込んで、スマホもしくはPCのWiFi接続設定でSSID ”4legRoboPWM”に接続
　パスワード：password
ブラウザで”192.168.55.29”にアクセス

動作モードはボタンクリックでONし、再度クリックでOFFします。

パラメータは “-“、”+”をクリックして値を増減させます。
値は電源OFF後も保持されます。

“サーボOffset調整”をクリックでオフセット調整画面に遷移して、サーボモータを中間位置 (パルス 1500usec)にするPWM信号が出力されます。

“-“、”+”をクリックして各サーボの位置の微調整します。

戻るボタンでホームに戻りサーボも初期姿勢の角度になります。

ロボット組立て

ロボットを組み立てます

ボディ組立て

base.stlの天板6個の穴をM3ネジ用のタッピングでネジ切り

base.stlにコントロール基板を20mm M3ネジスペーサで固定、
スイッチは裏からM3×6mmネジで固定します。

2セルLiPoバッテリを強力両面テープでbase.stlに固定して、コネクタに接続します (電源スイッチがOFFであることを事前に確認してください)。

電源をONにすると先ほど書き込んだコードによって姿勢に応じてLEDマトリクスのインジケータが動きます。

定番LEDインジケータ pic.twitter.com/fcqpdhxvHr

— HomeMadeGarbage (@H0meMadeGarbage) November 13, 2025

サーボ4個をなべタッピングネジ 2×8mm (2か所づつ)でbase.stlに固定します。

サーボのコネクタを以下のように基板に接続します。

基板コネクタ向きは棒線側がGNDで、信号名がシルク記載されているほうが信号線です。

ヒップサーボオフセット調整

付属のサーボホーンには筐体固定用に中心穴から３番目の穴を2mm径で穴あけしておきます (8個すべて加工してください)。

電源をONしてブラウザアプリからサーボOffset調整画面にはいり、サーボモータを中間位置 (パルス 1500usec)にします。
その状態でサーボホーンを以下のように水平になるようにはめ込みます (なるべく水平にできればOKです)。

サーボOffset調整アプリのOffset S1, 3, 5, 7の”-“、”+”をクリックして完全に水平になるように微調整してください。

オフセット調整モード pic.twitter.com/Ib95zVn1c5

— HomeMadeGarbage (@H0meMadeGarbage) November 13, 2025

調整が終わったら電源をOFFします。

大腿組立て

サーボホーンにleg2.stlを固定します。leg2.stlは中心の穴をM2ネジ用のタッピングでネジ切りして、サーボのアームに開けた2mm穴にM2×8mmネジで固定します。

M2×10mmネジでアーム軸とも固定します。

大腿パーツleg1-1.stl, leg1-2.stlをbase.stlの丸突に通してleg2.stlの先端の穴２か所でなべタッピングネジ 2×10mmでネジ止めします。

大腿パーツの先にサーボ4個をなべタッピングネジ 2×8mm (2か所づつ)で固定します。

ヒザサーボのコネクタを以下のように基板に接続します。

ヒザサーボオフセット調整

電源をONしてブラウザアプリからサーボOffset調整画面にはいり、サーボモータを中間位置 (パルス 1500usec)にします。
その状態でヒザのサーボホーンを以下のように下方向に直角になるようにはめ込みます。

サーボOffset調整アプリのOffset S2, 4, 6, 8の”-“、”+”をクリックして完全に直角になるように微調整してください。

調整が終わったら電源をOFFします。

下腿組立て

下腿パーツleg4.stlとサーボホーンを固定します。
leg4.stlは上から2番目の穴をM2ネジ用のタッピングでネジ切りして、サーボのアームに開けた2mm穴にM2×8mmネジで固定します。

leg4.stlとleg3.stlを2個のなべタッピングネジ2×10mmでネジ止めします。

機体完成

足先にTPUフィラメントで出力したshoe.stlを履かせてサーボ配線を整えて機体は完成です。

あらためてサーボOffset調整アプリで以下のようにコの字になるように再調整してください。

こんな感じ調整

オフセット調整 pic.twitter.com/8bx1cQjtJh

— HomeMadeGarbage (@H0meMadeGarbage) November 13, 2025

ブラウザアプリ動作

ブラウザアプリによる動作についてまとめました。

各パラメータは電源OFF後もメモリ保持されます。

動作の様子

レシピ書きますかぁ#Robotics pic.twitter.com/wtP9ptxev5

— HomeMadeGarbage (@H0meMadeGarbage) November 12, 2025

IMU動作モード

IMU動作モード pic.twitter.com/eSSaUKYYNp

— HomeMadeGarbage (@H0meMadeGarbage) November 14, 2025

PS4コントローラ

本ロボットはPS4コントローラでも動かすことができます。

　
ATOM Matrixの起動時に Bluetooth Mac Addressをシリアル出力させています。
アドレスを控えてコントローラと接続します。

PS4コントローラとESP32のペアリング方法の詳細は以下を参照ください。

・ESP32 (M5StickC) で PS4コントローラを使ってみた : Skyzoo ヨッシーの備忘録

Skyzoo ヨッシーの備忘録

ロボのBluetooth Mac Addressを書き込んだコントローラのホームボタン⌂を押すとペアリングされます。

PS4ペアリング後にはアプリによるWiFi接続ができませんので、両方実施したい場合はWiFiアプリ接続後にBluetoothコントローラペアリングしてください。

コントローラの対応は以下の通り

ジョイスティックの左右の上下でロボの右側と左側の歩幅を制御して移動が可能です。

ジョイスティックのジョイって喜びって意味じゃね？ pic.twitter.com/dcj9E5Qtsy

— HomeMadeGarbage (@H0meMadeGarbage) November 9, 2025

ボタンでバク転/起上り、トリガで足位置を変更できます。

なんだか楽しいよ pic.twitter.com/V6Xg8ljW4U

— HomeMadeGarbage (@H0meMadeGarbage) November 10, 2025

トリガで変更した足位置はメモリ記憶されないので電源OFFでリセットされます。

コメントはこちらからコメントをキャンセル

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください。

はじめに

PTK 7465 MG

構成

回路図

部品

専用基板

3Dモデル

筐体組み立てキット

コントロール基板

Arduinoコード

コード説明

アプリ画面

ロボット組立て

ボディ組立て

ヒップサーボ オフセット調整

大腿組立て

ヒザサーボ オフセット調整

下腿組立て

機体完成

ブラウザアプリ動作

PS4コントローラ

関連記事

コメントはこちらから コメントをキャンセル

ヒップサーボオフセット調整

ヒザサーボオフセット調整

コメントはこちらからコメントをキャンセル