Fork
5
Fav
1
View
3495
  • Play

Fullscreen

Smart Phone

Fork tree

  • Readme
  • JavaScript 527 lines
  • HTML 19 lines
  • CSS 10 lines
konashi.jsでBluetooth経由でピッコロボのコントローラーを作りました。

ピッコロボは、Arduino互換ボードを搭載し、4つのサーボを持った、ヴイストン株式会社製のロボット学習向けキットです。
http://www.vstone.co.jp/robotshop/index.php?main_page=product_info&products_id=4409

konashiは、Bluetoothを搭載した、ユカイ工学株式会社製のiOS向けフィジカルコンピューティング用の小さなデバイスです。
http://konashi.ux-xu.com/
今回は小型化拡張ボードと、マクニカのKoshianにkonashi2.0ファームウェアを利用しました。
http://yukaishop.base.ec/items/559791
http://www.m-pression.com/ja/solutions/boards/koshian

また、SBDBT5Vを用いたkonashi互換環境SBBLEでも動作を確認しています。
http://runningele.web.fc2.com/
http://sbble.micutil.com/

iOS用のkonashi.jsアプリをAppStoreからダウンロードしてこのプログラムを利用してください。

また、UIなどにenchant.jsならびにui.enchant.jsのサブセットを用いています。


【デバイス接続画面】
[Connect Device]ボタン:konashiと接続します。画面下から近くのkonashiデバイスの一覧が出るので接続するkonashiを選択してください。
[debug]ボタン:PCでのデバグ時に強制的にコントロール画面に遷移させるボタンです。

【コントロール画面】
[Disconnect Device]ボタン:konashiとの接続を切り、デバイス接続画面に戻ります。
[Home]ボタン:ホームポジションに戻ります。(首は戻らない)
[Adjust]ボタン:サーボのセンター位置を微調整する画面へ
十字キー:前進、後進、右回転、左回転
右側の赤いノブ:首の向きを変更
[debug]ボタン:StatusがWAIT状態で固まってしまった時にOKになるまでタップしてください。


ピッコロボ側のプログラムは以下になります。Arduino IDEで書き込んでください。

#include <Servo.h>
#include <SoftwareSerial.h>

//====================================================================================================================
//Parameter
//====================================================================================================================
#define SERV_NUM 4 //number of servo for array
#define FRAME 20 //interval time from current step to next step: 20msec

int current_angle[SERV_NUM];
int target_angle[SERV_NUM+1];
float rotating_angle[SERV_NUM]; //rotating angle on each frame: calcurated by (target_angle - current_angle)/number of steps
int servo_trim[SERV_NUM]; //trim to adjust each servo motors's angle to center

Servo servo[SERV_NUM];
#define CENTER 4
#define RIGHT 5
#define LEFT 6
#define NECK 7
#define DELAY 50

// for konashi / SBDBT with SBBLE
#define btrxPin A0
#define bttxPin A1
SoftwareSerial btSerial = SoftwareSerial(btrxPin, bttxPin);

byte lineEnding = 0x0A;
//byte positionCmd[SERV_NUM+1] = {0, 0, 0, 0, 0};
//int bytesRead;

//====================================================================================================================
//Servo Method
//====================================================================================================================
void initServo() {

//attach pins to each servo
servo[0].attach(CENTER);
servo[1].attach(RIGHT);
servo[2].attach(LEFT);
servo[3].attach(NECK);

//set trim
int tmp_trim[SERV_NUM] = {8, 10, -9, 0}; //set trim to each servo
for (int i = 0; i < SERV_NUM; i++) {
servo_trim[i] = tmp_trim[i];
}

//rotate all servos to center position
setCenterToServo();
}

void setCenterToServo() {
for (int i = 0; i < SERV_NUM; i++) {
servo[i].write(90 + servo_trim[i]);
current_angle[i] = 0;
target_angle[i] = 0;
}
}

