本帖最后由 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] |