はじめに

これまでに各種サーボモータを使用して４脚ロボットを製作してきました。
以下の記事ではシリアルサーボの STS3032 と SCS0009 、そしてPWMマイクロサーボでの比較を実施しました。

4脚ロボットの検討

どの４脚ロボットが魅力的であるかアンケートを募ったところ以下のような結果となりました。

サーボモータ８個による4脚ロボット
つくるならどれがいいですか？

— HomeMadeGarbage (@H0meMadeGarbage) March 4, 2024

やはり安価で手軽に使用できるSG90級のPWMサーボで作りたいという意見が多かったです。

SG90によるロボットは既に良書があって今さら製法公開に魅力は感じませんし、私は電子工作界の栗原はるみを目指しておりますので次点の高額なSTS3032での作例レシピを書くことにも抵抗がありました。

私が製法公開する意義を見失っていた中で、 SCS0009 による４脚ロボットでバク転を実現することができました。

バク転が出来るようになるまでの練習過程 pic.twitter.com/bz9YkIHpUv

— HomeMadeGarbage (@H0meMadeGarbage) March 26, 2024

比較的安価で入手性も良いシリアルサーボ SCS0009 でバク転可能な４脚ロボットというのはなかなか面白いと思いますので、ここで製法を公開したいと思います。

何かございましたらコメントなどでご意見いただけますと幸いです。

構成

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

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

回路図

以下が本4脚ロボットの回路図です。先の構成図の通り非常に単純です。

部品

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

ATOM Matrix
スイッチサイエンス：https://www.switch-science.com/products/6260
秋月電子：https://akizukidenshi.com/catalog/g/g117215/
シリアルサーボ SCS0009
AliExpress：https://ja.aliexpress.com/item/1005005950881566.html
スイッチサイエンス：https://www.switch-science.com/products/8042
マルツ：https://www.marutsu.co.jp/pc/i/2349126/
5V 降圧DCDC
秋月電子：https://akizukidenshi.com/catalog/g/g113536/
2セル 7.4V LiPoバッテリ
Amazon GRIFFIN ：https://amzn.to/3QeNr3K
Amazon Kypom：https://amzn.to/3WdmW2I
シリアルサーボ基板接続用コネクタ molex 5267-3
Amazon：https://amzn.to/4b4GdHv
千石電商：https://www.sengoku.co.jp/mod/sgk_cart/detail.php?code=EEHD-0HWA
LiPoバッテリ用配線コネクタ
Amazon：https://amzn.to/3QgJw6u
電源用スライドスイッチ
Amazon：https://amzn.to/3zRaI4j

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

なべタッピングネジ 2×8mm：12本　(ボディとサーボ固定部接続用)
M2×6mmネジ：9本　(サーボアームと筐体固定用、 ATOMと基板固定用)
なべタッピングネジ 2×10mm：16本　(足パーツ固定用)
M3ネジもしくはスペーサー：4本　（基板固定用）
M3×8mmネジとナット：２セット (スライドスイッチ固定用)

専用基板

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

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

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

3Dモデル

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

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

改訂履歴

2024年4月23日：公開（初版）
2024年4月30日：ネジ穴微調整
base.stl：servoFix1固定ネジ穴調整
servoFix1、servoFix2：サーボ固定ネジ穴縮小

筐体組み立てキット

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

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

ロボット製作

サーボモータ設定

SCS0009サーボは専用ソフトでIDを設定して以下のように配置しました。
ボーレートも1M bpsに変更します。

SCS0009は信号0~1023に対して0~300°回転します。専用ソフトで中点の信号511をした状態でサーボ付属のアームを以下のように水平になるようにはめ込みます。

片側アームが付属されていない場合はニッパーで切るなどで対応ください。
ここで完全にアームを水平に出来なくても後ほどオフセット調整しますので問題ありません。

アームには筐体固定用に中心穴から３番目の穴を2mm径で穴あけしておきます。

筐体組み立て

なべタッピングネジ 2×8mm (12か所)でbase.stlにservoFix1-1.stlとservoFix1-2.stlを２個づつネジ止めします。

サーボを前述のIDの通りに付属のネジでservoFix1に固定します。

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

ネジ切りせずに長めのネジでナット固定でも構いません。

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

大腿パーツservoFix2をservoFix1の丸突に通してarm1の先端の２か所となべタッピングネジ 2×10mmでネジ止めします。

servoFix2の先に膝となるサーボを前述のIDの通りに付属のネジで固定します。

膝サーボのアームはarm3との干渉を避けるために以下のようにニッパーで切り取ってください。
下肢パーツarm2とアームをarm1の時と同様に固定します。

arm2とarm3を2個の2×10mmでネジ止めします。

足先にTPUフィラメントで出力したshoe.stlを履かせて筐体は完成です。

コントローラ

前述の回路と部品で制御基板を構築します。
ATOM Matrixと基板はピンヘッダで接続して、基板間にM2×6mmネジでspacer.stlを通して固定します。

基板はbase.stlにM3ネジ用タッピングでネジ切りして４か所でねじ固定します。
ここではロボット反転時にATOM Matrixが地面などにぶつからないように17mm程度の高さになるネジタイプのスペーサで固定しました。

サーボを基板コネクタに接続 (信号線はパラレルに接続されているので接続位置はどこでもOK)し、バッテリを両面テープで固定します。

電源周り

コントロール基板の電源入力にバッテリ(7.4V)を接続します。ここではバッテリコネクタ間にスライドスイッチを挿入しております。
スイッチはswitch.stlをbodyに接着してM3×8mmネジとナットで固定しています。

組み立て動画

以下はサーボの設定から筐体組み立てまでの様子を配信したアーカイブ動画です。
宜しければ参考にしてください。

サーボオフセット調整

サーボにアームを差し込んだ際にメカ的誤差で水平がとれていない状況ですので調整します。

サーボ角度指定信号にオフセット値を与えて水平にして、ロボットのサーボ角度0°指定の際の姿勢を以下のようにビターっとまっすぐにします。

調整の様子

後述するアプリをATOM Matrixに書き込み、ATOMをアクセスポイントとしてWiFiを介してスマホ(もしくはPC)と接続しオフセット調整します。

以下が実際に調整している様子です。

調整用アプリ Arduinoコード

#include <M5Atom.h>
#include <Kalman.h>
#include <Preferences.h>
#include <WiFi.h>
#include <WebServer.h>


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

#define led_pin 27
#define NUM_LEDS 25


Preferences preferences;

//           ID  0   1   2  3   4   5   6   7
int offset[] = {0, 0, 0, 0, 0, 0, 0, 0}; 



