机械臂控制代码

机械臂扌才亠集团化小组：[VVO-JOPP28-JPPTL98-LOPPNN]voidsetup()//attachestheservoonpin9totheservoobjectservol.attach(8);servo2・attach(9);servo3・attach(10);servo4.attach(ll);servo5・attach(12);servo6・attach(13);posl=60;pos2=50;pos3=100;pos4=100;pos5=90;pos6=90;servo1.write(posl);servo2・write(pos2);servo3・write(pos3);servo4・write(pos4);servo5・write(pos5);servo6・write(pos6);wii.initWithPowerO;voidloop()if(true==wii.read())〃读取手柄值x二wii.getJoyAxisXO;y二wii.getJoyAxisY();c二wii.getButtonC();z二wii.getButtonZ();}if(x==255&&y>120&&y<140&&c=0&&z==l)//—号舵机{posl=posl+10;servo1.write(posl);delay(40);辻(posl>160){posl二160;servol.write(posl);}}else辻(x二二255&&y>120&&y<140&&z~0&&c==l){posl=posl-10;servo1.write(posl);delay(40);if(posl<20)posl二20;servo1.write(posl);}}else辻(x>220&&y=0&&c二二0&&z=l)//2号舵机{pos2=pos2+10;servo2・write(pos2);delay(40);辻(pos2>160){pos2=160;servo2・write(pos2);else辻(x>220&&y二二0&&z==0&&c==l){pos2=pos2T0;servo2・write(pos2);delay(40);辻(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);辻(pos3>160){pos3=160;servo3・write(pos3);}}else辻(x>110&&x<140&&y二二0&&z==0&&c==l)pos3=pos3T0;servo3・write(pos3);delay(40);辻(pos3<20){pos3二20;servo3・write(pos3);}}else辻(x=0&&y=0&&c=0&&z二二1)//4号舵机servo4.write(pos4);delay(40);辻(pos4>160){pos4=160;servo4.write(pos4);}}elseif(x==0&&y=0&&z二二0&&c==l)servo4.write(pos4);delay(40);辻(pos4<20){pos4=20;servo4・write(pos4);}}else辻(x=0&&尸二128&&c二二0&&z=l)〃5号舵机{servo5・write(pos5);pos5=pos5+10;delay(40);if(pos5>170)pos5=160;}else辻(x==0&&y==128&&z==0&&c==l)servo5・write(pos5);pos5=pos5T0;delay(40);pos5=30;}}else辻(x=0&&y>240&&c二二0&&z=l)//6号舵机{servo6・write(pos6);pos6=pos6+10;delay(40);辻(pos6>120)}else辻(x==0&&y>240&&z==0&&c==l){servo6・write(pos6);pos6=pos6T0;delay(40);辻(pos6<45){pos6=41;}