自動でルービックキューブを揃えるロボットを作ってみた【LEGOマインドストームEV3×MATLAB】

自動でルービックキューブを揃えるロボットを作ってみた【LEGOマインドストームEV3×MATLAB】

ゲーム製作

ルービックキューブロボットを作りたい

こんにちは。ハリボーです。

先日大掃除をしていたら部屋からこんなものが出てきました。

ルービックキューブです。

誰でも一度は遊んだことがあるのではないでしょうか。

世の中にはルービックキューブを数十秒、あるいは数秒で揃えてしまう超人もいるそうですが、私はこの2×2×2の簡単なルービックキューブさえなかなか揃えることはできません。

どこか揃ったと思ったら別の面が揃わなくなるなど、難しいものです。

そんなルービックキューブを弄り回していると、ある疑問が浮かびました。

これは人間がやるよりも機械がやった方が圧倒的に早いのではないか。

私がゆっくり回していては数分かかりますが、機械の力を借りれば数秒であっという間の完成も夢ではないような気がします。

というわけでルービックキューブを自動で完成させるロボットを作成してみます。

ルービックキューブロボに使用するもの

  • 2×2×2のルービックキューブ
  • LEGOマインドストームEV3
  • MATLAB

以上3点です。

メガハウス ルービックキューブ 2×2
created by Rinker
メガハウス(MegaHouse)

MATLABとEV3の連携はこちらをご参照ください。

ルービックキューブの攻略理論

ルービックキューブには「攻略法」も存在します。

1面ずつ確実に順序良く進めていく方法です。

2×2のルービックキューブであれば、数ステップの攻略法を正しく進めることで確実に6面を揃えることができます。

攻略法をそのままプログラムに起こしてもルービックキューブは解けます。

しかし、攻略法にはデメリットもあります。

「手数が多くなる」ということです。

攻略法では確実に揃いますが、最短で揃うわけではないのです。

攻略法に従って手で回して揃えても時間がかかるのはこのためです。

せっかくMATLABという優秀な頭脳を用いるので、理論上の最短で揃えることにもこだわりを持ってみます。

具体的には線形代数と反復深化深さ優先探索というアルゴリズムを用います。

ややこしくなり過ぎない様にできるだけ簡単に書いていきます。

初期状態を24×1のベクトルへ置き換え

まずはキューブの状態をベクトルに置き換えます。

キューブは1面あたり4色×6面で24要素です。

したがって、

Is = [0;0;0;0;1;1;1;1;2;2;2;2;3;3;3;3;4;4;4;4;5;5;5;5];% Initial Position

上記のコードのように24行のベクトルで表現できます。

キューブの回転は行列の掛け算

キューブの座標図を以下に示しました。

2×2のキューブはX+、X-、Y+、Y-、Z+、Z-の6通りの回転パターンしか存在しません。

数式モデルで表す場合、初期状態のベクトルの値を入れ替える変換行列として置き換えることができます。

X＋の場合の例を示します。

%%%%%%%%%%%%%%%%% Define Rotational Matrix %%%%%%%%%%%%%%%%%%%%%%%
% X+
Xp = [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0;
0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;
0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1];

このような24×24の変換行列をX+からZ-まで計6個定義します。

行列の演算によってルービックキューブは解ける

ルービックキューブの初期状態のベクトルを\( Is \)、完成形を\( I \)とします。

X+回転の1手でキューブが揃う場合の数式を以下に示します。

このときX+回転の変換行列を\( Xp \)とします。

\( Xp Is = I\)

複数の回転が必要な場合はさらに変換行列を左から掛けることで表現できます。

初期状態のベクトルから完成形のベクトルまでを繋ぐ変換行列の組み合わせを見つけることがルービックキューブを揃える手順を見つけることになります。

反復深化深さ優先探索を用いたキューブの解法

変換行列の組み合わせの見つけ方はさまざまな手法があります。

今回は探索アルゴリズムのひとつである反復深化深さ優先探索を用いてみます。

深さ優先探索の制限を徐々に増やし、最終的に目標状態の深さになるまで反復するものです。

深さ優先探索のメモリ効率と幅優先探索の完全性を併せ持っています。

下記のようなコードで実装してみました。

