MS（マスタースレーブロボ試作）

マスタースレーブ操作式ロボットの試作

全身の動きで小型ロボを操作する。使用サーボは12個。

とりあえず作ってはみたが課題は多い。

無線通信も結局諦めて有線にした。

写真、動画はこちら

製作期間 2022/10 ～ 2022/12 

--

2023.11.24　追記

無線通信で苦労したがいま思うとesp32の電源が安定していなかったのかと思う。

5Vのモバイルバッテリーで駆動させると安定する、気がするがそもそも結構難しいマイコンなのかもしれない。

--

以下はプログラムメモ

#include <Servo.h>

const int SERVO_MAX = 12;

Servo _servo[SERVO_MAX];

const int SERVO_PIN[SERVO_MAX] =

{ //0   1   2   3   4   5   6   7   8   9  10  11

  22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33

};

const int MAX = 12;

byte src_v[MAX];

byte now_v[MAX];

void setup() {

  Serial.begin(115200);

  delay(100);

  get_src();

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

    Serial.print("Servo at "); Serial.print(i); Serial.print(" ");

    Serial.print(SERVO_PIN[i]); Serial.print(" ");

    Serial.println(now_v[i]);

    _servo[i].attach(SERVO_PIN[i]);

    _servo[i].write(now_v[i]);

    delay(100);

  }

  Serial.println("setup ok.");

}

void loop() {

  get_src();

  //print_v(now_v);

  _servo[0].write(now_v[0]);//右ハンド

  _servo[1].write(now_v[1]);//右手首

  _servo[2].write(now_v[2]);//右肘

  _servo[3].write(now_v[3]);//右肩左右

  _servo[4].write(now_v[4]);//右肩上下

  _servo[5].write(now_v[5]);//左ハンド

  _servo[6].write(now_v[6]);//左手首

  _servo[7].write(now_v[7]);//左肘

  _servo[8].write(now_v[8]);//左肩左右

  _servo[9].write(now_v[9]);//左肩上下

  _servo[10].write(now_v[10]);//前屈

  _servo[11].write(now_v[11]);//腰回転

  //_servo[10].write(90);

  //_servo[11].write(90);

  delay(16);

}

void rhand_cal() {

  int v0 = analogRead(0);//右ハンド開閉

  int v1 = analogRead(1);//右手首回転

  int v2 = analogRead(2);//右肘　////150 480

  int v3 = analogRead(3);//右肩　左右

  int v4 = 1023-analogRead(4);//右肩　上下

  int x0 = val_calc(v0, 680, 800); src_v[0] = (byte)map(x0, 0, 120, 1, 180); //print_vx(0, v0, x0, src_v[0]);

  int x1 = val_calc(v1, 240, 750); src_v[1] = (byte)map(x1, 0, 510, 180, 1); //print_vx(1, v1, x1, src_v[1]);

  int x2 = val_calc(v2, 360, 780); src_v[2] = (byte)map(x2, 0, 420, 50, 160); //print_vx(2, v2, x2, src_v[2]);

  int x3 = val_calc(v3, 300, 750); src_v[3] = (byte)map(x3, 0, 450, 140, 30); //print_vx(3, v3, x3, src_v[3]);

  int x4 = val_calc(v4, 450, 1023); src_v[4] = (byte)map(x4, 0, 573, 30, 180); //print_vx(4, v4, x4, src_v[4]);

}

void lhand_cal() {

  int v5 = 1023 - analogRead(5);//左ハンド開閉 (705 -> 607) 320 -> 420

  int v6 = analogRead(6);//左手首回転

  int v7 = analogRead(7);//左肘 306 -> 695

  int v8 = analogRead(8);//左肩　左右 400 -> 820

  int v9 = analogRead(9);//左肩　上下

  int x5 = val_calc(v5, 340, 420); src_v[5] = (byte)map(x5, 0, 80, 1, 180); //print_vx(5, v5, x5, src_v[5]);

  int x6 = val_calc(v6, 160, 820); src_v[6] = (byte)map(x6, 0, 660, 180, 1); //print_vx(6, v6, x6, src_v[6]);

  int x7 = val_calc(v7, 305, 695); src_v[7] = (byte)map(x7, 0, 390, 50, 160); //print_vx(7, v7, x7, src_v[7]);

  int x8 = val_calc(v8, 400, 800); src_v[8] = (byte)map(x8, 0, 400, 30, 140); //print_vx(8, v8, x8, src_v[8]);

  int x9 = val_calc(v9, 270, 870); src_v[9] = (byte)map(x9, 0, 600, 150, 1); //print_vx(9, v9, x9, src_v[9]);

}

void koshi() {

  int v10 = analogRead(10);//前屈

  int v11 = analogRead(11);//腰回転

  int x10 = val_calc(v10, 500, 850); src_v[10] = (byte)map(x10, 0, 350, 90, 160); //print_vx(10, v10, x10, src_v[10]);

  int x11 = val_calc(v11, 350, 750); src_v[11] = (byte)map(x11, 0, 400, 180, 1); //print_vx(11, v11, x11, src_v[11]);

}

byte limit_servo(int no, int v) {

  if (no == 2 || no == 7) { //肘　左右 動き幅 120

    if (v > 160) return 160;

    if (v < 40) return 40;

  }

  if (no == 3 || no == 8) { //肩 左右 動き幅 110

    if (v > 140) return 140;

    if (v < 30) return 30;

  }

  if (no == 10) {//前屈

    if (v > 160) return 160;

    if (v < 30) return 30;

  }

  return v;

}

void get_src() {

  rhand_cal();

  lhand_cal();

  koshi();

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

    now_v[i] = limit_servo(i, src_v[i]);

  }

}

int val_calc(int v, int _min, int _max) {

  int x = 0;

  int max_v = _max - _min;

  if (v > _min) {

    x = v - _min;

    if (v > _max) {

      x = max_v;

    }

  }

  return x;;

}

void print_v(byte v[]) {

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

    Serial.print(i); Serial.print(".");

    Serial.print(analogRead(i)); Serial.print(".");

    Serial.print(v[i]);

    Serial.print("\t");

  }

  Serial.println("");

}

void print_vx(int no, int v, int x, int src_v) {

  Serial.print(no); Serial.print("\t");

  Serial.print(v); Serial.print("\t");

  Serial.print(x); Serial.print("\t");

  Serial.print(src_v);  Serial.print("\t");

  Serial.println("");

}