极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 32953|回复: 21

[感恩贴] 两足机器人静态步行(Mega2560控制)

[复制链接]
发表于 2013-6-21 22:45:26 | 显示全部楼层 |阅读模式
机器人的制作期间极客工坊给了我很大帮助,现分享两足机器人相关资料以表感谢。
先上图:


机器人目前总共有10个舵机,所用的舵机力量不够大,就没有装手臂了,两只脚,能走路的话其他功能也不在话下,呵呵。

控制电路。使用Mega2560作为控制器,32路舵机驱动板驱动舵机,蓝牙与电脑通讯。

MPU6050模块,数据处理代码借鉴弘毅(版主?)的相关帖子,非常感谢!

第一次发帖,先看看效果,后面再贴代码。

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复

使用道具 举报

发表于 2013-6-21 22:51:57 | 显示全部楼层
很期待呢,上个行走视频吧
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-6-21 22:54:55 | 显示全部楼层
wing 发表于 2013-6-21 22:51
很期待呢,上个行走视频吧

刚注册,以前都是游客来访,第一次发帖,后面会贴视频。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-6-21 23:40:49 | 显示全部楼层
静态步行侧面视频:

优酷上传真心慢啊,==
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-6-22 00:26:16 | 显示全部楼层
本帖最后由 koenig 于 2013-6-22 00:31 编辑

机器人控制代码(开环控制版):
[pre lang="arduino" line="1" file="RobotControl_V1.0"]/******************************************************************************
Biped Robot Control V1.0
By: Niko LI
Time: 2013.5
*******************************************************************************/
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
String SendBag=String("");
String ReceiveBag=String("");
//char* Data[]={"","","","","","","","","","","","","","",""};

int16_t ax, ay, az;
int16_t gx, gy, gz;

int StepCount, StepNum;
String Speed="1000";//T100<=speed<=T9999,default:1000
boolean SensorOn;//On=true  off=false
int Sensitivity;//0-10
//-----
String A_Wolk[14];
String A_Walk[14];
//-----
//Mode:H--HomePositon
//     S--Setup,test
//     A--Automatical
//     M--ManualControl
//     E--Error
char Mode;

//Action: 1--Wolk forword
//        2--Banlance
//        0--Stand(HomePosition)
char Action;
String Message=String("");//<= 20Byte

//boolean Blinker(boolean Blink,unsigned long T,int &Count);
unsigned long TOn=0,TOff=0;
boolean BlinkFlag;                    
unsigned long BlinkTime;//ms
boolean BlinkerONOFF=false;
int BlinkCount=0;