//browser
void handleRoot() {
  String temp ="<!DOCTYPE html> \n<html lang=\"ja\">";
  temp +="<head>";
  temp +="<meta charset=\"utf-8\">";
  temp +="<title>servoOffset</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 +="<h3>servoOffset</h3>";


  //ID0 offset
  temp +="ID0<br>";
  temp +="<a class=\"pm\" href=\"/id0M\">-</a>";
  temp +="<span>" + String(offset[0]) + "</span>";
  temp +="<a class=\"pm\" href=\"/id0P\">+</a><br>";

  //ID1 offset
  temp +="ID1<br>";
  temp +="<a class=\"pm\" href=\"/id1M\">-</a>";
  temp +="<span>" + String(offset[1]) + "</span>";
  temp +="<a class=\"pm\" href=\"/id1P\">+</a><br>";

  //ID2 offset
  temp +="ID2<br>";
  temp +="<a class=\"pm\" href=\"/id2M\">-</a>";
  temp +="<span>" + String(offset[2]) + "</span>";
  temp +="<a class=\"pm\" href=\"/id2P\">+</a><br>";
  
  //ID3 offset
  temp +="ID3<br>";
  temp +="<a class=\"pm\" href=\"/id3M\">-</a>";
  temp +="<span>" + String(offset[3]) + "</span>";
  temp +="<a class=\"pm\" href=\"/id3P\">+</a><br>";

  //ID4 offset
  temp +="ID4<br>";
  temp +="<a class=\"pm\" href=\"/id4M\">-</a>";
  temp +="<span>" + String(offset[4]) + "</span>";
  temp +="<a class=\"pm\" href=\"/id4P\">+</a><br>";

  //ID5 offset
  temp +="ID5<br>";
  temp +="<a class=\"pm\" href=\"/id5M\">-</a>";
  temp +="<span>" + String(offset[5]) + "</span>";
  temp +="<a class=\"pm\" href=\"/id5P\">+</a><br>";

  //ID6 offset
  temp +="ID6<br>";
  temp +="<a class=\"pm\" href=\"/id6M\">-</a>";
  temp +="<span>" + String(offset[6]) + "</span>";
  temp +="<a class=\"pm\" href=\"/id6P\">+</a><br>";
  
  //ID7 offset
  temp +="ID7<br>";
  temp +="<a class=\"pm\" href=\"/id7M\">-</a>";
  temp +="<span>" + String(offset[7]) + "</span>";
  temp +="<a class=\"pm\" href=\"/id7P\">+</a><br>";
  
   
  temp +="</div>";
  temp +="</body>";
  server.send(200, "text/HTML", temp);
}



void handleId0M() {
  if(offset[0] > -511){
    offset[0] -= 2;
    preferences.putInt("offset0", offset[0]);
  }
  handleRoot();
}

void handleId0P() {
  if(offset[0] < 511){
    offset[0] += 2;
    preferences.putInt("offset0", offset[0]);
  }
  handleRoot();
}


void handleId1M() {
  if(offset[1] > -511){
    offset[1] -= 2;
    preferences.putInt("offset1", offset[1]);
  }
  handleRoot();
}

void handleId1P() {
  if(offset[1] < 511){
    offset[1] += 2;
    preferences.putInt("offset1", offset[1]);
  }
  handleRoot();
}


void handleId2M() {
  if(offset[2] > -511){
    offset[2] -= 2;
    preferences.putInt("offset2", offset[2]);
  }
  handleRoot();
}

void handleId2P() {
  if(offset[2] < 511){
    offset[2] += 2;
    preferences.putInt("offset2", offset[2]);
  }
  handleRoot();
}

void handleId3M() {
  if(offset[3] > -511){
    offset[3] -= 2;
    preferences.putInt("offset3", offset[3]);
  }
  handleRoot();
}

void handleId3P() {
  if(offset[3] < 511){
    offset[3] += 2;
    preferences.putInt("offset3", offset[3]);
  }
  handleRoot();
}


void handleId4M() {
  if(offset[4] > -511){
    offset[4] -= 2;
    preferences.putInt("offset4", offset[4]);
  }
  handleRoot();
}

void handleId4P() {
  if(offset[4] < 511){
    offset[4] += 2;
    preferences.putInt("offset4", offset[4]);
  }
  handleRoot();
}


void handleId5M() {
  if(offset[5] > -511){
    offset[5] -= 2;
    preferences.putInt("offset5", offset[5]);
  }
  handleRoot();
}

void handleId5P() {
  if(offset[5] < 511){
    offset[5] += 2;
    preferences.putInt("offset5", offset[5]);
  }
  handleRoot();
}


void handleId6M() {
  if(offset[6] > -511){
    offset[6] -= 2;
    preferences.putInt("offset6", offset[6]);
  }
  handleRoot();
}

void handleId6P() {
  if(offset[6] < 511){
    offset[6] += 2;
    preferences.putInt("offset6", offset[6]);
  }
  handleRoot();
}


void handleId7M() {
  if(offset[7] > -511){
    offset[7] -= 2;
    preferences.putInt("offset7", offset[7]);
  }
  handleRoot();
}

void handleId7P() {
  if(offset[7] < 511){
    offset[7] += 2;
    preferences.putInt("offset7", offset[7]);
  }
  handleRoot();
}



void setup() {
  M5.begin(true, true, true); //SerialEnable, bool I2CEnable, DisplayEnable
  
  Serial1.begin(1000000, SERIAL_8N1, -1, 19);
  

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


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


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

  server.on("/", handleRoot); 
  

  server.on("/id0M", handleId0M);
  server.on("/id0P", handleId0P);
  server.on("/id1M", handleId1M);
  server.on("/id1P", handleId1P);
  server.on("/id2M", handleId2M);
  server.on("/id2P", handleId2P);
  server.on("/id3M", handleId3M);
  server.on("/id3P", handleId3P);
  server.on("/id4M", handleId4M);
  server.on("/id4P", handleId4P);
  server.on("/id5M", handleId5M);
  server.on("/id5P", handleId5P);
  server.on("/id6M", handleId6M);
  server.on("/id6P", handleId6P);
  server.on("/id7M", handleId7M);
  server.on("/id7P", handleId7P);

  
  server.begin(); 
}


void loop() {
  servo_write(0, 0);
  servo_write(1, 0);
  servo_write(2, 0);
  servo_write(3, 0);
  servo_write(4, 0);
  servo_write(5, 0);
  servo_write(6, 0);
  servo_write(7, 0);

  server.handleClient();

  delay(300);
}


