我想用自定义函数返回两个数据，两个数据用数组返回，这样写对吗？
函数体float *avedistance(float Ldistance,float Rdistance)//取10平均值函数{
float L;
float R;
float SL=0;
float SR=0;
int i;
float *AD=new float;
for(i=0;i<=9;i++)
{
L=Ldistance;
R=Rdistance;
}
i=0;
while(i<=9)
{
SL+=L;
SR+=R;
i++;
}
AD=SL/10;
AD=SR/10;
return AD;} 调用函数ldistance=avedistance;
rdistance=avedistance; 控制机械臂的全部程序在这里#include <Servo.h>
const int Ltrigpin=3;
const int Lechopin=2;
const int Rtrigpin=5;
const int Rechopin=4;
int i;
int catchopen=30;
int catchclose=85;
float Distance1;
float Distance2;
float between=4.5;
float Angle;
float x;
float x0;
float y;
float y0;
float Avedistance;
float ldistance;
float la;
float rdistance;
float ra;
float baseangle=20;
float baseangle1;
float baseangle2;
Servo servoarm1;
Servo servoarm2;
Servo servokich;
Servo servobase;
float l;
float r;
float Ldistance () //左超声波测距函数
{
float Ldistance;
digitalWrite(Ltrigpin,LOW);
delayMicroseconds(2);
digitalWrite(Ltrigpin,HIGH);
delayMicroseconds(10);
digitalWrite(Ltrigpin,LOW);
Ldistance=pulseIn(Lechopin,HIGH)/58.00;
return Ldistance;
delay(50);}
float Rdistance () //右超声波测距函数
{
float Rdistance;
digitalWrite(Rtrigpin,LOW);
delayMicroseconds(2);
digitalWrite(Rtrigpin,HIGH);
delayMicroseconds(10);
digitalWrite(Rtrigpin,LOW);
Rdistance=pulseIn(Rechopin,HIGH)/58.00;
return Rdistance;
delay(50);}
float *avedistance(float Ldistance,float Rdistance)//取10平均值函数
{
float L;
float R;
float SL=0;
float SR=0;
int i;
float *AD=new float;
for(i=0;i<=9;i++)
{
L=Ldistance;
R=Rdistance;
}
i=0;
while(i<=9)
{
SL+=L;
SR+=R;
i++;
}
AD=SL/10;
AD=SR/10;
return AD;}
float distance(float l,float r,float b) //长度函数
{
float d;
d=sqrt((l*l/2)+(r*r/2)-(b*b/4));
return d;}
float angle(float b,float l,float d) //角度函数
{
float angle;
angle=90-acos((l*l)+(d*d)-(b*b/4)/b*d);
return angle;}
float maindistance(float d,float b,float l) //轴心长度函数
{
float distance;
distance=sqrt(d*d+100-20*d*cos(90+acos((l*l)+(d*d)-(b*b/4)/b*d)));
return distance;}
float mainangle(float distance,float d) //轴心角度函数
{
float mainangle;
mainangle=acos((distance*distance+100-d*d)/20*distance);
return mainangle;}
float zuobiaozhuanhuan(float x,float x0,float dx,float y,float y0)//到下一位置的xy对应坐标
{
float dy;
float k;
if(x==x0)
{
dy=y;
}
k=(y-y0)/(x-x0);
dy=(k*dx)+y0;
return dy;}
float arm1bending(float a,float b,float x,float y)//臂1水平角度函数
{
float arm1angle1;
float arm1angle2;
float arm1angle;
float c;
c=x*x+y*y;
float d;
float e;
d=pow(a,2)+c-pow(b,2);
e=2*a*sqrt(c);
arm1angle1=(acos(d/e))*180/3.14159;
arm1angle2=atan(y/x)*180/3.14159;
arm1angle=arm1angle1+arm1angle2;
return arm1angle;}
float arm2bending(float arm1angle,float a,float b,float y)//臂2水平角度函数
{
float arm2angle;
float f;
f=(sin(arm1angle*3.14159/180))*a-y;
arm2angle=(asin(f/b))*180/3.14159;
return arm2angle;}
float springangle(float a) //舵机角度校正
{
float b;
b=0.61111 * a + 32;
return b;}
float movment(float x,float y,float x0,float y0) //机械臂移动
{
float arm1=296;
float arm2=320;
float arm1angle;
float actarm1angle;
float arm2angle;
float dx;
float dy;
if(x>x0)
{
for(dx=x0;dx<=x;dx++)
{
dy=zuobiaozhuanhuan(x,x0,dx,y,y0);
arm1angle=arm1bending(arm1,arm2,dx,dy);
actarm1angle=180-springangle(arm1angle);
arm2angle=arm2bending(arm1angle,arm1,arm2,dy);
servoarm1.write(actarm1angle);
servoarm2.write(arm2angle);
delay(20);
}
}
else
{ if(x<x0)
{
for(dx=x0;dx>=x;dx--)
{
dy=zuobiaozhuanhuan(x,x0,dx,y,y0);
arm1angle=arm1bending(arm1,arm2,dx,dy);
actarm1angle=180-springangle(arm1angle);
arm2angle=arm2bending(arm1angle,arm1,arm2,dy);
servoarm1.write(actarm1angle);
servoarm2.write(arm2angle);
delay(20);
}
}
else
{
if(y>y0)
{ for(dy=y0;dy<=y;dy++)
{
dx=x;
arm1angle=arm1bending(arm1,arm2,dx,dy);
actarm1angle=180-springangle(arm1angle);
arm2angle=arm2bending(arm1angle,arm1,arm2,dy);
servoarm1.write(actarm1angle);
servoarm2.write(arm2angle);
delay(20);
}
}
else
{
for(dy=y0;dy<=y;dy--)
{
dx=x;
arm1angle=arm1bending(arm1,arm2,dx,dy);
actarm1angle=180-springangle(arm1angle);
arm2angle=arm2bending(arm1angle,arm1,arm2,dy);
servoarm1.write(actarm1angle);
servoarm2.write(arm2angle);
delay(20);
}
}
}
}
x0=x;
y0=y;
}
void setup()
{
Serial.begin(9600);
pinMode(Ltrigpin,OUTPUT);
pinMode(Lechopin,INPUT);
pinMode(Rtrigpin,OUTPUT);
pinMode(Rechopin,INPUT);
servokich.attach(6);
servoarm1.attach(9);
servoarm2.attach(10);
servobase.attach(11);}
void loop()
{
do{
ldistance=Ldistance(); //第一阶段，左超声波传感器传回数据前底座转动
baseangle++;
servobase.write(baseangle);
delay(100);
}
while (ldistance<=50);
do{ rdistance=Rdistance(); //第二阶段，右超声波传感器传回数据前底座微调
baseangle+=0.5;
servobase.write(baseangle);
delay(100);
}
while (rdistance<=50);
baseangle-=5; //避免右超声波传感器传回空值
servobase.write(baseangle);
delay(300);
baseangle+=6;
servobase.write(baseangle);
baseangle1=baseangle;
delay(300);
ldistance=avedistance; //底板对准目标程序
rdistance=avedistance;
Distance1=distance(ldistance,rdistance,between);
Angle=angle(between,ldistance,Distance1);
Distance2=maindistance(Distance1,between,ldistance);
baseangle2+=mainangle(Distance2,Distance1);
for(baseangle=baseangle1;baseangle<=baseangle2;baseangle++)
{
baseangle+=0.5;
servobase.write(baseangle);
delay(50);
};
servokich.write(catchopen); //加持张开
movment(maindistance,50,200,100); //移动向目标位置
servokich.write(catchclose); //加持闭合
movment(200,100,maindistance,50); //机械臂收回
servobase.write(20); //机械臂对准放置位置
movment(200,50,200,100); //机械臂张开
servokich.write(catchopen); //加持放开
movment(200,100,200,50); //机械臂收回
baseangle=baseangle2;
servobase.write(baseangle); //旋转至前一目标角度
if (baseangle==160) //当大于160度时机械臂返回起始角度
{
baseangle=20;
servobase.write(baseangle);
}
} 程式太長了, 沒時間看, 只看了第一節, 有空再慢慢看.
以下的一部份, 好像什麼也沒計算, 把同一個數加 10 次再 除10?
float L;
float R;
float SL=0;
float SR=0;
int i;
float *AD=new float;
for(i=0;i<=9;i++)
{
L=Ldistance;
R=Rdistance;
}
i=0;
while(i<=9)
{
SL+=L;
SR+=R;
i++;
}
AD=SL/10;
AD=SR/10;
L - L 都是 Ldistance, SL 就是 10 個 Ldistance 加起來, 而 AD 就是把 SL / 10......
页:
[1]