65歳からのWordPress

Tinkerkit Braccio(1)

Arduino, Braccio 2022年4月17日

（１）Arduino側のプログラム作成

　やり方は頭脳と同じく単純に、ボケ老人にふさわしく。
　”s0,135,0,0,0,0;”　のように’s’で始まり’;’で終わり’,’で区切られた文字列を一文字ずつbluetoothで受信し、モータの制御を行うことに。
　頭の’s’と最後の’;’を取り除き、’,’で区切られた文字列を6個の数値に変換し、順番にM1～M6に入れ、これで各モータの角度を指定する。
　色々なサイトからの寄せ集めのコードがこちら、恥ずかしいかぎり。

BTBraccio.ino

/*
 Example sketch for the RFCOMM/SPP Bluetooth library - developed by Kristian Lauszus
 For more information visit my blog: http://blog.tkjelectronics.dk/ or
 send me an e-mail:  kristianl@tkjelectronics.com
 */
#include <SPP.h>
#include <usbhub.h>

// Satisfy the IDE, which needs to see the include statment in the ino too.
#ifdef dobogusinclude
#include <spi4teensy3.h>
#endif
#include <SPI.h>

USB Usb;
USBHub Hub1(&Usb); // Some dongles have a hub inside

BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
/* You can create the instance of the class in two ways */
SPP SerialBT(&Btd,"ArdBraccio","4321"); // This will set the name to the defaults: "Arduino" and the pin to "0000"
//SPP SerialBT(&Btd, "Lauszus's Arduino", "1234"); // You can also set the name and pin like so

bool firstMessage = true;

#include <Braccio.h>
#include <Servo.h>

Servo base;
Servo shoulder;
Servo elbow;
Servo wrist_ver;
Servo wrist_rot;
Servo gripper;

String recvStr;
String moveStr;
String moveStrold;

int motor[6] = {0,0,0,0,0,0}; // motor 

// String型のカンマ区切り文字列をint型配列に分解する関数
void stringToIntValues(String str, int value[], char delim) {
  int k = 0;
  int j = 0;
  char text[8];
  for (int i = 0; i <= str.length(); i++) {
    char c = str.charAt(i);
    if ( c == delim || i == str.length() ) {
      text[k] = '\0';
      value[j] = atoi(text);
      j++;
      k = 0;
    } else {
      text[k] = c;
      k++;
    }
  }
}

void setup() {
  Braccio.begin();
  recvStr = "";
  moveStr = "s0,135,0,0,0,0;";
  moveStrold = "s0,135,0,0,0,0;";
  Serial.begin(115200);
#if !defined(__MIPSEL__)
  while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
#endif
  if (Usb.Init() == -1) {
    Serial.print(F("\r\nOSC did not start"));
    while (1); //halt
  }
  Serial.print(F("\r\nSPP Bluetooth Library Started"));
}
void loop() {
  Usb.Task(); // The SPP data is actually not send until this is called, one could call SerialBT.send() directly as well

  if (SerialBT.connected) {
    if (firstMessage) {
      firstMessage = false;
      SerialBT.println(F("Hello from Arduino")); // Send welcome message
    }
    if (Serial.available())
      SerialBT.write(Serial.read());
    if (SerialBT.available()){
      char c = SerialBT.read();
      if(c == 's'){
        recvStr = "";
        recvStr = recvStr + c;
        moveStr = moveStrold;
      } else if(c==','||c=='0'||c=='1'||c=='2'||c=='3'||c=='4'||c=='5'||c=='6'||c=='7'||c=='8'||c=='9'){
        recvStr = recvStr + c;
        moveStr = moveStrold;
      } else if(c==';'){
        recvStr = recvStr + c;
        moveStr = recvStr;
        moveStrold = moveStr;
        recvStr = "";
      } else{
      }

      stringToIntValues(moveStr.substring(1,moveStr.length()-1),motor,',');
      //(stepdelay,  M1,  M2,  M3,  M4,  M5,  M6)
      Braccio.ServoMovement( 20,motor[0],motor[1],motor[2],motor[3],motor[4],motor[5]);
    }
  }
}