本帖最后由 xietu 于 2012-8-18 22:21 编辑
我在做6脚机器人,先编了其中一条腿代码;出现了莫名其妙的问题,各位大侠帮帮忙。
另外,感谢Ansifa 老师提供的“Arduino学习笔记A6(补充) - 在串口读取多个字符串,并且转换为数字数组 ”教学贴
从串口输入数据,有时候得出正确结果,有时候就出错了;如下:
Lb Lc W is 85,125,150
Lb=85
Lc=125
W=150
H Fa S is 59,-15,-17
H=59
Fa=-15
S=-17
3.14
Di=53.56
Da=47.31
Ac=162.85
AI=52.93
AII=140.76
AIII=160.01
H Fa S is 59,-15,-17
H=-59//出错了
Fa=-15
S=-17
3.14
//出错了
Di=0.00
Da=0.00
Ac=0.00
AI=0.00
AII=0.00
AIII=0.00
H Fa S is 59,-15,-17
H=-59//出错了
Fa=-15
S=-17
3.14
//出错了
Di=0.00
Da=0.00
Ac=0.00
AI=0.00
AII=0.00
AIII=0.00
H Fa S is 59,-15,-17
H=-59//出错了
Fa=-15
S=-17
3.14
//出错了
Di=0.00
Da=0.00
Ac=0.00
AI=0.00
AII=0.00
AIII=0.00
H Fa S is 59,15,17
H=-59//出错了
Fa=15
S=17
3.14
//出错了
Di=0.00
Da=0.00
Ac=0.00
AI=0.00
AII=0.00
AIII=0.00
H Fa S is 59,-15,17
H=59
Fa=-15
S=17
3.14
Di=53.56
Da=71.46
Ac=139.09
AI=76.69
AII=139.77
AIII=148.45
H Fa S is 59,-15,17
H=59
Fa=-15
S=17
3.14
Di=53.56
Da=71.46
Ac=139.09
AI=76.69
AII=139.77
AIII=148.45
H Fa S is 59,-15,17
H=59
Fa=-15
S=17
3.14
Di=53.56
Da=71.46
Ac=139.09
AI=76.69
AII=139.77
AIII=148.45
H Fa S is 59,-15,17
H=59
Fa=-15
S=17
3.14
Di=53.56
Da=71.46
Ac=139.09
AI=76.69
AII=139.77
AIII=148.45
H Fa S is 59,-15,-17
H=59
Fa=-15
S=-17
3.14
Di=53.56
Da=47.31
Ac=162.85
AI=52.93
AII=140.76
AIII=160.01
H Fa S is 59,-15,-17
H=-59//出错了
Fa=-15
S=-17
3.14
//出错了
Di=0.00
Da=0.00
Ac=0.00
AI=0.00
AII=0.00
AIII=0.00
H Fa S is 59,-15,17
H=-59//出错了
Fa=-15
S=17
3.14
//出错了
Di=0.00
Da=0.00
Ac=0.00
AI=0.00
AII=0.00
AIII=0.00
H Fa S is 59,15,17
H=59
Fa=15
S=17
3.14
Di=53.56
Da=79.89
Ac=75.88
AI=139.90
AII=138.58
AIII=143.96
H Fa S is 59,15,-17
H=59
Fa=15
S=-17
3.14
Di=53.56
Da=45.90
Ac=76.54
AI=139.24
AII=140.67
AIII=160.62
H Fa S is 59,15,-17
H=-59//出错了
Fa=15
S=-17
3.14
//出错了
Di=0.00
Da=0.00
Ac=0.00
AI=0.00
AII=0.00
AIII=0.00
H Fa S is 59,15,-17
H=-59//出错了
Fa=15
S=-17
3.14
//出错了
Di=0.00
Da=0.00
Ac=0.00
AI=0.00
AII=0.00
AIII=0.00
H Fa S is 59,15,17
H=-59//出错了
Fa=15
S=17
3.14
//出错了
Di=0.00
Da=0.00
Ac=0.00
AI=0.00
AII=0.00
AIII=0.00
H Fa S is 59,15,17
H=59
Fa=15
S=17
3.14
Di=53.56
Da=79.89
Ac=75.88
AI=139.90
AII=138.58
AIII=143.96
下面是代码,大侠们帮帮我把
#include <math.h>
String comdata = "";//定义一个comdata字符串变量,赋初值为空值
int mark1=0,mark2=0,mark3=0;
int numdata[4]={0};//numdata是分拆之后的数字数组
int H,Fa,S;//H机体底面距地高度;Fa前进方向;S行进距离
int Lb,Lc,W;//Lb上肢长度;Lc下肢长度;W整机重量
int Di;//最小支撑三角形时,落脚点的最近距离
int Da,AI,AII,AIII,Ac;//Da行走时,落脚点的距离;AI上肢b的水平舵机成角;AII上肢b的竖向舵机成角;AIII下肢c的竖向舵机成角
void setup()
{
Serial.begin(9600);
}
void loop()
{
int j = 0;
if (mark2==0)
{
while (Serial.available() > 0)
{
comdata += char(Serial.read());
delay(2);
mark1 = 1;
}
if(mark1 == 1)
{
Serial.print("Lb Lc W is ");
Serial.println(comdata);
for(int i = 0; i < comdata.length()-1 ; i++)
{
if(comdata == ',')
{
++j;
}
else
{
numdata[j] = numdata[j] * 10 + (comdata - '0');
}
}
comdata = String("");
Lb=numdata[0],Lc=numdata[1],W=numdata[2];
Serial.print("Lb=");
Serial.println(Lb);
Serial.print("Lc=");
Serial.println(Lc);
Serial.print("W=");
Serial.println(W);
numdata[0]=0,numdata[1]=0,numdata[2]=0;
mark1=0,mark2=1,j=0;
}//if(mark1==1)结尾
}//if (mark2==0)结尾
else
{
while (Serial.available() > 0)//不断循环检测串口缓存,一个个读入字符串,
{
comdata += char(Serial.read());//读入之后将字符串,串接到comdata上面。
delay(2);//延时一会,让串口缓存准备好下一个数字,不延时会导致数据丢失,
mark1 = 1;//标记串口读过数据,如果没有数据的话,直接不执行这个while了。
}
if(mark1 == 1)//如果接收到数据则执行comdata分析操作,否则什么都不做。
{
Serial.print("H Fa S is ");
Serial.println(comdata);
/*******************下面是重点*******************/
for(int i = 0; i < comdata.length()-1 ; i++)//以串口读取字符串长度循环,
{
if(comdata == ',') //逐个分析comdata字符串的文字,如果碰到文字是分隔符(这里选择逗号分割)则将结果数组位置下移一位,即比如11,22,33,55开始的11记到numdata[0];碰到逗号就j等于1了,再转换就转换到numdata[1];再碰到逗号就记到numdata[2];以此类推,直到字符串结束
{
++j;
mark3=0;
}//if(comdata == ',')结尾
else
{
if(comdata == '-')
{
mark3=1;
}
else if(mark3==1)
{
numdata[j] =abs(numdata[j]);
numdata[j] =numdata[j] * 10 + (comdata - '0');
numdata[j] =0-numdata[j];
}
else
{
numdata[j] = numdata[j] * 10 + (comdata - '0');
}
}//if(comdata == ',')的else结尾
}//for(int i = 0; i < comdata.length()-1 ; i++)的结尾
comdata = String(""); //comdata的字符串已经全部转换到numdata了,清空comdata以便下一次使用,如果不请空的话,本次结果极有可能干扰下一次。
H=numdata [0],Fa=numdata[1],S=numdata[2];
Serial.print("H=");
Serial.println(H);
Serial.print("Fa=");
Serial.println(Fa);
Serial.print("S=");
Serial.println(S);
numdata[0]=0,numdata[1]=0,numdata[2]=0;
mark1 = 0; //输出之后必须将读到数据的mark置0,不置0下次循环就不能使用了。
double Di=sqrt(sq(Lb)-sq(H-Lc));
double Da=sqrt(sq(((75+sqrt(0.75*sq(Di)+7.5*Di-56.25)-((0.5*Di+32.5)*tan((-Fa)*PI/180))+S)*cos((90-Fa)*PI/180))-((0.5*Di+32.5)/sin((90-Fa)*PI/180))+40)+sq(((75+sqrt(0.75*sq(Di)+7.5*Di-56.25)-((0.5*Di+32.5)*tan((-Fa)*PI/180))+S)*sin((90-Fa)*PI/180))-75));
double Ac=acos(((75+sqrt(0.75*sq(Di)+7.5*Di-56.25)-(0.5*Di+32.5)*tan(-Fa*(PI/180))+S)*cos((90-Fa)*(PI/180))-(0.5*Di+32.5)/sin((90-Fa)*(PI/180))+40)/Da)*(180/PI);
double AI=360-144.22-Ac;
double AII=acos((sq(Lb)+(sq(H)+sq(Da))-sq(Lc))/(2*Lb*sqrt(sq(H)+sq(Da))))*(180/PI)+acos(H/sqrt(sq(H)+sq(Da)))*(180/PI);
double AIII=180-acos((sq(Lb)+sq(Lc)-(sq(H)+sq(Da)))/(2*Lb*Lc))*(180/PI)+16.29;
Serial.println(PI);
Serial.print("Di=");
Serial.println(Di);
Serial.print("Da=");
Serial.println(Da);
Serial.print("Ac=");
Serial.println(Ac);
Serial.print("AI=");
Serial.println(AI);
Serial.print("AII=");
Serial.println(AII);
Serial.print("AIII=");
Serial.println(AIII);
}//if (mark1==1)结尾
}//if (mark2==0)的else结尾
}
|