Crane Swings

クレーンスウィングス

LINE Things Mini Award応募用

2019/09/22

みなみじま　こういち

概要

  • LINEで操作できるUFOキャッチャー
  • 食後 の一家団欒時に、食後のおやつをゲームで楽しむ。
  • Wifi不要。電源とLINEがあればどこでも楽しめる

システム構成（超簡易的）

使ったアイテム

  • arduino mega 互換機
  • esp32 devkit​
  • タミヤ ユニバーサルプレート等多数
  • ダンボールとか
  • 100均で細かいものをいろいろ
  • サーボモーター SG90x2基
  • DCモーター（タミヤ4速パワーギヤボックスHE) x2基
  • L293Dモーターシールド
  • 超音波センサー(HC-SR04)
  • 電池ボックス1.5v x 8 = 12v
  • 電池ボックス3.7v x 2 = 7.4v
  • サーボ付きのアーム

プログラムソース -LIFF-

https://github.com/ikegam1/line-things-starter/tree/master/liff-app

 

役割：

 line-things-starterのソースを改変したもの

 ボタンが押された時にESP32に信号を送っています。

※プロトタイプ

プログラムソース -ESP32-

https://github.com/ikegam1/arduino-crane-swings/tree/master/line-things-ble-hub

 

役割：

 LIFFと直接やりとりしており、裏でarduinoとSerial通信をしている。

 （ちょうどいいBLEモジュールが入手できなかったため）
SerialはPCとの通信時に、arduinoとはSerial2を使用

※まだ機能していないがロードセルでGETしたお菓子の重量を測っています。
​

#include "env.h"
#include <BLEServer.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <WiFi.h>

#define RXD1 18
#define TXD1 19
#define RXD2 25
#define TXD2 26

#define pin_dout  14
#define pin_slk   27

void AE_HX711_Init(void);
void AE_HX711_Reset(void);
long AE_HX711_Read(void);
long AE_HX711_Averaging(long adc,char num);
float AE_HX711_getGram(char num);

#define OUT_VOL   0.001f   //定格出力 [V]
#define LOAD      1000.0f    //定格容量 [g]

//HardwareSerial Serial2(2);
BLEServer* thingsServer;
BLESecurity *thingsSecurity;
BLEService* userService;
BLEService* psdiService;
BLECharacteristic* psdiCharacteristic;
BLECharacteristic* writeCharacteristic;
BLECharacteristic* notifyCharacteristic;

bool deviceConnected = false;
bool oldDeviceConnected = false;

volatile int btnAction = 0;
uint8_t btnValue;

class serverCallbacks: public BLEServerCallbacks {
  void onConnect(BLEServer* pServer) {
   deviceConnected = true;
  };

  void onDisconnect(BLEServer* pServer) {
    deviceConnected = false;
  }
};

class writeCallback: public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *bleWriteCharacteristic) {
    std::string value = bleWriteCharacteristic->getValue();
    Serial.println(value.c_str());
    Serial2.println(value.c_str());
    delay(10);
  }
};

float offset;

void setup() {
  //Wifiは使わない
  WiFi.mode(WIFI_OFF);
  Serial.begin(115200);
  Serial.setDebugOutput(true);

  Serial.println("BLE Deveice init");
  BLEDevice::init("");
  BLEDevice::setEncryptionLevel(ESP_BLE_SEC_ENCRYPT_NO_MITM);

  // Security Settings
  BLESecurity *thingsSecurity = new BLESecurity();
  thingsSecurity->setAuthenticationMode(ESP_LE_AUTH_REQ_SC_ONLY);
  thingsSecurity->setCapability(ESP_IO_CAP_NONE);
  thingsSecurity->setInitEncryptionKey(ESP_BLE_ENC_KEY_MASK | ESP_BLE_ID_KEY_MASK);

  setupServices();
  startAdvertising();

  Serial.println("Ready to Connect");
  Serial1.begin(9600, SERIAL_8N1, RXD1, TXD1);
  Serial2.begin(9600, SERIAL_8N1, RXD2, TXD2);
  Serial2.println(0);
  delay(50);

  AE_HX711_Init();
  AE_HX711_Reset();
  offset = AE_HX711_getGram(30); 
}

