机械臂控制代码

##include

#include

#include

#include "Wire.h"

#include "WiiChuck.h"

Servo servo1; // create servo object to control a servo

Servo servo2;

Servo servo3;

Servo servo4; // a maximum of eight servo objects can be created Servo servo5;

Servo servo6;

WiiChuck wii = WiiChuck();

int pos1,pos2,pos3,pos4,pos5,pos6,x,y,z,c; // variable to store the servo position

void setup()

{

// attaches the servo on pin 9 to the servo object

servo1.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();

}

void loop()

{

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;

servo1.write(pos1);

delay (40);

if(pos1>160)

{

pos1=160;

servo1.write(pos1);

}

}

else if(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);

}

}

else if(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);

}

else if(x>220 && y==0 && z==0 && c==1 )

{

pos2 = pos2-10;

servo2.write(pos2);

delay(40);

if(pos2<90)

{

pos2=90;

servo2.write(pos2);

}

}

else if(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);

}

}

else if(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);

}

}

else if(x==0 && y==0 && c == 0 && z==1) //4号舵机

{

pos4 = pos4+10;

servo4.write(pos4);

delay (40);

if(pos4>160)

{

pos4=160;

servo4.write(pos4);

}

}

else if(x==0 && y==0 && z==0 && c==1 )

{

pos4 = pos4-10;

servo4.write(pos4);

delay(40);

if(pos4<20)

{

pos4=20;

servo4.write(pos4);

}

}

else if(x==0 && y==128 && c == 0 && z==1) //5号舵机 {

servo5.write(pos5);

pos5 = pos5+10;

delay (40);

if(pos5>170)

{

pos5=160;

}

}

else if(x==0 && y==128 && z==0 && c==1 )

{

servo5.write(pos5);

pos5 = pos5-10;

delay(40);

if(pos5<20)

{

pos5=30;

}

}

else if(x==0 && y>240 && c == 0 && z==1) //6号舵机 {

servo6.write(pos6);

pos6 = pos6+10;

delay (40);

if(pos6>120)

{

pos6=119;

}

}

else if(x==0 && y>240 && z==0 && c==1 )

{

servo6.write(pos6);

pos6 = pos6-10;

delay(40);

if(pos6<45)

{

pos6=41;

}

}

}