|
|
为了达到完全自主还原魔方的目的，至少需要完成4个方面的工作：识别魔方状态；计算出魔方的还原步骤；将还原步骤发送到下位机指导其运行；下位机转动魔方将其还原。
具体实现起来的细节更加复杂一些。在接收到开始信号后机器首先分别展示魔方的6 个面，同时PC利用开源计算机视觉库OpenCV通过USB摄像头捕捉到魔方6个面的图像，再使用数字图像处理技术识别各个方块的颜色，以此建立魔方表面颜色的数字模型。将颜色模型转换为更适合运算的状态模型之后，参考手解三阶魔方CFOP还原法计算出魔方的还原步骤并转换成标准表示方法。然后PC机通过UART串口将结果发送到下位机。下位机则借用了数控设备的思想，先将结果翻译为基本动作指令组，再驱动4个机械手按照基本动作依次旋转魔方，逐步将魔方复原。
不用多介绍吧，很多小伙伴应该见过。先来简单的部分，arduino控制的下位机。
好吧言归正传，现在开始diy。
先来张硬件连线图：
一共用了8个舵机，还是看图吧
再详细点
这个没什么新意，而且，本人手工拙劣，这是这个机器人里最烂的一部分。
今天先上总体设计和Arduino上的部分代码，其实还没有完全完成，所以还要和大家多多讨论
整个系统的结构是这样的，
[size=0.83em]下载附件 [size=0.83em](196.46 KB)
我初步估计，仅凭arduino的性能要计算出魔方的解法还是有很大压力的，另外我没有串口摄像头，所以只能用usb摄像头，所以干脆就放到pc上吧。这个算法，不是我写的，我只是改了改输入输出接口，来源戳这里http://www.jaapsch.net/puzzles/cube3.htm
这个算法很优秀，平均步骤只有16步，我用PC计算时间没有超过1秒的，这部分以后再说，先弄arduino。
上arduino上的 代码
[mw_shl_code=bash,true]#include <Servo.h>
#define LED 13
#define BUTTON 12
const int WRISTP[4] = {169,173,173,171};
const int WRISTS[4] = {86,92,90,89};
const int WRISTN[4] = {5,5,5,5};
const int HANDH[4] = {60,50,47,60};
const int HANDR[4] = {120,120,120,120};
const int WAIT_TIME[4] = {100,450,800,1200};
static int State = 0;
Servo wrist[4];
Servo hand[4];
String command;
String answer;
static int ScaningFace = 0;
const String ScanCommand[6] = {"H0H2W0S1S3W1","P0N2W1","S0S2W1H1H3W0R0R2W0P1N3W1","P3N1W1","H0H2W0R1R3W0S1S3W1P2N0W1","P0N2W1W1"};
const String ReadyToStartCommand = "S0S2W1H0H1H2H3W1";
const String ReadyCommand = "";
const String StayCommand = "S0S1S2S3R0R1R2R3W1";
const String Test1 = "P0W1S0W1N0W1";
const String Test2 = "P1W1S1W1N1W1";
const String Test3 = "P2W1S2W1N2W1";
const String Test4 = "P3W1S3W1N3W1";
void setup()
{
Serial.begin(9600);
pinMode(LED,OUTPUT);
pinMode(BUTTON,INPUT);
digitalWrite(LED,0);
for(int i=0;i<4;i++)
{
wrist.attach(i+4);
hand.attach(i+8);
}
command = "";
answer = "";
Stay();
}
void loop()
{
if(digitalRead(BUTTON) == LOW && State == 0)
{
Stay();
}
else if(digitalRead(BUTTON) == LOW && State == 1)
{
digitalWrite(LED,LOW);
Ready();
}
else if(digitalRead(BUTTON) == LOW && State == 2)
{
digitalWrite(LED,HIGH);
Serial.println('s');
delay(2500);
Scan();
}
GetCom();
delay(50);
}
void GetCom()
{
while(Serial.available())
{
int inChar = Serial.read();
if(inChar != '\r' && inChar != '\n')
{
command += (char)inChar;
}
else if (inChar == '\r'||inChar=='\n')
{
Serial.println('>' + command);
ExeCom();
}
}
}
void ExeCom(String com)
{
command = com;
ExeCom();
}
void ExeCom()
{
if(command == "ready")
{
Ready();
}
else if(command == "scan")
{
Scan();
}
else if(command == "stay")
{
Stay();
}
else if(command == "t1")
{
BaseMove(Test1);
}
else if(command == "t2")
{
BaseMove(Test2);
}
else if(command == "t3")
{
BaseMove(Test3);
}
else if(command == "t4")
{
BaseMove(Test4);
}
else if(command[0] == '>')
{
DoMove();
}
else// if(command == "test")
{
Solve();
}
// else
// Serial.println("wrong command!");
command = "";
}
void BaseMove(String com)
{
for(int i=0;i<com.length()-2;i+=2)
{
switch(com)
{
case 'S':
{
wrist[(int)(com[i+1]-'0')].write(WRISTS[(int)(com[i+1]-'0')]);
break;
}
case 'P':
{
wrist[(int)(com[i+1]-'0')].write(WRISTP[(int)(com[i+1]-'0')]);
break;
}
case 'N':
{
wrist[(int)(com[i+1]-'0')].write(WRISTN[(int)(com[i+1]-'0')]);
break;
}
case 'H':
{
hand[(int)(com[i+1]-'0')].write(HANDH[(int)(com[i+1]-'0')]);
break;
}
case 'R':
{
hand[(int)(com[i+1]-'0')].write(HANDR[(int)(com[i+1]-'0')]);
break;
}
case 'W':
{
delay(WAIT_TIME[(int)(com[i+1]-'0')]);
break;
}
}
}
}
void Stay()
{
State = 1;
ScaningFace=0;
BaseMove(StayCommand);
}
void Ready()
{
State = 2;
ScaningFace=0;
BaseMove("R0R1R2R3W0N0P2S1S3W2");
delay(800);
wrist[1].write(WRISTP[1]-(WRISTP[1]-WRISTS[1])/3);
wrist[3].write(WRISTN[3]-(WRISTN[3]-WRISTS[3])/3);
delay(WAIT_TIME_WRIST);
}
void Test()
{
BaseMove(command);
}
void Scan()
{
State = 0;
for(;ScaningFace<6;ScaningFace++)
{
BaseMove(ScanCommand[ScaningFace]);
delay(WAIT_TIME[2]);
}
}
void Hold(int handID)
{
hand[handID].write(HANDH[handID]);
}
void Release(int handID)
{
hand[handID].write(HANDR[handID]);
}
void Move(char face,char a)
{
switch(face)
{
case 'U':
{
switch(a)
{
case '1':
{BaseMove("R1R3W0P1N3W1H1H3W0R0R2W0");
BaseMove("S1S3W1H0W0P0W1R0W0S1S3S0W1H0H2W0");
BaseMove("R1R3W0P3N1W1H1H3W0S1S3W1H0H2W0");break;}
case '3':
{BaseMove("R1R3W0P1N3W1H1H3W0R0R2W0");
BaseMove("S1S3W1H0W0N0W1R0W0S1S3S0W1H0H2W0");
BaseMove("R1R3W0P3N1W1H1H3W0S1S3W1H0H2W0");break;}
case '2':
{BaseMove("R1R3W0P1N3W1H1H3W0R0R2W0");
BaseMove("S1S3W1N0W1H0W0P0W1R0W0S1S3S0W1H0H2W0");
BaseMove("R1R3W0P3N1W1H1H3W0S1S3W1H0H2W0");break;}
}
break;
}
case 'D':
{
switch(a)
{
case '1':
{BaseMove("R1R3W0P1N3W1H1H3W0R0R2W0");
BaseMove("S1S3W1H0W0P2W1R0W0S1S3S2W1H0H2W0");
BaseMove("R1R3W0P3N1W1H1H3W0S1S3W1H0H2W0");break;}
case '3':
{BaseMove("R1R3W0P1N3W1H1H3W0R0R2W0");
BaseMove("S1S3W1H0W0N2W1R0W0S1S3S2W1H0H2W0");
BaseMove("R1R3W0P3N1W1H1H3W0S1S3W1H0H2W0");break;}
case '2':
{BaseMove("R1R3W0P1N3W1H1H3W0R0R2W0");
BaseMove("S1S3W1N2W1H0W0P2W1R0W0S1S3S2W1H0H2W0");
BaseMove("R1R3W0P3N1W1H1H3W0S1S3W1H0H2W0");break;}
}
break;
}
case 'F':
{
switch(a)
{
case '1':
{BaseMove("P2W1R2W0S2W1H2W0");break;}
case '3':
{BaseMove("N2W1R2W0S2W1H2W0");break;}
case '2':
{BaseMove("P2W1R2W0N2W1W1H2W0S2W1");break;}
}
break;
}
case 'B':
{
switch(a)
{
case '1':
{BaseMove("P0W1R0W0S0W1H0W0");break;}
case '3':
{BaseMove("N0W1R0W0S0W1H0W0");break;}
case '2':
{BaseMove("P0W1R0W0N0W1W1H0W0S0W1");break;}
}
break;
}
case 'R':
{
switch(a)
{
case '1':
{BaseMove("P1W1R1W0S1W1H1W0");break;}
case '3':
{BaseMove("N1W1R1W0S1W1H1W0");break;}
case '2':
{BaseMove("P1W1R1W0N1W1W1H1W0S1W1");break;}
}
break;
}
case 'L':
{
switch(a)
{
case '1':
{BaseMove("P3W1R3W0S3W1H3W0");break;}
case '3':
{BaseMove("N3W1R3W0S3W1H3W0");break;}
case '2':
{BaseMove("P3W1R3W0N3W1W1H3W0S3W1");break;}
}
break;
}
}
}
void DoMove()
{
String temp = "";
temp = command.substring(1);
BaseMove(temp);
}
void Solve()
{
BaseMove(ReadyToStartCommand);
answer = command;
for(int i=0;i<answer.length()-1;i+=2)
{
Move(answer,answer[i+1]);
}
}
[/mw_shl_code]
代码没加注释，因为比较简单。看起来很厉害的样子，其实我一说大家就明白了。是不是看到很多字符串，整个东西就是靠这些字符串工作起来的，S1表示第一个爪子回到中点，P1表示第一个爪子顺时针旋转，N1则是逆时针。H1表示第一个爪子夹住，R1表示第一个爪子松开，234当然是一样的啦。W是什么,是wait，咱们平时玩舵机的时候不是都得delay（）么，一个意思，只是这里换成了其他的表示。为什么要这样表示？这里有8个舵机，要是一个个自己去控制那不是找虐么，CNC上不是也用G代码控制动作么，这是一个意思。方便，可控性强，所以，有了这些字符串我们就能控制这4个爪子做还原魔方需要的所有的复杂动作了。
|
|
发表于 2016-1-20 23:18
楼主
