魔方机器人

qptimus

{:soso_e113:}
不用多介绍吧，很多小伙伴应该见过。
好吧言归正传，现在开始diy。
先来张硬件连线图：

一共用了8个舵机，还是看图吧

再详细点

这个没什么新意，而且，本人手工拙劣，这是这个机器人里最烂的一部分。小弟在此发帖还有一个目的就是请求机械大神帮忙弄个亚克力拼接的结构，或者3D打印的。因为我是搞软件的，所以，这方面呢，还要看机械大神的。
今天先上总体设计和Arduino上的部分代码，其实还没有完全完成，所以还要和大家多多讨论


整个系统的结构是这样的，

我初步估计，仅凭arduino的性能要计算出魔方的解法还是有很大压力的，另外我没有串口摄像头，所以只能用usb摄像头，所以干脆就放到pc上吧。这个算法，不是我写的，我只是改了改输入输出接口，来源戳这里 http://www.jaapsch.net/puzzles/cube3.htm
这个算法很优秀，平均步骤只有16步，我用PC计算时间没有超过1秒的，这部分以后再说，先弄arduino。
上arduino上的 代码

[pre lang="arduino" line="1" file="robot"]#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","0N2W1","S0S2W1H1H3W0R0R2W0P1N3W1","3N1W1","H0H2W0R1R3W0S1S3W1P2N0W1","0N2W1W1"};
const String ReadyToStartCommand = "S0S2W1H0H1H2H3W1";

const String ReadyCommand = "";
const String StayCommand = "S0S1S2S3R0R1R2R3W1";
const String Test1 = "0W1S0W1N0W1";
const String Test2 = "1W1S1W1N1W1";
const String Test3 = "2W1S2W1N2W1";
const String Test4 = "3W1S3W1N3W1";
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("2W1R2W0S2W1H2W0");break;}
        case '3':
        {BaseMove("N2W1R2W0S2W1H2W0");break;}
        case '2':
        {BaseMove("2W1R2W0N2W1W1H2W0S2W1");break;}
      }
      break;
    }
    case 'B':
    {
      switch(a)
      {
        case '1':
        {BaseMove("0W1R0W0S0W1H0W0");break;}
        case '3':
        {BaseMove("N0W1R0W0S0W1H0W0");break;}
        case '2':
        {BaseMove("0W1R0W0N0W1W1H0W0S0W1");break;}
      }
      break;
    }
    case 'R':
    {
      
      switch(a)
      {
        case '1':
        {BaseMove("1W1R1W0S1W1H1W0");break;}
        case '3':
        {BaseMove("N1W1R1W0S1W1H1W0");break;}
        case '2':
        {BaseMove("1W1R1W0N1W1W1H1W0S1W1");break;}
      }
      break;
    }
    case 'L':
    {
      switch(a)
      {
        case '1':
        {BaseMove("3W1R3W0S3W1H3W0");break;}
        case '3':
        {BaseMove("N3W1R3W0S3W1H3W0");break;}
        case '2':
        {BaseMove("3W1R3W0N3W1W1H3W0S3W1");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]);
  }
}[/code]


代码没加注释，因为比较简单。看起来很厉害的样子，其实我一说大家就明白了。是不是看到很多字符串，整个东西就是靠这些字符串工作起来的，S1表示第一个爪子回到中点，P1表示第一个爪子顺时针旋转，N1则是逆时针。H1表示第一个爪子夹住，R1表示第一个爪子松开，234当然是一样的啦。W是什么,是wait，咱们平时玩舵机的时候不是都得delay（）么，一个意思，只是这里换成了其他的表示。为什么要这样表示？这里有8个舵机，要是一个个自己去控制那不是找虐么，CNC上不是也用G代码控制动作么，这是一个意思。方便，可控性强，所以，有了这些字符串我们就能控制这4个爪子做还原魔方需要的所有的复杂动作了。

======================
先写这么多，另外啊，本人手工实在拙劣啊，自己手工切的4mm亚克力，有人体会过么，没台钻孔都钻不好啊，所以这个爪子工作起来实在是…………而且，由于爪子做得烂，会导致魔方卡在里面，我已经为此牺牲了一个舵机了，所以再次恳请机械大神帮帮忙，我自己也正在画图，准备用激光切个先看看，有没有那个大神能提供激光切割啊，价格好说。不胜感激

Super169

很厲害!  只是三百多行的程式, 就可以驅動機械臂解決魔方.
未有時間細看, 不明白是如何讀取及分析當前魔方的狀況, 今晚回家慢慢研究.

i7456

激光切割亚克力，可以在淘宝上找。
你花好cad格式的图给他们，他们直接按图给你切割出来。

qptimus

Super169 发表于 2014-5-30 16:34
很厲害!  只是三百多行的程式, 就可以驅動機械臂解決魔方.
未有時間細看, 不明白是如何讀取及分析當前魔方 ...

这个代码只是指挥爪子运动的，识别和计算在pc机上

qptimus

i7456 发表于 2014-5-30 16:50
激光切割亚克力，可以在淘宝上找。
你花好cad格式的图给他们，他们直接按图给你切割出来。

关键是不会cad啊…………

heiketiguo

不错挺赞！头像像罗永浩！

qptimus

heiketiguo 发表于 2014-5-30 18:48
不错挺赞！头像像罗永浩！

不是罗永浩

ttzaio

PC上，用什么写的？

野鬼｛ABO｝

楼主提供手稿，本人愿意无偿帮你画成CAD

wing

夹器很精致{:soso_e179:}

bearway

这个有意思，赞一下

学慧放弃

好厉害.有研究价值!!!

qptimus

ttzaio 发表于 2014-5-30 23:21
PC上，用什么写的？

VC++，用了OpenCV

qptimus

野鬼｛ABO｝ 发表于 2014-5-30 23:48
楼主提供手稿，本人愿意无偿帮你画成CAD

多谢多谢，还在设计改良

qptimus

wing 发表于 2014-5-31 12:08
夹器很精致

最不好的就是这4个爪了，多转几次魔方就卡里面了