//参考 https://qiita.com/Ninagawa123/items/7b79c5f5117dd1470ac9
void scs_moveToPos(byte id, int position) {
  // コマンドパケットを作成
  byte message[13];
  message[0] = 0xFF;  // ヘッダ
  message[1] = 0xFF;  // ヘッダ
  message[2] = id;    // サーボID
  message[3] = 9;     // パケットデータ長
  message[4] = 3;     // コマンド（3は書き込み命令）
  message[5] = 42;    // レジスタ先頭番号
  message[6] = (position >> 8) & 0xFF; // 位置情報バイト上位
  message[7] = position & 0xFF; // 位置情報バイト下位
  message[8] = 0x00;  // 時間情報バイト下位
  message[9] = 0x00;  // 時間情報バイト上位
  message[10] = 0x00; // 速度情報バイト下位
  message[11] = 0x00; // 速度情報バイト上位

  // チェックサムの計算
  byte checksum = 0;
  for (int i = 2; i < 12; i++) {
    checksum += message[i];
  }
  message[12] = ~checksum; // チェックサム

  // コマンドパケットを送信
  for (int i = 0; i < 13; i++) {
    Serial1.write(message[i]);
  }
}


void servo_write(int ch, float ang){
  int sig = 511 + offset[ch] + int((512.0 / 150.0) * ang);
  scs_moveToPos(ch, sig);
}

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

#include <M5Atom.h>

#include <Kalman.h>

#include <Preferences.h>

#include <WiFi.h>

#include <WebServer.h>

WebServer server(80);

const char ssid[] = "servoOffset"; // SSID

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

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

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

#define led_pin 27

#define NUM_LEDS 25

Preferences preferences;

// ID 0 1 2 3 4 5 6 7

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

//browser

void handleRoot() {

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

temp +="<head>";

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

temp +="<title>servoOffset</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 +="<h3>servoOffset</h3>";

//ID0 offset

temp +="ID0<br>";

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

temp +="<span>" + String(offset[0]) + "</span>";

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

//ID1 offset

temp +="ID1<br>";

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

temp +="<span>" + String(offset[1]) + "</span>";

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

//ID2 offset

temp +="ID2<br>";

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

temp +="<span>" + String(offset[2]) + "</span>";

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

//ID3 offset

temp +="ID3<br>";

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

temp +="<span>" + String(offset[3]) + "</span>";

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

//ID4 offset

temp +="ID4<br>";

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

temp +="<span>" + String(offset[4]) + "</span>";

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

//ID5 offset

temp +="ID5<br>";

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

temp +="<span>" + String(offset[5]) + "</span>";

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

//ID6 offset

temp +="ID6<br>";

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

temp +="<span>" + String(offset[6]) + "</span>";

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

//ID7 offset

temp +="ID7<br>";

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

temp +="<span>" + String(offset[7]) + "</span>";

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

temp +="</div>";

temp +="</body>";

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

}

void handleId0M() {

if(offset[0] > -511){

offset[0] -= 2;

preferences.putInt("offset0", offset[0]);

}

handleRoot();

}

void handleId0P() {

if(offset[0] < 511){

offset[0] += 2;

preferences.putInt("offset0", offset[0]);

}

handleRoot();

}

void handleId1M() {

if(offset[1] > -511){

offset[1] -= 2;

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

}

handleRoot();

}

void handleId1P() {

if(offset[1] < 511){

offset[1] += 2;

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

}

handleRoot();

}

void handleId2M() {

if(offset[2] > -511){

offset[2] -= 2;

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

}

handleRoot();

}

void handleId2P() {

if(offset[2] < 511){

offset[2] += 2;

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

}

handleRoot();

}

void handleId3M() {

if(offset[3] > -511){

offset[3] -= 2;

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

}

handleRoot();

}

void handleId3P() {

if(offset[3] < 511){

offset[3] += 2;

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

}

handleRoot();

}

void handleId4M() {

if(offset[4] > -511){

offset[4] -= 2;

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

}

handleRoot();

}

void handleId4P() {

if(offset[4] < 511){

offset[4] += 2;

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

}

handleRoot();

}

void handleId5M() {

if(offset[5] > -511){

offset[5] -= 2;

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

}

handleRoot();

}

void handleId5P() {

if(offset[5] < 511){

offset[5] += 2;

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

}

handleRoot();

}

void handleId6M() {

if(offset[6] > -511){

offset[6] -= 2;

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

}

handleRoot();

}

void handleId6P() {

if(offset[6] < 511){

offset[6] += 2;

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

}

handleRoot();

}

void handleId7M() {

if(offset[7] > -511){

offset[7] -= 2;

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

}

handleRoot();

}

void handleId7P() {

if(offset[7] < 511){

offset[7] += 2;

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

}

handleRoot();

}

void setup() {

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

Serial1.begin(1000000, SERIAL_8N1, -1, 19);

//parameter memory

preferences.begin("parameter", false);

offset[0] =preferences.getInt("offset0", offset[0]);

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

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

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

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

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

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

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

WiFi.softAP(ssid, pass);

delay(100);

WiFi.softAPConfig(ip, ip, subnet);

IPAddress myIP = WiFi.softAPIP();

server.on("/", handleRoot);

server.on("/id0M", handleId0M);

server.on("/id0P", handleId0P);

server.on("/id1M", handleId1M);

server.on("/id1P", handleId1P);

server.on("/id2M", handleId2M);

server.on("/id2P", handleId2P);

server.on("/id3M", handleId3M);

server.on("/id3P", handleId3P);

server.on("/id4M", handleId4M);

server.on("/id4P", handleId4P);

server.on("/id5M", handleId5M);

server.on("/id5P", handleId5P);

server.on("/id6M", handleId6M);

server.on("/id6P", handleId6P);

server.on("/id7M", handleId7M);

server.on("/id7P", handleId7P);

server.begin();

}

void loop() {

servo_write(0, 0);

servo_write(1, 0);

servo_write(2, 0);

servo_write(3, 0);

servo_write(4, 0);

servo_write(5, 0);

servo_write(6, 0);

servo_write(7, 0);

server.handleClient();

delay(300);

}

//参考 https://qiita.com/Ninagawa123/items/7b79c5f5117dd1470ac9