void loop() {
  String input;
  uint8_t finish = 36;
  uint8_t i;
  
  // Disconnection
  if (!deviceConnected && oldDeviceConnected) {
    delay(500); // Wait for BLE Stack to be ready
    thingsServer->startAdvertising(); // Restart advertising
    oldDeviceConnected = deviceConnected;
  }
  // Connection
  if (deviceConnected && !oldDeviceConnected) {
    oldDeviceConnected = deviceConnected;
  }

  float data;
  char S1[20];
  char s[20];
  data = AE_HX711_getGram(5);

  //data = 55.0 / 50.0; //ここで校正を行う

  sprintf(S1,"%s [g] (0x%4x)",dtostrf((data-offset), 5, 3, s),AE_HX711_Read());
  //10g以上で何かとれたと判断
  if((data-offset) >= 10.0){
    Serial.println((data-offset));
    Serial.println(S1);
    //Thingsに送る
    notifyCharacteristic->setValue(&finish, 1);
    notifyCharacteristic->notify();
  }
  
  //esp32からのリターン
  if (Serial1.available() > 0) {
      Serial.println("Receive:");
      // 改行コード(10)を検出したら、そこまでの文字列を取得
      input = Serial1.readStringUntil(10);
      Serial.println(input);
      i = input[0] - '0';
      if ( i >= 0 ){
        //Thingsに送る
        notifyCharacteristic->setValue(&i, 1);
        notifyCharacteristic->notify();
      }
  }

  delay(50);
}

void setupServices(void) {
  // Create BLE Server
  thingsServer = BLEDevice::createServer();
  thingsServer->setCallbacks(new serverCallbacks());

  // Setup User Service
  userService = thingsServer->createService(USER_SERVICE_UUID);
  // Create Characteristics for User Service
  writeCharacteristic = userService->createCharacteristic(WRITE_CHARACTERISTIC_UUID, BLECharacteristic::PROPERTY_WRITE);
  writeCharacteristic->setAccessPermissions(ESP_GATT_PERM_READ_ENCRYPTED | ESP_GATT_PERM_WRITE_ENCRYPTED);
  writeCharacteristic->setCallbacks(new writeCallback());

  notifyCharacteristic = userService->createCharacteristic(NOTIFY_CHARACTERISTIC_UUID, BLECharacteristic::PROPERTY_NOTIFY);
  notifyCharacteristic->setAccessPermissions(ESP_GATT_PERM_READ_ENCRYPTED | ESP_GATT_PERM_WRITE_ENCRYPTED);
  BLE2902* ble9202 = new BLE2902();
  ble9202->setNotifications(true);
  ble9202->setAccessPermissions(ESP_GATT_PERM_READ_ENCRYPTED | ESP_GATT_PERM_WRITE_ENCRYPTED);
  notifyCharacteristic->addDescriptor(ble9202);

  // Setup PSDI Service
  psdiService = thingsServer->createService(PSDI_SERVICE_UUID);
  psdiCharacteristic = psdiService->createCharacteristic(PSDI_CHARACTERISTIC_UUID, BLECharacteristic::PROPERTY_READ);
  psdiCharacteristic->setAccessPermissions(ESP_GATT_PERM_READ_ENCRYPTED | ESP_GATT_PERM_WRITE_ENCRYPTED);

  // Set PSDI (Product Specific Device ID) value
  uint64_t macAddress = ESP.getEfuseMac();
  psdiCharacteristic->setValue((uint8_t*) &macAddress, sizeof(macAddress));

  // Start BLE Services
  userService->start();
  psdiService->start();
}

void startAdvertising(void) {
  // Start Advertising
  BLEAdvertisementData scanResponseData = BLEAdvertisementData();
  scanResponseData.setFlags(0x06); // GENERAL_DISC_MODE 0x02 | BR_EDR_NOT_SUPPORTED 0x04
  scanResponseData.setName(DEVICE_NAME);

  thingsServer->getAdvertising()->addServiceUUID(userService->getUUID());
  thingsServer->getAdvertising()->setScanResponseData(scanResponseData);
  thingsServer->getAdvertising()->start();
}

void AE_HX711_Init(void)
{
  pinMode(pin_slk, OUTPUT);
  pinMode(pin_dout, INPUT);
}

void AE_HX711_Reset(void)
{
  digitalWrite(pin_slk,1);
  delayMicroseconds(100);
  digitalWrite(pin_slk,0);
  delayMicroseconds(100); 
}

