控制机械臂的全部程序在这里- #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[2];
- 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[10];
- float r[10];
- 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[10];
- float R[10];
- float SL=0;
- float SR=0;
- int i;
- float *AD=new float[2];
- for(i=0;i<=9;i++)
- {
- L[i]=Ldistance;
- R[i]=Rdistance;
- }
-
- i=0;
- while(i<=9)
- {
- SL+=L[i];
- SR+=R[i];
- i++;
- }
- AD[0]=SL/10;
- AD[1]=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[0]; //底板对准目标程序
- rdistance=avedistance[1];
- 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);
- }
- }
|
发表于 2015-6-8 11:39:13
楼主