本帖最后由 咸鱼geek 于 2011-12-14 17:37 编辑
动作组就是六个舵机中的其中几个轮流步进5度来实现,看着像同时运行一样。。。
se,v分别是舵机初始值和待输入的舵机值。。。a,b数组就是舵机执行动作必须改变的舵机角度数组 - #include <Servo.h>
- //定义变量
- Servo servo[6];
- int se[6]={80,110,80,90,100,130};//舵机初始化数组
- int i=0,t=1000;//t为延时
- String data="";
- int mark=0,v[6]={80,110,80,90,100,130}, over=0;
- boolean tag[6]={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[6]={0};
- for(int i = 0; i < data.length() ; i++)
- {
-
- if(data[i] == ',')
- {
- j++;
- }
- else
- {
- A[j] = A[j] * 10 + (data[i] - '0');
- v[j] =A[j];
- // v[j] =data[i]*10+(data[i] - '0');
- }
- }
- data=String("");
- for(int i = 0; i < 6; i++)
- {
- Serial.print("Servo");
- Serial.print(i);
- Serial.print(" = ");
- Serial.println(v[i]);
- }
- mark = 0;
- over=1;
- }
- }
- //单个舵机控制函数
- void fun(int a,int b,int i)
- {
- if(a!=b)
- {
- if(a<b)
- {
- while(a<b)
- {
- a+=5;
- servo[i].write(a);
- se[i]=a;
- Serial.println(a);
- delay(t);
-
- }
- }
- else if(a>b)
- {
- while(a>b) {
- a-=5;
- servo[i].write(a);
- se[i]=a;
- Serial.println(a);
- delay(t);
-
- }
- }
- }
- else
- return;
- }
- //舵机动作函数
- void Action(int a,int b,int i)
- {
- if(a<b)
- {
- a+=5;
- servo[i].write(a);
- se[i]=a;
- Serial.println(a);
- delay(t);
- }
- else if(a>b)
- {
- a-=5;
- servo[i].write(a);
- se[i]=a;
- Serial.println(a);
- delay(t);
- }
- }
- void setup()
- {
- servo[0].attach(22,380,1200);
- servo[1].attach(23,380,1200);
- servo[2].attach(24,380,1200);
- servo[3].attach(25,380,1200);
- servo[4].attach(26,380,1200);
- servo[5].attach(27,380,1200);
- for(i=0;i<6;i++)
- servo[i].write(se[i]);
- delay(2000);
- Serial.begin(9600);//设置波特率
- Serial.println("Ready");
- }
- void loop()
- {
- datadeal();
- //单个舵机控制函数
- if(over==1)
- {
- //boolean tag[6]={1,1,1,1,1,1};//初始化标志位数组,不相等
- //判断两个数组元素是否相等
- for(int i=0;i<6;i++)
- {
- //相等则置标志位为0
- if(se[i]==v[i])//对应元素相等则tag=0
- tag[i]=0;
- }
- int sum=0;
- for(int j=0;j<6;j++)
- sum+=tag[j];
- if(sum>0)//当tag的和大于0时,有对应元素需改变,调用循环
- {
- for(int num=0;num<6;num++)
- {
- Serial.println(v[num]);
- fun(se[num],v[num],num);//函数调用6次
- tag[num]=0;
- }
- }
- //}
- over=0;
- //舵机连续动作组控制
- while(Serial.available() > 0)
- {
- delay(20);
- char action=Serial.read();
- int a[6]={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[num]);
- delay (100);
- Action(se[num],a[num],num);//函数调用6次
- //tag[num]=0;
- delay(50);
- }
- for(int i=0;i<6;i++)
- {
- //相等则置标志位为0
- if(se[i]==v[i])//对应元素相等则tag=0
- tag[i]=0;
- }
- int sum=0;
- for(int j=0;j<6;j++)
- sum+=tag[j];
- if(sum==0)
- break;
- }
- break;
- case '2':
- break;
- //Action();
- }
- }
- }
- }
复制代码 |