long AE_HX711_Read(void)
{
  long data=0;
  while(digitalRead(pin_dout)!=0);
  delayMicroseconds(10);
  for(int i=0;i<24;i++)
  {
    digitalWrite(pin_slk,1);
    delayMicroseconds(5);
    digitalWrite(pin_slk,0);
    delayMicroseconds(5);
    data = (data<<1)|(digitalRead(pin_dout));
  }
  //Serial.println(data,HEX);   
  digitalWrite(pin_slk,1);
  delayMicroseconds(10);
  digitalWrite(pin_slk,0);
  delayMicroseconds(10);
  return data^0x800000; 
}


long AE_HX711_Averaging(long adc,char num)
{
  long sum = 0;
  for (int i = 0; i < num; i++) sum += AE_HX711_Read();
  return sum / num;
}

float AE_HX711_getGram(char num)
{
  #define HX711_R1  20000.0f
  #define HX711_R2  8200.0f
  #define HX711_VBG 1.25f
  #define HX711_AVDD      4.2987f//(HX711_VBG*((HX711_R1+HX711_R2)/HX711_R2))
  #define HX711_ADC1bit   HX711_AVDD/16777216 //16777216=(2^24)
  #define HX711_PGA 128
  #define HX711_SCALE     (OUT_VOL * HX711_AVDD / LOAD *HX711_PGA)
  
  float data;

  data = AE_HX711_Averaging(AE_HX711_Read(),num)*HX711_ADC1bit; 
  //Serial.println( HX711_AVDD);   
  //Serial.println( HX711_ADC1bit);   
  //Serial.println( HX711_SCALE);   
  //Serial.println( data);   
  data =  data / HX711_SCALE;
  

  return data;
}

プログラムソース -arduino-

https://github.com/ikegam1/arduino-crane-swings/tree/master/things-party-core

 

役割：

 メインのプログラム。

 状態をフラグで管理しており、DCモーター2基とサーボモーター2基を操作しています。

また、超音波センサーでアームの水平位置や垂直位置をキャッチし、制御に使用。

​

#include <SoftwareSerial.h>
#include <Servo.h>
//#include <MsTimer2.h>
//#include <TimerOne.h>
#include "config.h"
#include "env.h"
// Adafruit Motor Shild Libralyより
#include <AFMotor.h>

//BLEシリアルのPIN
SoftwareSerial BTserial(BTSerialRX,BTSerialTX); // RX, TX

// DCモーターのM4
// クレーンの左右
// わかりづらくなってしまったけど、M3がmotor4
AF_DCMotor motor4(3);
// HC-SR04の数値
long durationA;
int distanceA;

// DCモーターのM3
// クレーンの巻き上げ
AF_DCMotor motor3(4);
// HC-SR04の数値
long durationB;
int distanceB;

// アーム回転サーボ
Servo servoA;
int posA = 0;

// アーム開閉サーボ
Servo servoB;
int posB = 0;

//アクションフラグ
//各アクションを判別する(*=input)
// 0 = スタート
// 1 = クレーン右 *
// 2 = 水平移動ストップ *
// 3 = アーム回転 *
// 4 = アームストップ
// 5 = クレーン下
// 6 =
// 7 = アーム閉じる＆クレーン上 *
// 8 = クレーン左
// 9 = クレーン左
int f=0;

void setup() {
  // Open serial communications and wait for port to open:
  Serial.begin(115200);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
    
  // Define inputs and outputs:
  pinMode(trigPinA, OUTPUT);
  pinMode(echoPinA, INPUT);
  pinMode(trigPinB, OUTPUT);
  pinMode(echoPinB, INPUT);
  servoA.attach(ServoPinA);
  servoB.attach(ServoPinB);

  // turn on motor
  motor4.setSpeed(200);
  motor4.run(RELEASE);
  
  BTserial.begin(9600);  
  Serial.println("BTserial started at 9600");

  pinMode(LedGreen, OUTPUT);
  pinMode(LedBlue, OUTPUT);
  pinMode(LedRed, OUTPUT);

  OpenArm();
  servoA.write(0);
  delay(100);
  
  //timer
  //Timer1.initialize(1000000);
  //Timer1.attachInterrupt(LedBlink);
  //MsTimer2::set(1000, LedBlink);
  //MsTimer2::start();
}