void scs_moveToPos(byte id, int position) {

// コマンドパケットを作成

byte message[13];

message[0] = 0xFF; // ヘッダ

message[1] = 0xFF; // ヘッダ

message[2] = id; // サーボID

message[3] = 9; // パケットデータ長

message[4] = 3; // コマンド（3は書き込み命令）

message[5] = 42; // レジスタ先頭番号

message[6] = (position >> 8) & 0xFF; // 位置情報バイト上位

message[7] = position & 0xFF; // 位置情報バイト下位

message[8] = 0x00; // 時間情報バイト下位

message[9] = 0x00; // 時間情報バイト上位

message[10] = 0x00; // 速度情報バイト下位

message[11] = 0x00; // 速度情報バイト上位

// チェックサムの計算

byte checksum = 0;

for (int i = 2; i < 12; i++) {

checksum += message[i];

}

message[12] = ~checksum; // チェックサム

// コマンドパケットを送信

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

Serial1.write(message[i]);

}

void servo_write(int ch, float ang){

int sig = 511 + offset[ch] + int((512.0 / 150.0) * ang);

scs_moveToPos(ch, sig);

}

使用ライブラリ

ボードマネージャ
　ESP32 バージョン 2.0.15
　M5Stack バージョン 2.1.1

ライブラリマネージャ
　M5ATOM バージョン 0.1.2

サーボ通信用ライブラリ
　ESP32のGPIOを用いてFeetechのシリアルサーボを簡単に駆動できるライブラリです。
　サーボからの受信はできませんがUART信号変換基板を使用せずに直接信号送信が可能です。
　https://qiita.com/Ninagawa123/items/7b79c5f5117dd1470ac9

コード説明

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

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

L. 26～253ブラウザアプリ表示設定
　APモード接続時の各サーボのオフセット値設定用の表示を記述しています。
　オフセット調整は±2 分解能で実施できるようにしています。

L. 257～306 setup関数
　UART1：送信ピンGPIO19、ボーレート1Mbpsに設定 (L. 260)

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

　WiFiとブラウザアプリ用のハンドラを設定 (L. 277～305)

L. 309～322 loop関数
　servo_write() 関数でオフセット値を加味して各サーボの角度を0°指定

L. 326～353 サーボ駆動用 scs_moveToPos()関数
　参考：Feetechのサーボをマイコンから直接動かす

L. 356～359 servo_write() 関数
　サーボIDと角度を指定する。scs_moveToPos()関数を用いてオフセット値を加算して信号設定。

調整用アプリ画面

調整用アプリをATOM Matrixに書き込んで、スマホもしくはPCのWiFi接続設定でSSID ”servoOffset”に接続
パスワード：password
　
ブラウザで”192.168.55.28”にアクセス

“-“、”+”をクリックしてサーボが水平になるようにオフセット値を増減させます。

最終値を控えて、本動作コードにオフセット値として書き込みます。

調整アプリ制作動画

以下はサーボオフセット調整アプリの制作から実際に調整するところまでの様子を配信したアーカイブ動画です。
宜しければ参考にしてください。

Arduinoコード

4脚ロボット動作コードです。

#include <M5Atom.h>
#include <Preferences.h>
#include <Kalman.h>
#include <WiFi.h>
#include <WebServer.h>


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

#define led_pin 27
#define NUM_LEDS 25

Preferences preferences;

//           ID  0   1   2  3   4   5   6   7
int offset[] = {-8, -12, 6, 22, 20, 18, 0, 28}; 

//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;

float hX, hY, hXpre, hYpre;

int pos = 0;


int Mode;
#define L 1
#define R 2
#define Imu 3
#define Jump 4
#define Jump2 42
#define Step 5
#define Stretch 6
#define Advance 7
#define Back 8
#define Roll 9

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";


//パラメータ
//-------------------------------------------------
float wakeUpAngle = -1.0;
int period = 80, x = 60, height = 40, upHeight = 10, stride = 12;
float Kp = 0.3, Kd = 0.3;
//-------------------------------------------------


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

  if(Mode != Jump2){
    if(pos == 1 && accZ < -0.96) pos = 0;
    if(pos == 0 && accZ > 0.96) pos = 1;
  }
}

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


//browser
void handleRoot() {
  String temp ="<!DOCTYPE html> \n<html lang=\"ja\">";
  temp +="<head>";
  temp +="<meta charset=\"utf-8\">";
  temp +="<title>robot0009</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 +="<h3>robot0009</h3>";

  //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>";

  //wakeUpAngle
  temp +="wakeUpAngle (deg)<br>";
  temp +="<a class=\"pm\" href=\"/wakeUpAngleM\">-</a>";
  temp +="<span>" + String(wakeUpAngle) + "</span>";
  temp +="<a class=\"pm\" href=\"/wakeUpAngleP\">+</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 handleL() {
  if(modeBtL == "off"){
    modeBtL = "on";
    modeBtR = "off";
    modeBtJump = "off";
    modeBtStep = "off";
    modeBtStretch = "off";
    modeBtAd = "off";
    modeBtBack = "off";
    modeBtImu = "off";
    modeBtRoll = "off";

    Mode = L;
  }else{
    modeBtL = "off";

    Mode = 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";

    Mode = R;
  }else{
    modeBtR = "off";

    Mode = 0;
  }
  handleRoot();
}

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

    Mode = Jump;
  }else{
    modeBtJump = "off";

    Mode = 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";

    Mode = Step;
  }else{
    modeBtStep = "off";

    Mode = 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";

    Mode = Imu;
  }else{
    modeBtImu = "off";

    Mode = 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";

    Mode = Stretch;
  }else{
    modeBtStretch = "off";

    Mode = 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";

    Mode = Advance;
  }else{
    modeBtAd = "off";

    Mode = 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";

    Mode = Back;
  }else{
    modeBtBack = "off";

    Mode = 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";

    Mode = Roll;
  }else{
    modeBtRoll = "off";

    Mode = 0;
  }
  handleRoot();
}


void handleWakeUpAngleM() {
  if(wakeUpAngle > -90.0){
    wakeUpAngle -= 0.5;
    preferences.putFloat("wakeUpAngle", wakeUpAngle);
  }
  handleRoot();
}
void handleWakeUpAngleP() {
  if(wakeUpAngle <= 90.0){
    wakeUpAngle += 0.5;
    preferences.putFloat("wakeUpAngle", wakeUpAngle);
  }
  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 <= 3000){
    period += 5;
    preferences.putInt("period", period);
  }
  handleRoot();
}


void handlexM() {
  if(x > -615){
    x -= 5;
    preferences.putInt("x", x);
  }
  handleRoot();
}
void handlexP() {
  if(x < 515){
    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 <= 500){
    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();
}


//Core0
void browser(void *pvParameters) {
  M5.dis.setBrightness(20);
  
  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);
      }
    }

    delay(50);
      
    disableCore0WDT();
  }
}


