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

koenig 发表于 2013-6-21 22:45:26

机器人的制作期间极客工坊给了我很大帮助，现分享两足机器人相关资料以表感谢。
先上图：

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

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

MPU6050模块，数据处理代码借鉴弘毅（版主？）的相关帖子，非常感谢！

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

wing 发表于 2013-6-21 22:51:57

很期待呢，上个行走视频吧

koenig 发表于 2013-6-21 22:54:55

wing 发表于 2013-6-21 22:51 static/image/common/back.gif
很期待呢，上个行走视频吧

刚注册，以前都是游客来访，第一次发帖，后面会贴视频。

koenig 发表于 2013-6-21 23:40:49

静态步行侧面视频：
http://v.youku.com/v_show/id_XNTczODA5MjA0.html
优酷上传真心慢啊，==

koenig 发表于 2013-6-22 00:26:16

本帖最后由 koenig 于 2013-6-22 00:31 编辑

机器人控制代码（开环控制版）：
/******************************************************************************
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=trueoff=false
int Sensitivity;//0-10
//-----
String A_Wolk;
String A_Walk;
//-----
//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="#1P1000#2P850#3P1600#4P1350#5P1350#6P900#7P1250#8P1150#9P1500#10P1100";//stand,homeposition
A_Walk="#1P850#2P600#3P1000#4P950#5P1300#6P1000#7P1200#8P1250#9P1700#10P1100";//
A_Walk="#1P850#2P600#3P1000#4P950#5P1300#6P850#7P1500#8P1650#9P1800#10P1100";//
A_Walk="#1P860#2P600#3P1000#4P950#5P1300#6P800#7P1350#8P1300#9P1950#10P1100";//
A_Walk="#1P960#2P600#3P1000#4P950#5P1300#6P850#7P1000#8P1300#9P2000#10P1100";//
A_Walk= "#1P1060#2P600#3P1450#4P1400#5P1300#6P950#7P1550#8P1900#9P2150#10P1100";//
A_Walk= "#1P900#2P600#3P1450#4P1400#5P1300#6P1120#7P1550#8P1900#9P2250#10P1100";//
A_Walk= "#1P1060#2P600#3P550#4P600#5P1300#6P1120#7P1550#8P1900#9P2150#10P1100";//
A_Walk="#1P1100#2P750#3P1300#4P750#5P1300#6P1120#7P1550#8P1900#9P2150#10P1100";//
A_Walk= "#1P1160#2P1050#3P1400#4P750#5P1300#6P1050#7P1550#8P1900#9P2150#10P1100";//
A_Walk= "#1P1000#2P800#3P950#4P700#5P1300#6P850#7P1550#8P1700#9P1850#10P1100";//
A_Walk="#1P1000#2P600#3P900#4P800#5P1300#6P850#7P1550#8P1450#9P1600#10P1100";//
A_Walk="#1P850#2P600#3P900#4P800#5P1300#6P1150#7P1500#8P1450#9P1600#10P1100";//
A_Walk= "#1P850#2P600#3P1000#4P950#5P1300#6P850#7P1500#8P1850#9P2000#10P1100";//
//------------------------------------------------------------------------------------------------------------------------------
Serial.println("Initializing Robot...");
//initialize mode
Serial1.println(A_Wolk+Speed);//robot in homeposition,stand
Serial.println(A_Wolk+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!=','&& stemp!='}'){
   // Data +=stemp;
   D +=stemp;
   // D+=stemp.charAt(i+1);
   //Serial.print(D);

   }
   else{
   flag=i+1;
   // D.toCharArray(Data,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=='}'){
   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+'T'+Speed);
   Serial.println(A_Wolk+'T'+Speed);//test only
   Serial.println(millis()); //
   Serial.println(BlinkCount);//

}
if(BlinkCount-1==0 && BlinkOut==true ){//
   Serial1.println(A_Walk+'T'+Speed);
   Serial.println(A_Walk+'T'+Speed);//test only
   Serial.println(millis());//
   Serial.println(BlinkCount);//
}
if( BlinkOut==true){
   BlinkCount--;
   Serial1.println(A_Walk+'T'+Speed);
   Serial.println(A_Walk+'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+'T'+Speed);
Serial.println(A_Wolk+'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;
}
}

koenig 发表于 2013-6-22 00:32:44

本帖最后由 koenig 于 2013-6-22 00:49 编辑

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

koenig 发表于 2013-6-22 01:51:08

再来一小段正面视频：
http://v.youku.com/v_show/id_XNTczODUwMjI0.html

friskit 发表于 2013-6-22 08:44:57

很强大。。提一个小建议：
舵机运动有点儿僵硬（充满了急停疾走）。所以机器人肯定会有比较明显的晃动。。这样的话当你做第二版闭环控制的时候，用MPU6050可能会出现很多噪声。
可以尝试让运动温柔点儿。。。其实就是把动作插值成好多非线性的小动作。。例如sin，cos之类的。。。
忘了这个术语叫啥了……囧。。。

heiketiguo 发表于 2013-6-22 08:54:44

强大呀！以前我就想着人形机器人能加上陀螺仪保持稳定。楼主用了10个舵机？什么牌子的，不抖呀，看上去很不错！

firewise 发表于 2013-6-22 10:07:44

恭喜，期待进一步改善。。。。

koenig 发表于 2013-6-22 12:26:57

friskit 发表于 2013-6-22 08:44 static/image/common/back.gif
很强大。。提一个小建议：
舵机运动有点儿僵硬（充满了急停疾走）。所以机器人肯定会有比较明显的晃动。。 ...

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

koenig 发表于 2013-6-22 12:31:17

heiketiguo 发表于 2013-6-22 08:54 static/image/common/back.gif
强大呀！以前我就想着人形机器人能加上陀螺仪保持稳定。楼主用了10个舵机？什么牌子的，不抖呀，看上去很不 ...

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

koenig 发表于 2013-6-22 12:33:11

firewise 发表于 2013-6-22 10:07 static/image/common/back.gif
恭喜，期待进一步改善。。。。

估计第二版完成不了了，时间不够，要毕业了。

koenig 发表于 2013-6-23 03:10:57

闭环控制打算先从直立自平衡开始，下面是上位机端数据监控界面（Processing），程序是基于弘毅的传感器数据处理程序改进而来：

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

北色欧阳疯 发表于 2013-8-22 22:20:00

静态步就是每一步都是不会摔的，稳定，可是快步了，希望能做出个动态步。顶起啊

页: [1] 2

极客工坊's Archiver

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