int led = 11; //R=100, G=10, B = 1 while=111, yellow=110,  cyan=11
int recentDistanceA = 0;
int recentDistanceB = 0;
//床に置かれているお菓子などが超音波センサーに狂いを生じさせる。
//移動量は固定にして対応
int downCnt = 0;
int upCnt = 0; 
uint8_t i;
String input;
void loop() {
    if (BTserial.available() > 0) { // 受信したデータが存在する
        Serial.println("Receive:");
        // 改行コード(10)を検出したら、そこまでの文字列を取得
        input = BTserial.readStringUntil(10);
        Serial.println(input);
        i = input[0] - '0';
        if ( i >= 0 ){
          f = i;
          Serial.print("Receive:");
          Serial.println(f);
        }
    }
    
    LedBlink();
    //f=FlgRotate; //debug
        
    //start前
    if(f==FlgStart){
      led = 111;
      downCnt = 0;
      upCnt = 0;
      servoB.write(0);
      servoB.write(180);
      delay(10);
      return;
    }

    //Serial.print("mode=");
    //Serial.println(f);
    
    //右移動
    if(f==FlgRight){
      led = 1;
      GetDistanceA();
      Serial.println("mode=FlgRight");
      Serial.print("distanceA=");
      Serial.println(distanceA);
      //誤差がおかしいときは何もしない
      if(abs(distanceA - recentDistanceA) > 100 && recentDistanceA != 0){
        Serial.print("abs(distanceA - recentDistanceA)");
        Serial.println(abs(distanceA - recentDistanceA));
        return;
      }
      //limitを超えるとStop
      if(distanceA >= limitMaxPinA){
        motor4.run(RELEASE);
        delay(100);
        f = FlgStop;
        return;
      }

      //モーターを動かす
      motor4.run(FORWARD);
      Serial.print("motor4 run");
      for (i=0; i<=180; i+=1) {
        motor4.setSpeed(i);
        delay(50);
      }
      delay(10);
    
      recentDistanceA = distanceA;
    }

    //stop
    if(f==FlgStop){
      led = 0;
      //Serial.print("mode=FlgStop");
      motor4.run(RELEASE);
      delay(10);      
    }

    //アーム角度
    if(f==FlgRotate){
      led = 10;
      //Serial.println("mode=FlgRotate");
      Serial.println(posA);
      posA+=1;
      if(posA >= 360){
        posA = 0;
        i = 0;
      }else if(posA > 180){
        i = 180 - (posA - 180);
      }else{
        i = posA;
      }
      servoA.write(i);
      delay(20);
    }

    if(f==FlgDown){
      GetDistanceB();
      Serial.print("mode=FlgDown");
      delay(200);
      f=FlgCatch;
    }

    if(f==FlgCatch){
      led = 100;
      GetDistanceB();
      Serial.println("mode=FlgCatch");

      //誤差がおかしいときは何もしない
      if(abs(distanceB - recentDistanceB) > 100 && recentDistanceB != 0){
        Serial.print("abs(distanceB - recentDistanceB)");
        Serial.println(abs(distanceB - recentDistanceB));
        //return;
      }
      //地面に近づくとCatch
      downCnt+=1;
      Serial.print("downCnt=");
      Serial.println(downCnt);
      //if(distanceB <= limitMinPinB){
      if(downCnt >= 5){
        motor3.run(RELEASE);
        delay(1000);

        CloseArm();
        delay(2000);
        
        f = FlgUp;
        return;
      }

      //モーターを動かす アームダウン
      motor3.run(FORWARD);
      Serial.print("motor3 run");
      for (i=0; i<180; i+=1) {
        motor3.setSpeed(i);
        delay(30);
      }
      motor3.run(RELEASE);
      delay(10);
    
      recentDistanceB = distanceB;

    }

    //クレーンをUp
    if(f==FlgUp){
      led = 110;
      GetDistanceB();

      //誤差がおかしいときは何もしない
      if(abs(distanceB - recentDistanceB) > 100 && recentDistanceB != 0){
        Serial.print("abs(distanceB - recentDistanceB)");
        Serial.println(abs(distanceB - recentDistanceB));
        //return;
      }
      
      //地面はなれるまでUp
      upCnt+=1;
      Serial.print("upCnt=");
      Serial.println(upCnt);
      //if(distanceB >= limitMaxPinB){
      if(upCnt >= 3){
        motor3.run(RELEASE);
        delay(500);
        
        f = FlgLeft;
        return;
      }
      
      //モーターを動かす アームアップ
      motor3.run(BACKWARD);
      Serial.print("motor3 run");
      for (i=0; i<100; i+=1) {
        motor3.setSpeed(i);
        delay(12);
      }
      motor3.run(RELEASE);
      delay(10);
      recentDistanceB = distanceB;
    }
    
    //左移動
    if(f==FlgLeft){
      led = 110;
      GetDistanceA();
      Serial.println("mode=FlgLeft");
      Serial.print("distanceA=");
      Serial.println(distanceA);
      //誤差がおかしいときは何もしない
      if(abs(distanceA - recentDistanceA) > 100 && recentDistanceA != 0){
        Serial.print("abs(distanceA - recentDistanceA)");
        Serial.println(abs(distanceA - recentDistanceA));
        return;
      }
      //limitを下回るとStop
      if(distanceA <= limitMinPinA){
        motor4.run(RELEASE);
        delay(1000);

        // アームをオープン
        OpenArm();
        delay(1000);
               
        f = FlgStart;
        return;
      }

      //モーターを動かす
      motor4.run(BACKWARD);
      Serial.print("motor4 run");
      for (i=0; i<100; i+=5) {
        motor4.setSpeed(i);
        delay(100);
      }
      delay(10);
    
      recentDistanceA = distanceA;
    }
}