%%%%%%%%%%%%%%%%%%%%% Cube Solver %%%%%%%%%%%%%%%%%%%%%
% depth-first search (iterative deeping)
path = zeros(15,1);
%x = Zp*Xn*Xn*Yp*Zp*Yp*Zp*Yp*Is;
finish=0;
limit=2;
while(finish==0)
    [y1,y2,y3,y4,y5,y6,y7,y8,y9,y10,y11,y12,y13,y14,y15,finish] = dfs(1,limit,xyz,x,Is,path,finish);
    if(limit<15)
        limit=limit+1;
    else
        break;
    end
end
solve=[y1;y2;y3;y4;y5;y6;y7;y8;y9;y10;y11;y12;y13;y14;y15];
xtest=x;
for i=1:15
    if(solve(i)==0)
        break;
    else
        xtest=xyz(:,:,solve(i))*xtest;
    end
end

if(xtest==Is)
    clearLCD(mylego)
    writeLCD(mylego,' Cube Solver',2,1)
    writeLCD(mylego,' Success',4,1)
else
    beep(mylego)
    clearLCD(mylego)
    writeLCD(mylego,' Cube Solver',2,1)
    writeLCD(mylego,' Failed',4,1)
end

このアルゴリズムによって変換行列の組み合わせを求め、キューブを揃える手順を導出します。

ここまでがルービックキューブの理論に関するまとめです。

プログラムの流れ

今回用いるプログラムの流れは次の通りです。

  1. ルービックキューブの初期状態を光センサで取得
  2. アルゴリズムを用いて変換行列の組み合わせ(=キューブを解く手順)を導出
  3. 手順に従いキューブを1ステップずつ回転させる

手順1と手順3の精度はハードウェアのクオリティがモノを言います。

LEGOブロックを用いたハードウェアの作成

LEGOマインドストームEV3の基本パーツだけで実装しました。

台座部分が回転することでキューブを回します。

キューブの色合いを正確に読み取るために光センサの距離は細かい調整が必要でした。

光センサによるキューブの初期状態測定プログラムはこんな感じです。

ちゃんと回転させないと6面読み取ることはできません。

%%%%%%%%%%%%%%%%%%%%%%%%% Read Cube Colors %%%%%%%%%%%%%%%%%%%%%%%%%%%
clearLCD(mylego)
writeLCD(mylego,' Reading cube',2,1)
writeLCD(mylego,' Processing...',4,1)

% 0 dimension
motor_move(cmotor,0,cmp,cinter)
[xx(4),xx(2),xx(1),xx(3)]=readonedimension(amotor,cmotor,mycolorsensor,amp,cmp);
motor_move(dmotor,dangle,dmp,dinter)
motor_move(dmotor,0,dmp,dinter)
% 2nd dimension
[xx(12),xx(10),xx(9),xx(11)]=readonedimension(amotor,cmotor,mycolorsensor,amp,cmp);
motor_move(dmotor,dangle,dmp,dinter)
motor_move(dmotor,0,dmp,dinter)
% 5th dimension
[xx(24),xx(22),xx(21),xx(23)]=readonedimension(amotor,cmotor,mycolorsensor,amp,cmp);
motor_move(dmotor,dangle,dmp,dinter)
motor_move(dmotor,0,dmp,dinter)
% 4th dimension
[xx(17),xx(19),xx(20),xx(18)]=readonedimension(amotor,cmotor,mycolorsensor,amp,cmp);
motor_move(cmotor,-90,cmp,cinter)
motor_move(dmotor,dangle,dmp,dinter)
motor_move(dmotor,0,dmp,dinter)
motor_move(cmotor,0,cmp,cinter)
% 1st dimension
[xx(5),xx(7),xx(8),xx(6)]=readonedimension(amotor,cmotor,mycolorsensor,amp,cmp);
motor_move(dmotor,dangle,dmp,dinter)
motor_move(dmotor,0,dmp,dinter)
motor_move(dmotor,dangle,dmp,dinter)
motor_move(dmotor,0,dmp,dinter)
% 3rd dimension
[xx(16),xx(14),xx(13),xx(15)]=readonedimension(amotor,cmotor,mycolorsensor,amp,cmp);
motor_move(cmotor,-180,cmp,cinter)
motor_move(dmotor,dangle,dmp,dinter)
motor_move(dmotor,0,dmp,dinter)
motor_move(cmotor,-90,cmp,cinter)
% make color vector
x_sort=sort(xx);
%section1
for i=1:24
    if(xx(i)<x_sort(5))
        x(i)=0;
    elseif(xx(i)<x_sort(9))
        x(i)=5;
    elseif(xx(i)<x_sort(13))
        x(i)=2;
    elseif(xx(i)<x_sort(17))
        x(i)=4;
    elseif(xx(i)<x_sort(21))
        x(i)=3;
    else
        x(i)=1;
    end
