机械臂控制代码.docx
《机械臂控制代码.docx》由会员分享,可在线阅读,更多相关《机械臂控制代码.docx(6页珍藏版)》请在冰豆网上搜索。
机械臂控制代码
##include#include
#include
#include"Wire.h"
#include"WiiChuck.h"
Servoservo1;//createservoobjecttocontrolaservo
Servoservo2;
Servoservo3;
Servoservo4;//amaximumofeightservoobjectscanbecreated
Servoservo5;
Servoservo6;
WiiChuckwii=WiiChuck();
intpos1,pos2,pos3,pos4,pos5,pos6,x,y,z,c;//variabletostorethe
servoposition
voidsetup()
//attachestheservoonpin9totheservoobjectservo1.attach(8);
servo2.attach(9);
servo3.attach(10);
servo4.attach(11);
servo5.attach(12);
servo6.attach(13);
pos1=60;
pos2=50;
pos3=100;
pos4=100;
pos5=90;
pos6=90;
servo1.write(pos1);
servo2.write(pos2);
servo3.write(pos3);
servo4.write(pos4);
servo5.write(pos5);
servo6.write(pos6);
wii.initWithPower();
voidloop()
if(true==wii.read())
{
//读取手柄值
x=wii.getJoyAxisX();y=wii.getJoyAxisY();c=wii.getButtonC();z=wii.getButtonZ();
号舵机
}
if(x==255&&y>120&&y<140&&c==0&&z==1)//
{pos1=pos1+10;
delay(40);
if(pos1>160)
{
pos1=160;
servo1.write(pos1);
}
}
elseif(x==255&&y>120&&y<140&&z==0&&c==1)
{pos1=pos1-10;
servo1.write(pos1);
delay(40);
if(pos1<20)
pos1=20;
servo1.write(pos1);
}
号舵机
}
elseif(x>220&&y==0&&c==0&&z==1)//2
{
pos2=pos2+10;
servo2.write(pos2);
delay(40);
if(pos2>160)
{
pos2=160;
servo2.write(pos2);
elseif(x>220&&y==0&&z==0&&c==1){
pos2=pos2-10;
servo2.write(pos2);
delay(40);
if(pos2<90)
{
pos2=90;
servo2.write(pos2);
}
号舵机
elseif(x>110&&x<140&&y==0&&c==0&&z==1)//3
{
pos3=pos3+10;servo3.write(pos3);
delay(40);
if(pos3>160)
{
pos3=160;
servo3.write(pos3);
}elseif(x>110&&x<140&&y==0&&z==0&&c==1)
pos3=pos3-10;
servo3.write(pos3);
delay(40);
if(pos3<20)
{
pos3=20;
servo3.write(pos3);
}
号舵机
}
elseif(x==0&&y==0&&c==0&&z==1)//4
{
servo4.write(pos4);
delay(40);
if(pos4>160)
{
pos4=160;
servo4.write(pos4);
}
}
elseif(x==0&&y==0&&z==0&&c==1)
{
servo4.write(pos4);
delay(40);
if(pos4<20)
{
pos4=20;
servo4.write(pos4);
}
号舵机
}
elseif(x==0&&y==128&&c==0&&z==1)//5
{
servo5.write(pos5);
pos5=pos5+10;
delay(40);
if(pos5>170){
pos5=160;
}
}
elseif(x==0&&y==128&&z==0&&c==1)
{
servo5.write(pos5);
pos5=pos5-10;
delay(40);
pos5=30;
}
号舵机
}
elseif(x==0&&y>240&&c==0&&z==1)//6
{
servo6.write(pos6);
pos6=pos6+10;
delay(40);
if(pos6>120)
{
}
elseif(x==0&&y>240&&z==0&&c==1)
{
servo6.write(pos6);
pos6=pos6-10;
delay(40);
if(pos6<45)
{
pos6=41;
}