首页 › Arduino › 机械手动作组编号转换成了角度。。。程序复杂化，求教啊

咸鱼geek 发表于 2011-12-14 17:22:49

机械手动作组编号转换成了角度。。。程序复杂化，求教啊

本帖最后由 咸鱼geek 于 2011-12-14 17:37 编辑

动作组就是六个舵机中的其中几个轮流步进5度来实现，看着像同时运行一样。。。
se，v分别是舵机初始值和待输入的舵机值。。。a，b数组就是舵机执行动作必须改变的舵机角度数组
#include <Servo.h>
//定义变量
Servo servo;
int se={80,110,80,90,100,130};//舵机初始化数组
int i=0,t=1000;//t为延时

String data="";
int mark=0,v={80,110,80,90,100,130}, over=0;
boolean tag={1,1,1,1,1,1};//初始化标志位数组,不相等

//舵机初始化
void datadeal()
{
int j = 0;
while(Serial.available() > 0)
{
delay(20);
data+=char(Serial.read());
mark=1;
}
if(mark==1)
{
Serial.println(data);
Serial.println(data.length());
int A={0};
for(int i = 0; i < data.length() ; i++)
{

   if(data == ',')
      {
      j++;
      }
       else
   {
   A = A * 10 + (data - '0');
   v =A;
// v =data*10+(data - '0');
   }
}
data=String("");
for(int i = 0; i < 6; i++)
    {
      Serial.print("Servo");
      Serial.print(i);
      Serial.print(" = ");
       Serial.println(v);

    }
    mark = 0;
    over=1;
}
}


//单个舵机控制函数
void fun(int a,int b,int i)
{
if(a!=b)
{
if(a<b)
{
while(a<b)
       {
         a+=5;
   servo.write(a);
   se=a;
   Serial.println(a);
   delay(t);
   
       }
}
else if(a>b)
{
   while(a>b) {
       a-=5;
       servo.write(a);
       se=a;
   Serial.println(a);
   delay(t);
   
   }
}
}

else
return;
}
//舵机动作函数
void Action(int a,int b,int i)
{
if(a<b)
{
         a+=5;
   servo.write(a);
   se=a;
   Serial.println(a);
   delay(t);      
}
else if(a>b)
{
       a-=5;
       servo.write(a);
       se=a;
   Serial.println(a);
   delay(t);   
   }
}

void setup()
{
servo.attach(22,380,1200);
servo.attach(23,380,1200);
servo.attach(24,380,1200);
servo.attach(25,380,1200);
servo.attach(26,380,1200);
servo.attach(27,380,1200);
for(i=0;i<6;i++)
servo.write(se);
    delay(2000);
    Serial.begin(9600);//设置波特率
    Serial.println("Ready");
}
void loop()
{
datadeal();
//单个舵机控制函数
if(over==1)
{

//boolean tag={1,1,1,1,1,1};//初始化标志位数组,不相等
//判断两个数组元素是否相等
for(int i=0;i<6;i++)
{
//相等则置标志位为0
if(se==v)//对应元素相等则tag=0
tag=0;

}
int sum=0;
for(int j=0;j<6;j++)
sum+=tag;
if(sum>0)//当tag的和大于0时，有对应元素需改变，调用循环
{
for(int num=0;num<6;num++)
{
Serial.println(v);
fun(se,v,num);//函数调用6次
tag=0;
}
}
//}
over=0;
//舵机连续动作组控制
while(Serial.available() > 0)
{
delay(20);
char action=Serial.read();

int a={80,175,80,90,100,130};//下降
switch(action)
{
case 'a'://下降

while(sum>0)//当tag的和大于0时，有对应元素需改变，调用循环
{
for(int num=0;num<6;num++)
{
Serial.println(v);
delay (100);
Action(se,a,num);//函数调用6次
//tag=0;
delay(50);
}
for(int i=0;i<6;i++)
{
//相等则置标志位为0
if(se==v)//对应元素相等则tag=0
tag=0;
}
int sum=0;
for(int j=0;j<6;j++)
sum+=tag;
if(sum==0)
break;
}
break;
case '2':
break;
//Action();
}
}
}
}

回复地方 发表于 2011-12-14 17:41:54

不懂，帮顶:lol

查看完整版本: 机械手动作组编号转换成了角度。。。程序复杂化，求教啊