end

これでハードウェア、ソフトウェアの準備が整いました。

実際に動かしてみた

まずは1手だけ動かしてみた様子がこちらです。

初期状態を読み取るまでに時間がかかりますが、測定後は見事に揃えることができました。

ステップを増やしても問題なく揃えることができます。

完成したはずなのに回しているのは完成形のベクトルが絶対的なものだからです。

色配置を相対的にしておけば完成が少し早くできましたね。

光センサによる読み取り時間とLEGOのパワー・精度の制約により一瞬で6面完成とまではいけませんでしたが、ルービックキューブを確実に揃えるロボットを作ることができました。

今回書いたMATLABのmファイルはこちら（534行）

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Pocket Cube Solver        %
% (c) Harilab               %
% Last Update: 2021.01.25   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%mylego = legoev3
amotor = motor(mylego,'A');
cmotor = motor(mylego,'C');
dmotor = motor(mylego,'D');

resetRotation(amotor);
resetRotation(cmotor);
resetRotation(dmotor);

mycolorsensor = colorSensor(mylego);
x = zeros(24,1);   % to solve matrix
xx = zeros(24,1);  % row sensor value matrix
amp = 7;% a (sensor) motor power ratio
cmp = 7;% c (table) motor power ratio
cmprotate = 1;
dmp = 7;% d (arm) motor power ratio
ainter = 50;%  a (sensor) motor power adjustment
cinter = 50;% c (table) motor power adjustment
dinter = 50;% d (arm) motor power adjustment
dangle = 140;
Is = [0;0;0;0;1;1;1;1;2;2;2;2;3;3;3;3;4;4;4;4;5;5;5;5];% Initial Position

%%%%%%%%%%%%%%%%% Define Rotational Matrix %%%%%%%%%%%%%%%%%%%%%%%
% X+
Xp = [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0;
0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;
0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1];
% X-
Xn = [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1];
% Y+
Yp = [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0];
% Y-
Yn = [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;
0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1;
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0;
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0;
0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0];
% Z+
Zp = [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0;
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0];
% Z-
Zn = [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;
0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0];
% I
I = [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1];
% Marge Rotational Matrix
xyz(:,:,1)=Xp;
xyz(:,:,2)=Xn;
xyz(:,:,3)=Yp;
xyz(:,:,4)=Yn;
xyz(:,:,5)=Zp;
xyz(:,:,6)=Zn;

%%%%%%%%%%%%%%%%%%%%%%%%% Read Cube Colors %%%%%%%%%%%%%%%%%%%%%%%%%%%
clearLCD(mylego)
writeLCD(mylego,' Reading cube',2,1)
writeLCD(mylego,' Processing...',4,1)

% 0 dimension
motor_move(cmotor,0,cmp,cinter)
[xx(4),xx(2),xx(1),xx(3)]=readonedimension(amotor,cmotor,mycolorsensor,amp,cmp);
motor_move(dmotor,dangle,dmp,dinter)
motor_move(dmotor,0,dmp,dinter)
% 2nd dimension
[xx(12),xx(10),xx(9),xx(11)]=readonedimension(amotor,cmotor,mycolorsensor,amp,cmp);
motor_move(dmotor,dangle,dmp,dinter)
motor_move(dmotor,0,dmp,dinter)
% 5th dimension
[xx(24),xx(22),xx(21),xx(23)]=readonedimension(amotor,cmotor,mycolorsensor,amp,cmp);
motor_move(dmotor,dangle,dmp,dinter)
motor_move(dmotor,0,dmp,dinter)
% 4th dimension
[xx(17),xx(19),xx(20),xx(18)]=readonedimension(amotor,cmotor,mycolorsensor,amp,cmp);
motor_move(cmotor,-90,cmp,cinter)
motor_move(dmotor,dangle,dmp,dinter)
motor_move(dmotor,0,dmp,dinter)
motor_move(cmotor,0,cmp,cinter)
% 1st dimension
[xx(5),xx(7),xx(8),xx(6)]=readonedimension(amotor,cmotor,mycolorsensor,amp,cmp);
motor_move(dmotor,dangle,dmp,dinter)
motor_move(dmotor,0,dmp,dinter)
motor_move(dmotor,dangle,dmp,dinter)
motor_move(dmotor,0,dmp,dinter)
% 3rd dimension
[xx(16),xx(14),xx(13),xx(15)]=readonedimension(amotor,cmotor,mycolorsensor,amp,cmp);
motor_move(cmotor,-180,cmp,cinter)
motor_move(dmotor,dangle,dmp,dinter)
motor_move(dmotor,0,dmp,dinter)
motor_move(cmotor,-90,cmp,cinter)
% make color vector
x_sort=sort(xx);
%section1
for i=1:24
    if(xx(i)<x_sort(5))
        x(i)=0;
    elseif(xx(i)<x_sort(9))
        x(i)=5;
    elseif(xx(i)<x_sort(13))
        x(i)=2;
    elseif(xx(i)<x_sort(17))
        x(i)=4;
    elseif(xx(i)<x_sort(21))
        x(i)=3;
    else
        x(i)=1;
    end
