M5Stackで作る二足歩行ロボット Bluetooth接続の Mac版サーボコントロールアプリをXcodeで作った。

The biped robot made with M5Stack Bluetooth-connected Mac version servo control app made by Xcode.

こんにちはRoboTakaoです。
Hello everyone! I'm RoboTakao.

３Dプリンターが調子悪かったので、部品手配中にロボットのサーボをコントロールするためのMac版アプリをXcodeで作りました。
Since the 3D printer was in a bad condition, I made a Mac version app by Xcode to control the servo of the robot.



M5Stack側はBluetoothSerialを使います。
M5Stack side uses BluetoothSerial.

iPhoneとかに繋げるためにはBLEが必要ですが、パソコンならレガシーなBluetoothでシリアル接続すれば簡単です。
BLE is necessary to connect to iPhone, but if it is a PC, it is easy if it is serial connection with the legacy Bluetooth.

BluetoothSerialの使い方は簡単
How to use BluetoothSerial is easy.

BluetoothSerial.hをインクルードすればOK
Include BluetoothSerial.h

BluetoothSerial SerialBT; でインスタンス生成　Instantiate
SerialBT.begin("M5Stack"); でPC側から”M5Stack"という名前でペアリングできるようにします。Make it possible to pair with the name "M5Stack" from the PC side.



後は
SerialBT.read(); でPC側からのデータを読み出します。Reads data from the PC side.

今回はMac版のサーボコントロールアプリをXcodeでObjective-Cを使ってプログラミングしました。
This time, I programmed the Mac version of the servo control application using Objective-C with Xcode.






動作確認　Operation check

アプリの中の数字を動かしていけばサーボも回転してくれます。
If you change the numbers in the app, the servo will also rotate.



それではこの辺で失礼します。
Thank you. Good-bye!

スケッチ　Scketch

#include <M5Stack.h>
#include <ESP32Servo.h>    //ESPservo
#include <Wire.h>          // I2C setting
#include <PCA9685.h>       //PCA9685用ヘッダーファイル
#include "BluetoothSerial.h" //BluetoothSerial

BluetoothSerial SerialBT;

int ang0[] = {90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90};
int ang1[] = {90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90,90};
int ang_b[18];
char ang_c[18];
int ts=400;
int td=10;
int a[18],b[18];

// create four servo objects (ESP32servo)
Servo servo3;
Servo servo4;

//PCA9685のアドレス指定
PCA9685 pwm = PCA9685(0x40);

// Published values for SG90 servos; adjust if needed (ESP32servo)
int minUs = 700;
int maxUs = 2300;

#define SERVOMIN 150            //最小パルス幅 (GWS Micro2BBMG) (PCA9685)
#define SERVOMAX 500            //最大パルス幅 (GWS Micro2BBMG) (PCA9685)

// These are all GPIO pins on the ESP32
// Recommended pins include 2,4,12-19,21-23,25-27,32-33
//int servo1Pin = 2;  //ESP32servo
//int servo2Pin = 5;  //ESP32servo 
int servo3Pin = 12;  //ESP32servo
int servo4Pin = 13;  //ESP32servo
//int servo5Pin = 16;  //ESP32servo
//int servo6Pin = 17;  //ESP32servo

void setup() 
{ 
  M5.begin();
  dacWrite(25,0);
  M5.Lcd.println("Bluetooth Now");
  
  Serial.begin(9600);
  SerialBT.begin("M5Stack");  //PC側で確認するときの名前

  servo3.setPeriodHertz(50);      // Standard 50hz servo  (ESP32servo)
  servo4.setPeriodHertz(50);      // Standard 50hz servo  (ESP32servo)
    
  servo3.attach(servo3Pin, minUs, maxUs);  // ESP32servo
  servo4.attach(servo4Pin, minUs, maxUs);  // ESP32servo
  
  pwm.begin();                   //初期設定 (アドレス0x40用) (PCA9685)
  pwm.setPWMFreq(50);            //PWM周期を50Hzに設定 (アドレス0x40用) (PCA9685)
 
} 
 
void loop() 
{   
  for (int j=0; j <=17 ; j++){
      a[j] = ang1[j] - ang0[j];
      b[j] = ang0[j];
      ang0[j] = ang1[j];
  }

  for (int k=0; k <=td ; k++){

      // ESP32servo

      servo3.write(a[0]*k/td+b[0]);
      servo4.write(a[1]*k/td+b[1]);

      // PCA9685
      servo_write(0,a[2]*k/td+b[2]);
      servo_write(1,a[3]*k/td+b[3]);
      servo_write(2,a[4]*k/td+b[4]);
      servo_write(3,a[5]*k/td+b[5]);
      servo_write(4,a[6]*k/td+b[6]);
      servo_write(5,a[7]*k/td+b[7]);
      servo_write(6,a[8]*k/td+b[8]);
      servo_write(7,a[9]*k/td+b[9]);
      servo_write(8,a[10]*k/td+b[10]);
      servo_write(9,a[11]*k/td+b[11]);
      servo_write(10,a[12]*k/td+b[12]);
      servo_write(11,a[13]*k/td+b[13]);
      servo_write(12,a[14]*k/td+b[14]);
      servo_write(13,a[15]*k/td+b[15]);
      servo_write(14,a[16]*k/td+b[16]);
      servo_write(15,a[17]*k/td+b[17]);

      delay(ts/td);
  }

  if(SerialBT.available()){
    for (int i=0; i <=17; i++){
      rs_gets_3c(ang_c);
      ang_b[i] = atoi(ang_c);
      if ((ang_b[i] > 0) && (ang_b[i] <=180))
      {
        M5.Lcd.print(ang_b[i]);
        M5.Lcd.print(",");
        ang0[i] = ang1[i];
        ang1[i] = ang_b[i];
      }
    }
    M5.Lcd.println("");
  }

  delay(500);
} 

void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
  ang = map(ang, 0, 180, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅（150～500）に変換
  pwm.setPWM(ch, 0, ang);
}

void rs_gets_3c(char gtc[4])
{
  char gc;
  int i;
 
    sprintf(gtc,"    ");
    for( i = 0 ; i < 4 ; i++ )
    {
      gc = SerialBT.read();
      if (gc == ',')
        break;
      else
        gtc[i] = gc;
    }
}