Posted on Leave a comment

SNA-100-6dof arduino source code

<pre lang="”LANGUAGE”" line="”0″">#include &lt;Servo.h&gt; 

Servo myservoA; 

Servo myservoB;

Servo myservoC;

Servo myservoD;

Servo myservoE;

Servo myservoF;

int i,pos,myspeed;

int sea,seb,sec,sed,see,sef;

static int v=0;

String mycommand=""; //捕捉串口发来的指令 #auto:自动运行 #com:计算机串口控制 #stop:静止状态

static int mycomflag=2; // #auto:2 自动运行 , #com: 1 计算机控制 #stop:0 静止状态 

void myprint()

{

 sea=myservoA.read();

 seb=myservoB.read();

 sec=myservoC.read();

 sed=myservoD.read();

 see=myservoE.read();

 sef=myservoF.read();
 

 Serial.print("A=");

 Serial.print(sea);

 Serial.print(" B=");

 Serial.print(seb);

 Serial.print(" C=");

 Serial.print(sec);

 Serial.print(" D=");

 Serial.print(sed);

 Serial.print(" E=");

 Serial.print(see);

 Serial.print(" F=");

 Serial.println(sef); 

}

void myservosetup() //舵机初始化到等待状态

{

 sea=myservoA.read();

 seb=myservoB.read();

 sec=myservoC.read();

 sed=myservoD.read();

 see=myservoE.read();

 sef=myservoF.read();
 

 myspeed=500;

 for(pos=0;pos&lt;=myspeed;pos+=1)

 {

 myservoA.write(int(map(pos,1,myspeed,sea,66)));

 myservoB.write(int(map(pos,1,myspeed,seb,90)));

 myservoC.write(int(map(pos,1,myspeed,sec,50)));

 myservoD.write(int(map(pos,1,myspeed,sed,90)));

 myservoE.write(int(map(pos,1,myspeed,see,120)));

 myservoF.write(int(map(pos,1,myspeed,sef,90))); 

 delay(1);

 }

}

void setup() 

{ 

 Serial.begin(9600,SERIAL_8N1);
 

 mycomflag=2; // 机械臂默认上电状态为:2 自动运行

 myservoA.attach(3); // 控制腰部(A)的端口是~3号

 myservoB.attach(5); // 控制大臂(B)的端口是~5号

 myservoC.attach(6); // 控制小臂(C)的端口是~6号

 myservoD.attach(9); // 控制小臂旋转(D)的端口是~9号

 myservoE.attach(10); // 控制腕部(E)的端口是~10号

 myservoF.attach(11); // 控制腕部旋转(F)的端口是~11号
 

 myservoA.write(66);

 myservoB.write(90);

 myservoC.write(55);

 myservoD.write(90);

 myservoE.write(120);

 myservoF.write(90);

}

void loop() 

{ 

 while (Serial.available() &gt; 0) 

 {

 mycommand += char(Serial.read());

 delay(2);

 }

 if (mycommand.length() &gt; 0)

 {

 if(mycommand=="#auto")

 {

 mycomflag=2;

 Serial.println("auto station");

 mycommand="";

 }

 if(mycommand=="#com")

 {

 mycomflag=1;

 Serial.println("computer control station");

 mycommand="";

 myservosetup();

 }

 if(mycommand=="#stop")

 {

 mycomflag=0;

 Serial.println("stop station");

 mycommand="";

 } 

 } 
 

 if(mycomflag==1) //如果是计算机串口控制 1

 { 
 

 for(int m=0;m&lt;mycommand.length();m++) // 

 {

 char ch = mycommand[m]; //读取串口数据

 switch(ch)

 {

 case '0'...'9':

 v = v*10 + ch - '0'; //字符转换成十进制

 break;
 

 case 'a': //如果数据后带a,则表示是一号舵机的数据,比如串口发送85a

 if(v &gt;= 5 || v &lt;= 175 ) myservoA.write(v); //用于设定舵机旋转角度的语句,可设定的角度范围是0°到180°,“V”得到所输入的值而改变角度,比如85a为85度角

 v = 0;

 myprint();

 break;

 case 'b': //如果数据后带b,则表示是二号舵机的数据,比如串口发送85a

 myservoB.write(v); //用于设定舵机旋转角度的语句,可设定的角度范围是0°到180°,“V”得到所输入的值而改变角度,比如90b为90度角

 v = 0;

 myprint();

 break;
 

 case 'c': 

 if(v &gt;= 20 ) myservoC.write(v); 

 v = 0;

 myprint();

 break;

 case 'd': 

 myservoD.write(v); 

 v = 0;

 myprint();

 break;

 case 'e': 

 myservoE.write(v); 

 v = 0;

 myprint();

 break;
 

 case 'f': 

 myservoF.write(v); 

 myprint();

 v = 0;

 break;

 }
 

 } 

 mycommand=""; 
 

 } // end if(mycomflag=2)
 

 if(mycomflag==2) //如果是自动运行状态2 

 { 

 delay(3000); 

 //Serial.println("auto station"); 

 myservosetup();

 myspeed=500;

 for(pos = 0; pos &lt;=myspeed; pos += 1) 

 { 

 myservoA.write(int(map(pos,1,myspeed,66,90))); // 让A从66度旋转到90度 (可修改角度)

 myservoB.write(int(map(pos,1,myspeed,90,70))); //让B从90度旋转到40度 (可修改角度)

 delay(1); 

 }

 delay(1000);

 myspeed=500;

 for(pos = 0; pos &lt;=myspeed; pos += 1) 

 { 

 myservoC.write(int(map(pos,1,myspeed,50,65))); // 

 myservoD.write(int(map(pos,1,myspeed,90,170))); //

 myservoE.write(int(map(pos,1,myspeed,90,5))); 

 delay(1); 

 }

 myspeed=1000;

 for(pos = 0; pos &lt;=myspeed; pos += 1) 

 { 

 myservoB.write(int(map(pos,1,myspeed,70,90))); // 

 myservoC.write(int(map(pos,1,myspeed,65,50))); //

 delay(1); 

 }

 myspeed=500;

 for(pos = 0; pos &lt;=myspeed; pos += 1) 

 { 

 myservoC.write(int(map(pos,1,myspeed,50,45))); // 

 myservoD.write(int(map(pos,1,myspeed,170,90))); //

 myservoE.write(int(map(pos,1,myspeed,5,50)));

 myservoF.write(int(map(pos,1,myspeed,90,40)));

 delay(1); 

 }

 myspeed=1000;

 for(pos = 0; pos &lt;=myspeed; pos += 1) 

 { 

 myservoA.write(int(map(pos,1,myspeed,90,140))); // 

 myservoF.write(int(map(pos,1,myspeed,40,130))); 

 delay(1); 

 } 

 myspeed=500;

 for(pos = 0; pos &lt;=myspeed; pos += 1) 

 { 

 myservoA.write(int(map(pos,1,myspeed,140,90))); // 

 myservoC.write(int(map(pos,1,myspeed,45,50))); // 

 myservoB.write(int(map(pos,1,myspeed,90,70))); //

 myservoE.write(int(map(pos,1,myspeed,50,120))); //

 delay(1); 

 } 

 }
 

 if(mycomflag==0) //如果是静止状态0

 {

 myservosetup();

 }

}</pre>
Leave a Reply

Your email address will not be published. Required fields are marked *