void setup() {
  M5.begin(true, true, true); //SerialEnable, bool I2CEnable, DisplayEnable
  
  Serial1.begin(1000000, SERIAL_8N1, -1, 19);
  
  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);

  delay(200); 
  

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

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

  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);


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

  server.on("/", handleRoot); 
  
  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("/wakeUpAngleM", handleWakeUpAngleM);
  server.on("/wakeUpAngleP", handleWakeUpAngleP);

  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.begin(); 

  //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() {
  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();

  //Serial.println(kalAngleX);


  if(Mode == Jump){ //ジャンプ
    fRIK(20, 35);
    fLIK(20, 35);
    rLIK(-25, 95);
    rRIK(-25, 95);
    delay(500);

    fRIK(-35, 100);
    fLIK(-35, 100);
    delay(160);
    
    
    rRIK(107, -42);
    rLIK(107, -42);
    fRIK(-70, 90);
    fLIK(-70, 90);
    delay(100);

    rRIK(80, 30);
    rLIK(80, 30);
    
    Mode = Jump2;

    fRIK(-85, 40);
    fLIK(-85, 40);

  }else if(Mode == Jump2){
    if(kalAngleX < 0.0 && kalAngleX > wakeUpAngle){
      fRIK(60, 40);
      fLIK(60, 40);
      rRIK(60, 40);
      rLIK(60, 40);
      
      Mode = 0;
    }
  }else if(Mode == 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(Mode == 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 * (0.5 * cos(2*tt) - 0.5));
      fLIK(x, height);
      rLIK(x, height + upHeight * (0.5 * cos(2*tt) - 0.5));
      rRIK(x, height);
    }

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

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

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x, height);
      fLIK(x, height + upHeight * (-0.5 * cos(2*tt) - 0.5));
      rLIK(x, height);
      rRIK(x, height + upHeight * (-0.5 * cos(2*tt) - 0.5));
    }
  }else if(Mode == 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 * (0.5 * cos(2*tt) - 0.5));
      fLIK(x + stride * cos(tt), height);
      rLIK(x + stride * cos(tt), height + upHeight * (0.5 * cos(2*tt) - 0.5));
      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 * (-0.5 * cos(2*tt) - 0.5));
      fLIK(x - stride * sin(tt), height);
      rLIK(x - stride * sin(tt), height + upHeight * (-0.5 * cos(2*tt) - 0.5));
      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 * (0.5 * cos(2*tt) - 0.5));
      rLIK(x - stride * cos(tt), height);
      rRIK(x + stride * cos(tt), height + upHeight * (0.5 * cos(2*tt) - 0.5));
    }

    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 * (-0.5 * cos(2*tt) - 0.5));
      rLIK(x + stride * sin(tt), height);
      rRIK(x - stride * sin(tt), height + upHeight * (-0.5 * cos(2*tt) - 0.5));
    }
  }else if(Mode == 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 * (0.5 * cos(2*tt) - 0.5));
      fLIK(x - stride * cos(tt), height);
      rLIK(x - stride * cos(tt), height + upHeight * (0.5 * cos(2*tt) - 0.5));
      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 * (-0.5 * cos(2*tt) - 0.5));
      fLIK(x + stride * sin(tt), height);
      rLIK(x + stride * sin(tt), height + upHeight * (-0.5 * cos(2*tt) - 0.5));
      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 * (0.5 * cos(2*tt) - 0.5));
      rLIK(x + stride * cos(tt), height);
      rRIK(x - stride * cos(tt), height + upHeight * (0.5 * cos(2*tt) - 0.5));
    }

    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 * (-0.5 * cos(2*tt) - 0.5));
      rLIK(x - stride * sin(tt), height);
      rRIK(x + stride * sin(tt), height + upHeight * (-0.5 * cos(2*tt) - 0.5));
    }
  }else if(Mode == 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(10, 40 + 10 * (0.5 * cos(2*tt) - 0.5));
      fLIK(10 + 10 * cos(tt), 40);
      rLIK(10 + 10 * cos(tt), 40 + 10 * (0.5 * cos(2*tt) - 0.5));
      rRIK(10, 40);
    }

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

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(10, 40);
      fLIK(10 - 10 * cos(tt), 40 + 10 * (0.5 * cos(2*tt) - 0.5));
      rLIK(10 - 10 * cos(tt), 40);
      rRIK(10, 40 + 10 * (0.5 * cos(2*tt) - 0.5));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(10, 40);
      fLIK(10 + 10 * sin(tt), 40 + 10 * (-0.5 * cos(2*tt) - 0.5));
      rLIK(10 + 10 * sin(tt), 40);
      rRIK(10, 40 + 10 * (-0.5 * cos(2*tt) - 0.5));
    }
  }else if(Mode == 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(10 - 10 * cos(tt), 40 + 10 * (0.5 * cos(2*tt) - 0.5));
      fLIK(10, 40);
      rLIK(10, 40 + 10 * (0.5 * cos(2*tt) - 0.5));
      rRIK(10 - 10 * cos(tt), 40);
    }

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

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(10 + 10 * cos(tt), 40);
      fLIK(10, 40 + 10 * (0.5 * cos(2*tt) - 0.5));
      rLIK(10, 40);
      rRIK(10 + 10 * cos(tt), 40 + 10 * (0.5 * cos(2*tt) - 0.5));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(10 - 10 * sin(tt), 40);
      fLIK(10, 40 + 10 * (-0.5 * cos(2*tt) - 0.5));
      rLIK(10, 40);
      rRIK(10 - 10 * sin(tt), 40 + 10 * (-0.5 * cos(2*tt) - 0.5));
    }
  }else if(Mode == Roll){ //反転
    fRIK(50, 30);
    fLIK(50, 30);
    rRIK(50, 30);
    rLIK(50, 30);
    delay(500);

    fRIK(10, 45);
    fLIK(10, 45);
    rRIK(0, 80);
    rLIK(0, 80);
    delay(500);

    fRIK(10, 90);
    fLIK(10, 90);  
    delay(150);

    fRIK(70, -50);
    fLIK(70, -50);
    delay(200);
    
    rRIK(70, -50);
    rLIK(70, -50);
    delay(600);
    
    Mode = 0;
  }else if(Mode == Imu){
    hX = hXpre + Kp / 100.0 * kalAngleX + Kd / 100.0 * kalAngleDotX;
    hY = hYpre + Kp / 100.0 * kalAngleY + Kd / 100.0 * kalAngleDotY;

    hXpre = hX;
    hYpre = hY;

    if(pos == 1){ //反転時
      hX = -hX; 
      hY = -hY;
    }

    fRIK(x, height - hX + hY);
    fLIK(x, height - hX - hY);
    rRIK(x, height + hX + hY);
    rLIK(x, height + hX - hY);
  }else{ //初期姿勢
    fRIK(x, height);
    fLIK(x, height);
    rRIK(x, height);
    rLIK(x, height);
  }
}