end

% check cube vector 
if(cubecheck(x)==1)
    clearLCD(mylego)
    writeLCD(mylego,' Making Vector',2,1)
    writeLCD(mylego,' Success',4,1)
else
    beep(mylego)
    clearLCD(mylego)
    writeLCD(mylego,' Making Vector',2,1)
    writeLCD(mylego,' Failed',4,1)
end

%%%%%%%%%%%%%%%%%%%%% Cube Solver %%%%%%%%%%%%%%%%%%%%%
% depth-first search (iterative deeping)
path = zeros(15,1);
%x = Zp*Xn*Xn*Yp*Zp*Yp*Zp*Yp*Is;
finish=0;
limit=2;
while(finish==0)
    [y1,y2,y3,y4,y5,y6,y7,y8,y9,y10,y11,y12,y13,y14,y15,finish] = dfs(1,limit,xyz,x,Is,path,finish);
    if(limit<15)
        limit=limit+1;
    else
        break;
    end
end
solve=[y1;y2;y3;y4;y5;y6;y7;y8;y9;y10;y11;y12;y13;y14;y15];
xtest=x;
for i=1:15
    if(solve(i)==0)
        break;
    else
        xtest=xyz(:,:,solve(i))*xtest;
    end
end

if(xtest==Is)
    clearLCD(mylego)
    writeLCD(mylego,' Cube Solver',2,1)
    writeLCD(mylego,' Success',4,1)
else
    beep(mylego)
    clearLCD(mylego)
    writeLCD(mylego,' Cube Solver',2,1)
    writeLCD(mylego,' Failed',4,1)
end

%%%%%%%%%%%%%%%%%%%% Oparate Cube %%%%%%%%%%%%%%%%%%%%%%%%%%%
cp = -90;
for i=1:15
    if(solve(i)==0)
        break;
    else
        cp = motor_rotate(solve(i),cp,cmotor,dmotor,cmp,cmprotate,dmp,cinter,dinter,dangle);
    end
end

clearLCD(mylego)
writeLCD(mylego,' Cube',2,1)
writeLCD(mylego,' Finish!!',4,1)

playTone(mylego,450.0,0.25,0.5)
pause(0.25)
playTone(mylego,500.0,0.25,0.5)
pause(0.25)
playTone(mylego,550.0,0.25,0.5)
pause(0.25)
%%%%%%%%%%%%%%%%%%%%%% Functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%

% read colors of one dimension (4 states)
function [xx1,xx2,xx3,xx4] = readonedimension(amotor,cmotor,mycolorsensor,amp,cmp)
    motor_move(amotor,0,amp,5)
    pause(0.25)
    xx1 = readLightIntensity(mycolorsensor,'reflected');
    motor_move(cmotor,90,cmp,20)
    pause(0.25)
    xx2 = readLightIntensity(mycolorsensor,'reflected');
    motor_move(cmotor,180,cmp,20)
    pause(0.25)
    xx3 = readLightIntensity(mycolorsensor,'reflected');
    motor_move(cmotor,270,cmp,20)
    pause(0.25)
    xx4 = readLightIntensity(mycolorsensor,'reflected');
    motor_move(cmotor,0,cmp,20)
    motor_move(amotor,90,amp,5)
end