int toggle = 1;
int timerCnt = 1;
int totalCnt = 1;
int _led = 0;
//点滅の間隔は1/100
void LedBlink() {
  totalCnt+=1;

  //3000Cnt毎にステータスを進める
  if(DEBUG == 2 && totalCnt % 3000 == 0){
    f+=1;
  }
  
  if(timerCnt < 100){
    timerCnt += 1;
    return;
  }else{
    if(DEBUG >= 1){
      Serial.print("flag = ");
      Serial.println(f);
      GetDistanceA();
      GetDistanceB();
      Serial.print("distansA=");
      Serial.println(distanceA);
      Serial.print("distansB=");
      Serial.println(distanceB);
    }
  }
  timerCnt = 1;

  _led = led;
  digitalWrite(LedRed, HIGH);
  digitalWrite(LedGreen, HIGH);
  digitalWrite(LedBlue, HIGH);

  if (_led >= 100 && toggle == 1){
      digitalWrite(LedRed, LOW);
      _led-=100;
  }
  if (_led >= 10 && toggle == 1){
      digitalWrite(LedGreen, LOW);
      _led-=10;
  }
  if (_led >= 1 && toggle == 1){
      digitalWrite(LedBlue, LOW);
  }

  toggle = 1 - toggle;
}

void GetDistanceA(){
    // 横方向のスタートからの距離を計測
    digitalWrite(trigPinA, LOW);
    delayMicroseconds(5);
    digitalWrite(trigPinA, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPinA, LOW);
    durationA = pulseIn(echoPinA, HIGH);
    distanceA= durationA*0.034/2;
}

void GetDistanceB(){
    // 横方向のスタートからの距離を計測
    digitalWrite(trigPinB, LOW);
    delayMicroseconds(5);
    digitalWrite(trigPinB, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPinB, LOW);
    durationB = pulseIn(echoPinB, HIGH);
    distanceB= durationB*0.034/2;
}

void CloseArm(){
  for(posB = 180; posB>0; posB-=10)
  {
    servoB.write(posB);
    delay(100);
  }
}

void OpenArm(){
  for(posB = 0; posB<=180; posB+=10)
  {
    servoB.write(posB);
    delay(100);
  }
}

推しポイント

製品化のイメージ

  1. 子供向けの工作キット。​

  2. ソースをセットで販売するのでカスタマイズ自在

  3. 比較的安価なパーツで構成しているのでお手頃価格

最後に

プロトタイプを作るところまでで精一杯でした。

まだ動作は怪しいです。

 

https://youtu.be/2U3tfvzyruA

以上ですがよろしくお願いします。

Crane Swings

By Koichi Minamijima

Made with Slides.com

Crane Swings

LINE Things Mini Award 応募用

  • 1,067

More from Koichi Minamijima