//参考 https://qiita.com/Ninagawa123/items/7b79c5f5117dd1470ac9
void scs_moveToPos(byte id, int position) {
  // コマンドパケットを作成
  byte message[13];
  message[0] = 0xFF;  // ヘッダ
  message[1] = 0xFF;  // ヘッダ
  message[2] = id;    // サーボID
  message[3] = 9;     // パケットデータ長
  message[4] = 3;     // コマンド（3は書き込み命令）
  message[5] = 42;    // レジスタ先頭番号
  message[6] = (position >> 8) & 0xFF; // 位置情報バイト上位
  message[7] = position & 0xFF; // 位置情報バイト下位
  message[8] = 0x00;  // 時間情報バイト下位
  message[9] = 0x00;  // 時間情報バイト上位
  message[10] = 0x00; // 速度情報バイト下位
  message[11] = 0x00; // 速度情報バイト上位

  // チェックサムの計算
  byte checksum = 0;
  for (int i = 2; i < 12; i++) {
    checksum += message[i];
  }
  message[12] = ~checksum; // チェックサム

  // コマンドパケットを送信
  for (int i = 0; i < 13; i++) {
    Serial1.write(message[i]);
  }
}


void servo_write(int ch, float ang){
  int sig = 511 + offset[ch] + int((512.0 / 150.0) * ang);
  scs_moveToPos(ch, sig);
  //delayMicroseconds(500);
}



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

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

  if(pos == 0){
    th2 = constrain(th2, -120.0, 120.0);
    th1 = constrain(th1, -80.0, 120.0);
    servo_write(6, th2);
    servo_write(4, th1);
  }else{
    th2 = constrain(th2, -120.0, 120.0);
    th1 = constrain(th1, -120.0, 80.0);
    servo_write(6, -th2);
    servo_write(4, -th1);
  }
}

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

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

  if(pos == 0){
    th2 = constrain(th2, -120.0, 120.0);
    th1 = constrain(th1, -80.0, 120.0);
    servo_write(7, -th2);
    servo_write(5, -th1);
  }else{
    th2 = constrain(th2, -120.0, 120.0);
    th1 = constrain(th1, -120.0, 80.0);
    servo_write(7, th2);
    servo_write(5, th1);
  }
}

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

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

  if(pos == 0){
    th2 = constrain(th2, -120.0, 120.0);
    th1 = constrain(th1, -80.0, 120.0);
    servo_write(3, -th2);
    servo_write(1, -th1);
  }else{
    th2 = constrain(th2, -120.0, 120.0);
    th1 = constrain(th1, -120.0, 80.0);
    servo_write(3, th2);
    servo_write(1, th1);
  }
}

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

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

  if(pos == 0){
    th2 = constrain(th2, -120.0, 120.0);
    th1 = constrain(th1, -80.0, 120.0);
    servo_write(2, th2);
    servo_write(0, th1);
  }else{
    th2 = constrain(th2, -120.0, 120.0);
    th1 = constrain(th1, -120.0, 80.0);
    servo_write(2, -th2);
    servo_write(0, -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

#include <M5Atom.h>

#include <Preferences.h>

#include <Kalman.h>

#include <WiFi.h>

#include <WebServer.h>

WebServer server(80);

const char ssid[] = "robot0009"; // SSID

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

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

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

#define led_pin 27

#define NUM_LEDS 25

Preferences preferences;

// ID 0 1 2 3 4 5 6 7

int offset[] = {-8, -12, 6, 22, 20, 18, 0, 28};

//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;

float hX, hY, hXpre, hYpre;

int pos = 0;

int Mode;

#define L 1

#define R 2

#define Imu 3

#define Jump 4

#define Jump2 42

#define Step 5

#define Stretch 6

#define Advance 7

#define Back 8

#define Roll 9

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";

//パラメータ

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

float wakeUpAngle = -1.0;

int period = 80, x = 60, height = 40, upHeight = 10, stride = 12;

float Kp = 0.3, Kd = 0.3;

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

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

void get_theta() {

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

//傾斜角導出単位はdeg

theta_X = atan(accY / -accZ) * 57.29578f;

theta_Y = -atan(-accX / -accZ) * 57.29578f;

if(Mode != Jump2){

if(pos == 1 && accZ < -0.96) pos = 0;

if(pos == 0 && accZ > 0.96) pos = 1;

}

//Y軸角速度取得

void get_gyro_data() {

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

theta_Xdot = -gyroX;

theta_Ydot = gyroY;

}

//browser

void handleRoot() {

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

temp +="<head>";

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

temp +="<title>robot0009</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 +="<h3>robot0009</h3>";

//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>";

//wakeUpAngle

temp +="wakeUpAngle (deg)<br>";

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

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

temp +="<a class=\"pm\" href=\"/wakeUpAngleP\">+</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 handleL() {

if(modeBtL == "off"){

modeBtL = "on";

modeBtR = "off";

modeBtJump = "off";

modeBtStep = "off";

modeBtStretch = "off";

modeBtAd = "off";

modeBtBack = "off";

modeBtImu = "off";

modeBtRoll = "off";

Mode = L;

}else{

modeBtL = "off";

Mode = 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";

Mode = R;

}else{

modeBtR = "off";

Mode = 0;

}

handleRoot();

}

void handleJump() {

if(modeBtJump == "off" && pos == 0){

modeBtL = "off";

modeBtR = "off";

modeBtJump = "on";

modeBtStep = "off";

modeBtStretch = "off";

modeBtAd = "off";

modeBtBack = "off";

modeBtImu = "off";

modeBtRoll = "off";

Mode = Jump;

}else{

modeBtJump = "off";

Mode = 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";

Mode = Step;

}else{

modeBtStep = "off";

Mode = 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";

Mode = Imu;

}else{

modeBtImu = "off";

Mode = 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";

Mode = Stretch;

}else{

modeBtStretch = "off";

Mode = 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";

Mode = Advance;

}else{

modeBtAd = "off";

Mode = 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";

Mode = Back;

}else{

modeBtBack = "off";

Mode = 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";

Mode = Roll;

}else{

modeBtRoll = "off";

Mode = 0;

}

handleRoot();

}

void handleWakeUpAngleM() {

if(wakeUpAngle > -90.0){

wakeUpAngle -= 0.5;

preferences.putFloat("wakeUpAngle", wakeUpAngle);

}

handleRoot();

}

void handleWakeUpAngleP() {

if(wakeUpAngle <= 90.0){

wakeUpAngle += 0.5;

preferences.putFloat("wakeUpAngle", wakeUpAngle);

}

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 <= 3000){

period += 5;

preferences.putInt("period", period);

}

handleRoot();

}

void handlexM() {

if(x > -615){

x -= 5;

preferences.putInt("x", x);

}

handleRoot();

}

void handlexP() {

if(x < 515){

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 <= 500){

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();

}

//Core0

void browser(void *pvParameters) {

M5.dis.setBrightness(20);

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);

}

delay(50);

disableCore0WDT();

}

void setup() {

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

Serial1.begin(1000000, SERIAL_8N1, -1, 19);

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);

delay(200);

//parameter memory

preferences.begin("parameter", false);

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

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);