//============================Setup function======================================
void setup() {
  Wire.begin();
  Serial.begin(9600);//programm,test
  Serial1.begin(9600);//servo control
  Serial2.begin(9600);//bluetooth
  // initialize device
  Serial.println("Initializing I2C devices...");
  accelgyro.initialize();
  // verify connection
  Serial.println("Testing device connections...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

//-----------------------------------------Servo Motor control instructions---------------------------------------
  A_Wolk[0]="#1P1000#2P850#3P1600#4P1350#5P1350#6P900#7P1250#8P1150#9P1500#10P1100";//stand,homeposition
  A_Walk[1]="#1P850#2P600#3P1000#4P950#5P1300#6P1000#7P1200#8P1250#9P1700#10P1100";//
  A_Walk[2]="#1P850#2P600#3P1000#4P950#5P1300#6P850#7P1500#8P1650#9P1800#10P1100";//
  A_Walk[3]=  "#1P860#2P600#3P1000#4P950#5P1300#6P800#7P1350#8P1300#9P1950#10P1100";//
  A_Walk[4]=  "#1P960#2P600#3P1000#4P950#5P1300#6P850#7P1000#8P1300#9P2000#10P1100";//
  A_Walk[5]= "#1P1060#2P600#3P1450#4P1400#5P1300#6P950#7P1550#8P1900#9P2150#10P1100";//
  A_Walk[6]= "#1P900#2P600#3P1450#4P1400#5P1300#6P1120#7P1550#8P1900#9P2250#10P1100";//
  A_Walk[7]= "#1P1060#2P600#3P550#4P600#5P1300#6P1120#7P1550#8P1900#9P2150#10P1100";//
  A_Walk[8]="#1P1100#2P750#3P1300#4P750#5P1300#6P1120#7P1550#8P1900#9P2150#10P1100";//
  A_Walk[9]= "#1P1160#2P1050#3P1400#4P750#5P1300#6P1050#7P1550#8P1900#9P2150#10P1100";//
  A_Walk[10]= "#1P1000#2P800#3P950#4P700#5P1300#6P850#7P1550#8P1700#9P1850#10P1100";//
  A_Walk[11]=  "#1P1000#2P600#3P900#4P800#5P1300#6P850#7P1550#8P1450#9P1600#10P1100";//
  A_Walk[12]=  "#1P850#2P600#3P900#4P800#5P1300#6P1150#7P1500#8P1450#9P1600#10P1100";//
  A_Walk[13]= "#1P850#2P600#3P1000#4P950#5P1300#6P850#7P1500#8P1850#9P2000#10P1100";//
//------------------------------------------------------------------------------------------------------------------------------
  Serial.println("Initializing Robot...");
  //initialize mode
  Serial1.println(A_Wolk[0]+Speed);//robot in homeposition,stand
  Serial.println(A_Wolk[0]+Speed);//test
  Mode='D';//default,start
  StepCount=0;
  Action='0';
  SensorOn=false;  
  Message=String("Start!");
  SendBag=String ("");
  ReceiveBag=String ("");
  BlinkFlag=true;
  BlinkTime=1000;
  Serial.println("Robot Start successful!");

}
//==============================loop function==============================
void loop() {
  //Main Function
  ReceiveDataBag();
  getData();
  Actions();
  Modes();
  getSensorData();
  ReceiveDataBag();
  getData();
  if(SensorOn!=false){
    SendDataBag();
  }//else{Serial.println();}
  delay(10);
}

//------------------------------------------------SendData------------------------------------------------------------
void SendDataBag(){
  SendBag=SendBag+'['+ax+','+ay+','+az+','+gx+','+gy+','+gz+','+StepCount+','+Mode+','+Action+','+Message+']';
  delay(20);
  Serial.println(SendBag); //test
  Serial2.println(SendBag); //via Bluetooth
  Message=String("");
  SendBag=String("");

}
//--------------------------------------------------get Sensor Data ---------------------------------------------
void getSensorData(){  // read raw accel/gyro measurements from device
  if (SensorOn && accelgyro.testConnection()){
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  }
  else{
    ax=0;
    ay=0;
    az=0;
    gx=0;
    gy=0;
    gz=0;
    Message="SensorStop!";
  }
  // these methods (and a few others) are also available
  //accelgyro.getAcceleration(&ax, &ay, &az);
  //accelgyro.getRotation(&gx, &gy, &gz);
}
//-----------------------------------------------------ReceiveDataBag----------------------------------------------------
void ReceiveDataBag(){
  //int readb=Serial.available();
// Serial.print(readb);Serial.println("<===");
  while (Serial.available())  //USB com
  {
    ReceiveBag += char(Serial.read());
    delay(2);
  }
  while (Serial2.available())  //Bluetooth com
  {
    ReceiveBag += char(Serial2.read());
    delay(2);
  }
  while(Serial.available())//clear input buffer of comport
  {      
    Serial.read();
  }


   if(ReceiveBag.length()>10 ){//for Serve test
  Serial.print(ReceiveBag);
  Serial.println("<-----");
    delay(100);
     }
}
//-------------------------------------------------get data from ReceiveBag----------------------------------------
void getData(){
  String stemp=String("");

  String D=String("");
  String temD=String("");
  stemp=ReceiveBag;
  stemp.trim();
  int flag=stemp.indexOf('{');//Returns :The index of { within the String, or -1 if not found.
  if (flag==-1){
    ReceiveBag=String ("");
    return;
  }

  for (int j=0;j<15;j++){

    for (int i=flag, k=0;i<=stemp.length();i++,k++){
      if (stemp[i+1]!=','&& stemp[i+1]!='}'){
        // Data[j][k] +=stemp[i+1];
        D +=stemp[i+1];
        // D+=stemp.charAt(i+1);
        //Serial.print(D);

      }
      else{
        flag=i+1;
        // D.toCharArray(Data[j],D.length());
        // Serial.println(j);
        switch(j){
        case 0:
          temD=D;
          StepNum=temD.toInt();
          Serial.print("StepNum= ");
          Serial.println(StepNum);
          break;
        case 1:
          Speed=D;
          Serial.print("Speed= T");
          Serial.println(Speed);
          break;
        case 2:
          Mode=D.charAt(0);
          Serial.print("Mode= ");
          Serial.println(Mode);
          break;
        case 3:
          Action=D.charAt(0);
          Serial.print("Action= ");
          Serial.println(Action);
          break;
        case 4:
          temD=D;
          BlinkTime=temD.toInt();
          Serial.print("Time between Actions= ");
          Serial.println(BlinkTime);
          break;
        case 5:
          break;
        }
        D=String("");
        break;
      }


    }


    // Serial.println(D);

    if (stemp[flag]=='}'){
      break;
    }
  }
  ReceiveBag=String ("");
}

//-----------------------------------------------------Actions control----------------------------------------------------
//Action: 1--Wolk forword
//        2--Banlance
//        0--Stand(HomePosition)
//        H--Hold position,==0,but not send data to servo
void Actions(){//robot's action control
  String tem2;
  tem2=Speed;

  boolean BlinkOut=Blinker(BlinkerONOFF,BlinkTime,BlinkCount);
  if(Action!='1'){

    BlinkerONOFF=false;
    BlinkCount=0;
    //  Action='0';
  }
  switch (Action){
  case '1':
    BlinkerONOFF=true;
    //BlinkTime=5000;

    //for(int i=0;i<=13;i++){
    if(BlinkCount==0 && BlinkFlag==true&& StepCount==0){
     
      Serial1.println(A_Wolk[0]+'T'+Speed);
      Serial.println(A_Wolk[0]+'T'+Speed);//test only   
      Serial.println(millis()); //
      Serial.println(BlinkCount);//
      
    }
    if(BlinkCount-1==0 && BlinkOut==true ){//
      Serial1.println(A_Walk[1]+'T'+Speed);
      Serial.println(A_Walk[1]+'T'+Speed);//test only  
      Serial.println(millis());  //
      Serial.println(BlinkCount);//
    }
    if( BlinkOut==true){
       BlinkCount--;
      Serial1.println(A_Walk[BlinkCount]+'T'+Speed);
      Serial.println(A_Walk[BlinkCount]+'T'+Speed);//test only  
      Serial.println(millis());  //
      Serial.println(BlinkCount);//
      BlinkCount++;
    }
    if(BlinkCount==14){

      BlinkCount=3;
      StepCount+=2;

      Serial.println("<-------<--------<");

    }
    //delay(tem2.toInt());//low speed will make a lang instruction cycle!!!!!!!!
    // }
    break;
  case '2':
    break;
  case '0': //Homeposition,test
    //if(){break;}
    if(BlinkCount!=0){

      break;
    }
    Serial.println(millis()); //
    Serial1.println(A_Wolk[0]+'T'+Speed);
    Serial.println(A_Wolk[0]+'T'+Speed);//only for test
    //delay(tem2.toInt());
    Action='H';
    break;
  case'H'://hold this position

    break;
  default:
    break;
  }
}
//-------------------------------------------------------------Mode----------------------------------------------------------
//Mode:H--HomePositon
//     S--Setup,test
//     A--Automatical
//     M--ManualControl
//     E--Error
void Modes(){//Sensor control
  switch (Mode){
  case 'S': //setting
    //StepCount=0;
    //Action='0';//home
    SensorOn=true;
    Message="SensorOn!";  
    // Message=String("Mode:S");
    break;
  case 'A': //Balance
    //Action='0';//home
    SensorOn=true;  
    Message="SensorOn!";
    //Message=String("Mode:A");
    break;
  case 'M':
    break;
  case 'D'://Default
    SensorOn=false;
    break;
  default:
    break;
  }
}
//-------------------------------------------interrupt function (timer/Blinker)-------------------------------------------
boolean Blinker(boolean Blink,unsigned long T,int &Count)
{
  unsigned long nowtime=millis();
  if(Blink){   
    if(BlinkFlag==true)            
    {
      if(nowtime>=TOn)            
      {
        TOn=nowtime;              
        TOff=nowtime+T;         
        BlinkFlag=false;                 
        Count++;
        if((Count-1)==0){
          Count--;
          return false;
        }
        return true;      
      }
      else return false;
    }   
    else{      
      if(nowtime>=TOff)
      {   
        TOff=nowtime;
        TOn=nowtime+T;
        BlinkFlag=true;
        Count++;
        return true;
      }
      else return false;
    }
  }
  else{   
    if((nowtime>=TOn && BlinkFlag==true) || (nowtime>=TOff &&BlinkFlag==false)){
      Count=0;
      TOn=0;
      TOff=0;
      BlinkFlag=true;
    }
    return false;
  }
}
[/code]
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-6-22 00:32:44 | 显示全部楼层
本帖最后由 koenig 于 2013-6-22 00:49 编辑

代码没有精心排版,有点毛躁,见谅。。
V1.0的控制程序只是开环控制并让机器人静态步行,电脑通过蓝牙发送指令可让其停止或步行,目前可在线调步行速度,可后续添加步数控制、步行距离控制等功能。
V2.0可实现闭环控制,即利用MPU6050数据计算机器人倾斜角度,然后控制脚步舵机旋转让机器人恢复直立姿态。目前控制程序仍在完善中。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-6-22 01:51:08 | 显示全部楼层
再来一小段正面视频:
回复 支持 反对

使用道具 举报

发表于 2013-6-22 08:44:57 | 显示全部楼层
很强大。。提一个小建议:
舵机运动有点儿僵硬(充满了急停疾走)。所以机器人肯定会有比较明显的晃动。。这样的话当你做第二版闭环控制的时候,用MPU6050可能会出现很多噪声。
可以尝试让运动温柔点儿。。。其实就是把动作插值成好多非线性的小动作。。例如sin,cos之类的。。。
忘了这个术语叫啥了……囧。。。
回复 支持 反对

使用道具 举报

发表于 2013-6-22 08:54:44 | 显示全部楼层
强大呀!以前我就想着人形机器人能加上陀螺仪保持稳定。楼主用了10个舵机?什么牌子的,不抖呀,看上去很不错!
回复 支持 反对

使用道具 举报

发表于 2013-6-22 10:07:44 | 显示全部楼层
恭喜,期待进一步改善。。。。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-6-22 12:26:57 | 显示全部楼层
friskit 发表于 2013-6-22 08:44
很强大。。提一个小建议:
舵机运动有点儿僵硬(充满了急停疾走)。所以机器人肯定会有比较明显的晃动。。 ...

谢谢指点,机器人现在一个步行循环(两步)有10条舵机指令,循环中可以添加更多指令让动作过度更平滑。术语好像就叫“动作插值”,偶然看到过相关资料,忘了。。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-6-22 12:31:17 | 显示全部楼层
heiketiguo 发表于 2013-6-22 08:54
强大呀!以前我就想着人形机器人能加上陀螺仪保持稳定。楼主用了10个舵机?什么牌子的,不抖呀,看上去很不 ...

MG995的舵机,不推荐,舵机在高负载的时候不能保持给定位置,所以就没有安装手臂了。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-6-22 12:33:11 | 显示全部楼层
firewise 发表于 2013-6-22 10:07
恭喜,期待进一步改善。。。。

估计第二版完成不了了,时间不够,要毕业了。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-6-23 03:10:57 | 显示全部楼层
闭环控制打算先从直立自平衡开始,下面是上位机端数据监控界面(Processing),程序是基于弘毅的传感器数据处理程序改进而来:




Aduino上传到PC的数据格式如下:


本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 反对

使用道具 举报

发表于 2013-8-22 22:20:00 | 显示全部楼层
静态步就是每一步都是不会摔的,稳定,可是快步了,希望能做出个动态步。顶起啊
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-29 12:56 , Processed in 0.048132 second(s), 23 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表