//move to next position
void moveToNextPosition() {

Serial.println("move");

//check limit
for (int i = 0; i < SERV_NUM; i++) {
if (target_angle[i] > 90) {
target_angle[i] = 90;
} else if (target_angle[i] < -90) {
target_angle[i] = -90;
}
}

int numberOfStep = target_angle[SERV_NUM] / FRAME; //total number of steps to move to next position

for (int i = 0; i < SERV_NUM; i++){
rotating_angle[i] = ((float)target_angle[i] - (float)current_angle[i]) / (float)numberOfStep;
}

int next_timing = millis() + FRAME;
int current_time;
float tmp_angle[SERV_NUM];

for (int i = 0; i < SERV_NUM; i++){
tmp_angle[i] = (float)current_angle[i];
}

while (numberOfStep) {
current_time = millis();
if (current_time > next_timing) {
for (int i = 0; i < SERV_NUM; i++) {
tmp_angle[i] += rotating_angle[i];

if (rotating_angle[i] < 0) {
if (current_angle[i] > target_angle[i]) {
current_angle[i] = (int)tmp_angle[i];
}
} else if (rotating_angle[i] > 0){
if (current_angle[i] < target_angle[i]) {
current_angle[i] = (int)tmp_angle[i];
} else {
current_angle[i] = target_angle[i];
}
}
servo[i].write(current_angle[i] + servo_trim[i] + 90);
}
next_timing = next_timing + FRAME;
numberOfStep--;
}
}

//adjust current_angle
for (int i = 0; i < SERV_NUM; i++) {
if ((rotating_angle[i] < 0) && (current_angle[i] > target_angle[i])) {
current_angle[i] = target_angle[i];
servo[i].write(current_angle[i] + servo_trim[i] + 90);
} else if ((rotating_angle[i] > 0) && (current_angle[i] < target_angle[i])) {
current_angle[i] = target_angle[i];
servo[i].write(current_angle[i] + servo_trim[i] + 90);
} else if ((rotating_angle[i] < 0) && (current_angle[i] < target_angle[i])) {
current_angle[i] = target_angle[i];
servo[i].write(current_angle[i] + servo_trim[i] + 90);
} else if ((rotating_angle[i] > 0) && (current_angle[i] > target_angle[i])) {
current_angle[i] = target_angle[i];
servo[i].write(current_angle[i] + servo_trim[i] + 90);
}
}
}

//call moveToNextPosition() method continuously
void playMotion(int motion[][SERV_NUM+1], int numberOfMotion) {

for (int i = 0; i < numberOfMotion; i++) {
for (int j = 0; j < SERV_NUM + 1; j++) {
target_angle[j] = motion[i][j];
}
moveToNextPosition();
}

}

//====================================================================================================================
//Main
//====================================================================================================================
void setup(){

initServo();
Serial.begin(9600);
Serial.println("Init Servo.");
// for SoftwareSerial
pinMode(btrxPin, INPUT);
pinMode(bttxPin, OUTPUT);
btSerial.begin(9600);
btSerial.listen();
}

void loop(){

// konashiからのシリアル処理
if (btSerial.available() >= SERV_NUM + 4) {
byte recieved_command[SERV_NUM + 4] = {0, 0, 0, 0, 0, 0 ,0 ,0};
if (btSerial.available() > SERV_NUM + 4) {
// データが大量に来てしまった場合はシリアルバッファを破棄
btSerial.flush();
} else {
Serial.print("Serial Buffer: ");
Serial.println(btSerial.available());
// 6バイト(サーボ数+2)のデータを取得
int bytes_read = btSerial.readBytesUntil(lineEnding, recieved_command, SERV_NUM + 4);
Serial.print(bytes_read);
Serial.println(" Data Recieved.");
if (bytes_read == SERV_NUM + 4) {
// サーボ数の符号付きcharのサーボ角情報と、符号のない2byteの変化時間情報
// サーボの角度情報(符号付きcharに変換する)
if (recieved_command[0] == 0xff) {
int check_sum = 0;
for (int i = 0; i < SERV_NUM + 2; i++) {
check_sum += recieved_command[i + 1];
}
check_sum %= 256;
if (check_sum == recieved_command[SERV_NUM + 3]) {
Serial.println("Data Processing.");
for (int i = 0; i < SERV_NUM; i++) {
target_angle[i] = int(char(recieved_command[i + 1]));
//Serial.println(int(char(recieved_command[i + 1])));
}
// 変化時間のデータを取得(符号なしの2byteで処理)
target_angle[SERV_NUM] = int(recieved_command[SERV_NUM + 1] * 256 + recieved_command[SERV_NUM + 2]);
for (int i = 0; i < SERV_NUM + 1; i++) {
Serial.println(target_angle[i]);
}
moveToNextPosition();
}
}
btSerial.print('0');
}
}
Serial.print("Serial Buffer: ");
Serial.println(btSerial.available());
}
delay(100);
}

  • ピッコロボのBTコントローラーを作る
  • ピッコロボのBTコントローラーを作る

play

Complete!

Description What kind of game?

Control Device

jsdo.it websocket controller

Mouse

keyboard

smartphone

Fullscreen

hine.gdw

Author

  • Twitter
  • Facebook

Default Panel

Size

  • Width: px
  • Height: px

code

QR Code

Discussion

Questions on this code?

Tags

Favorite by

Jun.ToyodaJun.Toyoda:

すごいです

Forked

sort by