WiFi.softAP(ssid, pass);

delay(100);

WiFi.softAPConfig(ip, ip, subnet);

IPAddress myIP = WiFi.softAPIP();

server.on("/", handleRoot);

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("/wakeUpAngleM", handleWakeUpAngleM);

server.on("/wakeUpAngleP", handleWakeUpAngleP);

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.begin();

//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() {

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();

//Serial.println(kalAngleX);

if(Mode == Jump){ //ジャンプ

fRIK(20, 35);

fLIK(20, 35);

rLIK(-25, 95);

rRIK(-25, 95);

delay(500);

fRIK(-35, 100);

fLIK(-35, 100);

delay(160);

rRIK(107, -42);

rLIK(107, -42);

fRIK(-70, 90);

fLIK(-70, 90);

delay(100);

rRIK(80, 30);

rLIK(80, 30);

Mode = Jump2;

fRIK(-85, 40);

fLIK(-85, 40);

}else if(Mode == Jump2){

if(kalAngleX < 0.0 && kalAngleX > wakeUpAngle){

fRIK(60, 40);

fLIK(60, 40);

rRIK(60, 40);

rLIK(60, 40);

Mode = 0;

}

}else if(Mode == 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(Mode == 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 * (0.5 * cos(2*tt) - 0.5));

fLIK(x, height);

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

rRIK(x, height);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

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

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

fLIK(x, height);

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

rRIK(x, height);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

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

fRIK(x, height);

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

rLIK(x, height);

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

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

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

fRIK(x, height);

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

rLIK(x, height);

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

}

}else if(Mode == 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 * (0.5 * cos(2*tt) - 0.5));

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

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

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 * (-0.5 * cos(2*tt) - 0.5));

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

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

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 * (0.5 * cos(2*tt) - 0.5));

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

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

}

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 * (-0.5 * cos(2*tt) - 0.5));

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

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

}

}else if(Mode == 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 * (0.5 * cos(2*tt) - 0.5));

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

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

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 * (-0.5 * cos(2*tt) - 0.5));

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

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

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 * (0.5 * cos(2*tt) - 0.5));

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

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

}

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 * (-0.5 * cos(2*tt) - 0.5));

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

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

}

}else if(Mode == 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(10, 40 + 10 * (0.5 * cos(2*tt) - 0.5));

fLIK(10 + 10 * cos(tt), 40);

rLIK(10 + 10 * cos(tt), 40 + 10 * (0.5 * cos(2*tt) - 0.5));

rRIK(10, 40);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

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

fRIK(10, 40 + 10 * (-0.5 * cos(2*tt) - 0.5));

fLIK(10 - 10 * sin(tt), 40);

rLIK(10 - 10 * sin(tt), 40 + 10 *(-0.5 * cos(2*tt) - 0.5));

rRIK(10, 40);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

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

fRIK(10, 40);

fLIK(10 - 10 * cos(tt), 40 + 10 * (0.5 * cos(2*tt) - 0.5));

rLIK(10 - 10 * cos(tt), 40);

rRIK(10, 40 + 10 * (0.5 * cos(2*tt) - 0.5));

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

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

fRIK(10, 40);

fLIK(10 + 10 * sin(tt), 40 + 10 * (-0.5 * cos(2*tt) - 0.5));

rLIK(10 + 10 * sin(tt), 40);

rRIK(10, 40 + 10 * (-0.5 * cos(2*tt) - 0.5));

}

}else if(Mode == 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(10 - 10 * cos(tt), 40 + 10 * (0.5 * cos(2*tt) - 0.5));

fLIK(10, 40);

rLIK(10, 40 + 10 * (0.5 * cos(2*tt) - 0.5));

rRIK(10 - 10 * cos(tt), 40);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

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

fRIK(10 + 10 * sin(tt), 40 + 10 * (-0.5 * cos(2*tt) - 0.5));

fLIK(10, 40);

rLIK(10, 40 + 10 *(-0.5 * cos(2*tt) - 0.5));

rRIK(10 + 10 * sin(tt), 40);

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

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

fRIK(10 + 10 * cos(tt), 40);

fLIK(10, 40 + 10 * (0.5 * cos(2*tt) - 0.5));

rLIK(10, 40);

rRIK(10 + 10 * cos(tt), 40 + 10 * (0.5 * cos(2*tt) - 0.5));

}

time_mSt= millis();

tim=0;

while(tim<period){

tim=millis()-time_mSt;

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

fRIK(10 - 10 * sin(tt), 40);

fLIK(10, 40 + 10 * (-0.5 * cos(2*tt) - 0.5));

rLIK(10, 40);

rRIK(10 - 10 * sin(tt), 40 + 10 * (-0.5 * cos(2*tt) - 0.5));

}

}else if(Mode == Roll){ //反転

fRIK(50, 30);

fLIK(50, 30);

rRIK(50, 30);

rLIK(50, 30);

delay(500);

fRIK(10, 45);

fLIK(10, 45);

rRIK(0, 80);

rLIK(0, 80);

delay(500);

fRIK(10, 90);

fLIK(10, 90);

delay(150);

fRIK(70, -50);

fLIK(70, -50);

delay(200);

rRIK(70, -50);

rLIK(70, -50);

delay(600);

Mode = 0;

}else if(Mode == Imu){

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

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

hXpre = hX;

hYpre = hY;

if(pos == 1){ //反転時

hX = -hX;

hY = -hY;

}

fRIK(x, height - hX + hY);

fLIK(x, height - hX - hY);

rRIK(x, height + hX + hY);

rLIK(x, height + hX - hY);

}else{ //初期姿勢

fRIK(x, height);

fLIK(x, height);

rRIK(x, height);

rLIK(x, height);

}

//参考 https://qiita.com/Ninagawa123/items/7b79c5f5117dd1470ac9

