この記事はなに？
私は現在2x2x2ルービックキューブを解くロボットを開発中です。これはそのロボットのプログラムの解説記事集です。
かつてこちらの記事に代表される記事集を書きましたが、この時からソフトウェアが大幅にアップデートされたので新しいプログラムについて紹介しようと思います。
該当するコードはこちらで公開しています。
関連する記事集
「ルービックキューブを解くロボットを作ろう！」
ルービックキューブロボットのソフトウェアをアップデートした
今回は機械操作(Arduino)編として、soltvvo3_arduino.inoを紹介します。
定数と変数
グローバルに置いた定数と変数です。
const int magnet_threshold = 50;
const long turn_steps = 400;
const int step_dir[2] = {11, 9};
const int step_pul[2] = {12, 10};
const int sensor[2] = {14, 15};
const int grab_deg[2] = {74, 74};
const int release_deg[2] = {96, 96};
const int offset = 3;
char buf[30];
int idx = 0;
long data[3];
定数はポート番号だったりサーボモーターの角度だったりです。変数はスペース区切りで値が与えられるのを分割する時に使います。
サーボライブラリ周り
サーボモーターを使う関係でサーボライブラリをインクルードしてservo0, servo1を定義しておきます。
#include <Servo.h>
Servo servo0;
Servo servo1;
セットアップ
セットアップです。
void setup() {
Serial.begin(115200);
for (int i = 0; i < 2; i++) {
pinMode(step_dir[i], OUTPUT);
pinMode(step_pul[i], OUTPUT);
pinMode(sensor[i], INPUT);
}
servo0.attach(7);
servo1.attach(8);
servo0.write(release_deg[0] + 5);
servo1.write(release_deg[1] + 5);
delay(70);
servo0.write(release_deg[0]);
servo1.write(release_deg[1]);
}
ピンのインプット、アウトプットの定義と、サーボモーターを動かしてアームを外側に移動させます。
アームを回転させる
ステッピングモーターを回す関数です。台形駆動を実装しましたが、その関係でタイマーなどを使わずdelayを使う、少しカッコ悪い実装です。
void move_motor(long num, long deg, long spd) {
bool hl = true;
if (deg < 0) hl = false;
digitalWrite(step_dir[num], hl);
long steps = abs(deg) * turn_steps / 360;
long avg_time = 1000000 * 60 / turn_steps / spd;
long max_time = 1500;
long slope = 50;
bool motor_hl = false;
long accel = min(steps / 2, max(0, (max_time - avg_time) / slope));
int num1 = (num + 1) % 2;
// 加速
for (int i = 0; i < accel; i++) {
motor_hl = !motor_hl;
digitalWrite(step_pul[num], motor_hl);
delayMicroseconds(max_time - slope * i);
}
// 平常運転
for (int i = 0; i < steps * 2 - accel * 2; i++) {
motor_hl = !motor_hl;
digitalWrite(step_pul[num], motor_hl);
delayMicroseconds(avg_time);
}
// 減速
for (int i = 0; i < accel; i++) {
motor_hl = !motor_hl;
digitalWrite(step_pul[num], motor_hl);
delayMicroseconds(max_time - slope * accel + accel * (i + 1));
}
}
基本的には以下の流れで処理しています。
- 角度から回すべきステップ数を計算
- 回転数からパルスの幅を計算
- 加速、減速に使うステップ数を計算
アームのキャリブレーション
アームのステッピングモーターを使っている部分は適宜ちゃんとした角度になるようにキャリブレーションします。このとき、アーム側につけた磁石とステッピングモーター側につけたホールセンサ(磁気センサ)を使います。
void motor_adjust(long num, long spd) {
int max_step = 150;
int delay_time = 800;
bool motor_hl = false;
digitalWrite(step_dir[num], LOW);
while (analogRead(sensor[num]) <= magnet_threshold) {
motor_hl = !motor_hl;
digitalWrite(step_pul[num], motor_hl);
delayMicroseconds(delay_time);
}
while (analogRead(sensor[num]) > magnet_threshold) {
motor_hl = !motor_hl;
digitalWrite(step_pul[num], motor_hl);
delayMicroseconds(delay_time);
}
}
1つ目のwhile文がトリッキーですが、すでにアームが正しい位置にあると判定される場合でも、少しアームの角度がずれていることがあるので一回アームをずらしています。
2つ目のwhile文で磁石がホールセンサの前に来るまでステッピングモーターを回転させます。
パズルを離す
アームを外側に移動させ、パズルを離す関数です。
void release_arm(int num) {
if (num == 0)servo0.write(release_deg[num] + offset);
else servo1.write(release_deg[num] + offset);
}
あまり綺麗な書き方ではありません。綺麗な書き方をご存知の方はぜひ教えてください。offsetについては速くモーターを動かすためのオーバーシュートを実装したのですが、オーバーシュートしたまま目標値に戻さなくても良いことがわかりoffsetを足したままになっています。
パズルを掴む
アームを中心に移動させ、パズルを掴む関数です。
void grab_arm(int num) {
if (num == 0)servo0.write(grab_deg[num] - offset);
else servo1.write(grab_deg[num] - offset);
delay(70);
if (num == 0)servo0.write(grab_deg[num]);
else servo1.write(grab_deg[num]);
}
ここではoffsetで作ったオーバーシュートが役に立ちます。これによってより速くパズルを掴むことを期待しています(実際どれくらい効果があるのかはわかりません)
メインループ
メインループはコマンドを受け取って該当の関数を呼び出します。
void loop() {
while (1) {
if (Serial.available()) {
buf[idx] = Serial.read();
if (buf[idx] == '\n') {
buf[idx] = '\0';
data[0] = atoi(strtok(buf, " "));
data[1] = atoi(strtok(NULL, " "));
data[2] = atoi(strtok(NULL, " "));
if (data[1] == 1000) grab_arm(data[0]);
else if (data[1] == 2000) release_arm(data[0]);
else if (data[1] == 0) motor_adjust(data[0], data[2]);
else move_motor(data[0], data[1], data[2]);
idx = 0;
}
else {
idx++;
}
}
}
}
コマンドの種類によって呼び出す関数が違います。
まとめ
今回は実際にアクチュエータを動かすArduinoのプログラムを紹介しました。私の言語に対する知識不足によるところなのか、一部綺麗でない書き方をしていると思います。ぜひコメントで良い書き方を教えてください。
次回はPythonのメイン処理を解説します。