|
|
楼主 |
发表于 2016-4-11 09:04
|
显示全部楼层
/*
D5 a轴水平旋转
D6 b轴主臂
D9 c轴升降轴
D10 d轴机械手
A0 左x轴
A1 左y轴
A2 右x轴
A3 右Y轴
*/
int v_a=100;
int v_b=100;
int v_c=100;
int v_d=100;
int f19=4;//消除抖动值。值越大越迟钝
int servopin;
int servopin_a=5;//定义数字接口5连接伺服舵机信号线
int servopin_b=6;//定义数字接口6 连接伺服舵机信号线
int servopin_c=9;//定义数字接口9 连接伺服舵机信号线
int servopin_d=10;//定义数字接口10 连接伺服舵机信号线
int in_wlx=0;//定义模拟接口0
int in_wly=1;//定义模拟接口1
int in_wrx=2;//定义模拟接口2
int in_wry=3;//定义模拟接口3
//+++++++++++++++++++++++++++++++++++++++++++++++++++
int myangle;//定义角度变量
int pulsewidth;//定义脉宽变量
int val;
void servopulse(int servopin,int myangle)//定义一个脉冲函数
{
for(int i=0;i<=5;i++) //给予舵机足够的时间让它转到指定角度
{
pulsewidth=(myangle*11)+500;//将角度转化为500-2480 的脉宽值
digitalWrite(servopin,HIGH);//将舵机接口电平至高
delayMicroseconds(pulsewidth);//延时脉宽值的微秒数
digitalWrite(servopin,LOW);//将舵机接口电平至低
delay(20-pulsewidth/1000);
}
}
void setup()
{
pinMode( servopin_a,OUTPUT);//设定舵机接口为输出接口
pinMode( servopin_b,OUTPUT);//设定舵机接口为输出接口
pinMode( servopin_c,OUTPUT);//设定舵机接口为输出接口
pinMode( servopin_d,OUTPUT);//设定舵机接口为输出接口
Serial.begin(9600);//连接到串行端口，波特率为9600
Serial.println("servo=o_seral_simple ready" ) ;
}
void loop()//将0 到9 的数转化为0 到180 角度，并让LED 闪烁相应数的次数
{
val=analogRead(in_wlx);// 读取传感器的模拟值并赋值给val
Serial.print(val ) ;
Serial.print("," ) ;
Serial.print(v_a ) ;
Serial.println("A" ) ;
if(abs(val-v_a)>f19){
v_a=val;
Serial.print(val ) ;
Serial.print("," ) ;
Serial.print(v_a ) ;
Serial.println("A" ) ;
val=map(val,0,1023,0,180);
servopin=servopin_a;
servopulse(servopin,val);//引用脉冲函数
}
val=analogRead(in_wly);// 读取传感器的模拟值并赋值给val
if(abs(val-v_b)>f19){
v_b=val;
val=map(val,0,1023,60,160);
servopin=servopin_b;
servopulse(servopin,val);//引用脉冲函数
Serial.print(val ) ;
Serial.print("," ) ;
Serial.print(v_b ) ;
Serial.println("B" ) ;
}
val=analogRead(in_wrx);// 读取传感器的模拟值并赋值给val
if(abs(val-v_c)>f19){
v_c=val;
val=map(val,0,1023,40,150);
servopin=servopin_c;
servopulse(servopin,val);//引用脉冲函数
Serial.print(val ) ;
Serial.print("," ) ;
Serial.print(v_c ) ;
Serial.println("C" ) ;
}
val=analogRead(in_wry);// 读取传感器的模拟值并赋值给val
if(abs(val-v_d)>f19){
v_d=val;
val=map(val,0,1023,60,180);
servopin=servopin_d;
servopulse(servopin,val);//引用脉冲函数
Serial.print(val ) ;
Serial.print("," ) ;
Serial.print(v_d ) ;
Serial.println("D" ) ;
}
}
|
|
发表于 2016-4-5 20:04