void scs_moveToPos(byte id, int position) {

// コマンドパケットを作成

byte message[13];

message[0] = 0xFF; // ヘッダ

message[1] = 0xFF; // ヘッダ

message[2] = id; // サーボID

message[3] = 9; // パケットデータ長

message[4] = 3; // コマンド（3は書き込み命令）

message[5] = 42; // レジスタ先頭番号

message[6] = (position >> 8) & 0xFF; // 位置情報バイト上位

message[7] = position & 0xFF; // 位置情報バイト下位

message[8] = 0x00; // 時間情報バイト下位

message[9] = 0x00; // 時間情報バイト上位

message[10] = 0x00; // 速度情報バイト下位

message[11] = 0x00; // 速度情報バイト上位

// チェックサムの計算

byte checksum = 0;

for (int i = 2; i < 12; i++) {

checksum += message[i];

}

message[12] = ~checksum; // チェックサム

// コマンドパケットを送信

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

Serial1.write(message[i]);

}

void servo_write(int ch, float ang){

int sig = 511 + offset[ch] + int((512.0 / 150.0) * ang);

scs_moveToPos(ch, sig);

//delayMicroseconds(500);

}

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

//IK

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

void fRIK(float x, float z){

float phi,ld, th1, th2;

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

phi = atan2(x, z);

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

th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) * 180.0 / PI;

if(pos == 0){

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

th1 = constrain(th1, -80.0, 120.0);

servo_write(6, th2);

servo_write(4, th1);

}else{

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

th1 = constrain(th1, -120.0, 80.0);

servo_write(6, -th2);

servo_write(4, -th1);

}

void fLIK(float x, float z){

float phi,ld, th1, th2;

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

phi = atan2(x, z);

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

th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) * 180.0 / PI;

if(pos == 0){

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

th1 = constrain(th1, -80.0, 120.0);

servo_write(7, -th2);

servo_write(5, -th1);

}else{

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

th1 = constrain(th1, -120.0, 80.0);

servo_write(7, th2);

servo_write(5, th1);

}

void rRIK(float x, float z){

float phi,ld, th1, th2;

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

phi = atan2(x, z);

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

th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) * 180.0 / PI;

if(pos == 0){

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

th1 = constrain(th1, -80.0, 120.0);

servo_write(3, -th2);

servo_write(1, -th1);

}else{

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

th1 = constrain(th1, -120.0, 80.0);

servo_write(3, th2);

servo_write(1, th1);

}

void rLIK(float x, float z){

float phi,ld, th1, th2;

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

phi = atan2(x, z);

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

th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) * 180.0 / PI;

if(pos == 0){

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

th1 = constrain(th1, -80.0, 120.0);

servo_write(2, th2);

servo_write(0, th1);

}else{

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

th1 = constrain(th1, -120.0, 80.0);

servo_write(2, -th2);

servo_write(0, -th1);

}

使用ライブラリ

ボードマネージャ
　ESP32 バージョン 2.0.15
　M5Stack バージョン 2.1.1

ライブラリマネージャ
　M5ATOM バージョン 0.1.2

Kalman Filter Library 　バージョン1.0.2
　https://github.com/TKJElectronics/KalmanFilter

コード説明

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

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

L. 19 サーボオフセット調整アプリで導出したオフセット値をoffset[]配列に入力してください。

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

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

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

　また、Z軸の加速度によってロボットの反転状態を検出しています(pos)。

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

L. 93～515 ブラウザアプリ表示設定
　APモード接続時の動作モード選択ボタンや各パラメータ設定用の表示を記述しています。　

L. 519～608 core0動作
　ATOM Matrix デュアルコアのcore0でディスプレイ表示とブラウザ表示とハンドラ処理を実行します。

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

L. 611～694 setup関数
　UART1：送信ピンGPIO19、ボーレート1Mbpsに設定 (L. 614)

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

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

　WiFiとブラウザアプリ用のハンドラを設定 (L. 644～694)

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

　バク転動作モード (L. 718~751)
　　後述の各足の逆運動学関数で足先の座標を指定してバク転動作を実現しています。
　　ATOM Matrixが上向きの姿勢の時のみ起動します。反転時は足移動の制約によりバク転できません。
　　着地後に伏せの姿勢に戻るタイミングを機体角度(X軸)をトリガにしており、パラメータwakeUpAngle(初期値 -1.0°)で調整可能です。

　屈伸、足踏み、前後進、左右旋回動作モード (L. 753~995)
　　逆運動学関数で足先の座標を指定して各動作を実現しています。
　　足の上下、左右の移動にはそれぞれ正弦波形で緩やかに動かしています。
　　以下のパラメータで動作を調整可能です。
　　・period：動作の周期時間を指定
　　・x：各足先の前後位置指定
　　・height：各足先の付け根からの高さ位置指定
　　・upHeight：足上げ動作時の上げ量を指定
　　・stride：歩行時の歩幅指定

　反転動作モード (L. 996~1021)
　　起上って反転動作します。

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

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

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

L. 1048～1075 サーボ駆動用 scs_moveToPos()関数
　参考：Feetechのサーボをマイコンから直接動かす

L. 1078～1082 servo_write() 関数
　サーボIDと角度を指定する。scs_moveToPos()関数を用いてオフセット値を加算して信号設定。

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

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

　機体の反転時もサーボ指定角度の符号変えるだけで同じ座標指定シーケンスでの動作制御が可能となってます。
　各サーボ角度は他のパーツなどに干渉しないようにconstrain関数で動作範囲角度を制限しております。

アプリ画面

調整用アプリをATOM Matrixに書き込んで、スマホもしくはPCのWiFi接続設定でSSID ”robot0009”に接続
　パスワード：password
ブラウザで”192.168.55.27”にアクセス

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

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

動作

動作の様子を以下の動画にまとめました。

おわりに

ここではシリアルサーボ SCS0009による4脚ロボットの製法を記載しました。

シンプルなシステム構成で豊富な動作表現を実現できたと考えておりますので、是非参考にしていただければ幸いです。

何かございましたらコメントいただけますと幸いです。

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

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

はじめに

構成

回路図

部品

専用基板

3Dモデル

改訂履歴

筐体組み立てキット

ロボット製作

サーボモータ設定

筐体組み立て

コントローラ

電源周り

組み立て動画

サーボオフセット調整

調整の様子

調整用アプリ Arduinoコード

使用ライブラリ

コード説明

調整用アプリ画面

調整アプリ制作動画

Arduinoコード

使用ライブラリ

コード説明

アプリ画面

動作

おわりに

関連記事

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

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