%motor_move(motor_port, target degree (absolute angle), power ratio (min->increase))
function motor_move(motornum,target,power_ratio,intercept)
rotation = readRotation(motornum);
error = target - rotation;
%olderror = 0;
integral = 0;
    while(abs(error)>2)
        motornum.Speed = error/power_ratio+0.001*integral;
        %+sign(error)*intercept;
        %error/power_ratio+0.01*integral+0.001*differential
        %olderror = error;
        rotation = readRotation(motornum);
        error = target - rotation;
        integral = integral + error;
        %differential = (error-olderror);
        start(motornum)
    end
    stop(motornum)
    pause(0.5)
end

%check cube vector 
function result = cubecheck(x)
zero=0;
one=0;
two=0;
three=0;
four=0;
five=0;
for i = 1:24
    if(x(i)==0)
        zero=zero+1;
    elseif(x(i)==1)
        one=one+1;
    elseif(x(i)==2)
        two=two+1;
    elseif(x(i)==3)
        three=three+1;
    elseif(x(i)==4)
        four=four+1;
    elseif(x(i)==5)
        five=five+1;
    end
end
if(4==zero&&zero==one&&one==two&&two==three&&three==four&&four==five)
    result=1;
else
    result=0;
end
end

%(depth-first search(count,loop limit,rotational matrix,state,goal,path))
function [y1,y2,y3,y4,y5,y6,y7,y8,y9,y10,y11,y12,y13,y14,y15,finish] = dfs(n,limit,xyz,x,Is,path,finish)
if(finish==1)
else
    for j=1:6
        if(finish==0)
        z = xyz(:,:,j)*x;
        if(z==Is)
            path(n)=j;
            finish=1;
            for k = n+1:9
                path(k)=0;
            end
            break;
        elseif(limit==n)
            path(n)=j;
        else
            path(n)=j;
            [w1,w2,w3,w4,w5,w6,w7,w8,w9,w10,w11,w12,w13,w14,w15,finish]=dfs(n+1,limit,xyz,z,Is,path,finish);
            path(1)=w1;
            path(2)=w2;
            path(3)=w3;
            path(4)=w4;
            path(5)=w5;
            path(6)=w6;
            path(7)=w7;
            path(8)=w8;
            path(9)=w9;
            path(10)=w10;
            path(11)=w11;
            path(12)=w12;
            path(13)=w13;
            path(14)=w14;
            path(15)=w15;
        end
        end
    end
end
    y1 = path(1);
    y2 = path(2);
    y3 = path(3);
    y4 = path(4);
    y5 = path(5);
    y6 = path(6);
    y7 = path(7);
    y8 = path(8);
    y9 = path(9);
    y10 = path(10);
    y11 = path(11);
    y12 = path(12);
    y13 = path(13);
    y14 = path(14);
    y15 = path(15);
end

% Operation Cube
function c_position = motor_rotate(x,cp,cmotor,dmotor,cmp,cmprotate,dmp,cinter,dinter,dangle)
    if(x==1)%Xp
        motor_move(cmotor,cp+90,cmp,cinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(cmotor,cp,cmprotate,cinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(cmotor,cp-90,cmp,cinter)
        c_position = cp-90;
    elseif(x==2)%Xn
        motor_move(cmotor,cp+90,cmp,cinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(cmotor,cp+180,cmprotate,cinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(cmotor,cp+90,cmp,cinter)
        c_position=cp+90;
    elseif(x==3)%Yp
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(cmotor,cp-90,cmprotate,cinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        c_position=cp-90;
    elseif(x==4)%Yn
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(cmotor,cp+90,cmprotate,cinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        c_position=cp+90;
    elseif(x==5)%Zp
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(cmotor,cp-90,cmprotate,cinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        c_position=cp-90;
    elseif(x==6)%Zn
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(cmotor,cp+90,cmprotate,cinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        motor_move(dmotor,dangle,dmp,dinter)
        motor_move(dmotor,0,dmp,dinter)
        c_position=cp+90;
    else
    end
end
メガハウス ルービックキューブ 2×2
created by Rinker
メガハウス(MegaHouse)
(Visited 1,504 times, 1 visits today)

関連記事

Processingで横スクロールアクションゲームをつくる【その３】 EV3で2自由度マニピュレータを構築し逆運動学で動かしてみた【EV3+MATLAB】 Processingで横スクロールアクションゲームをつくる【その１】 『Android Studio』でアプリを作ってみよう【環境構築～Hello world】

コメントを残す

メールアドレスが公開されることはありません。 ※ が付いている欄は必須項目です