Mens et manus

//  2 servo motor
#include <Servo.h>
Servo servo1;
Servo servo2;
int pos1[] = {89,90};
int pos2[] = {89,90};

// read degree的副程式會用到的變數
int i = 0;
int j = 0;
int flag;

char W[8];
char w[8];
int N[8];

void setup()
{
  Serial.begin(9600);
  for( j = 0; j < 7; j++)
  {
    w[j] = '0';
    W[j] = '0';
  }
  servo1.attach(4);//舵機一接pin4
  servo2.attach(8);//舵機一接pin8
  motor();//先把兩個舵機轉到90度
}

void loop()
{
  while(Serial.available() > 0)
  {
    my_Read_Degree();
    if( pos1[1] != pos1[0] || pos2[1] != pos2[0])
    {
      Serial.println("rotate!!");
      motor();
    }
  }
}

void motor()//單純讀取輸入的角度,並同時旋轉兩個舵機的副程式
{
  int j = pos1[0];
  int k = pos2[0];  
  while( pos1[1] != pos1[0] || pos2[1] != pos2[0] )//輸入的角度不等於初始角度時開始執行
  {
    if( pos1[1] > j )
    {    
      servo1.write(j);
      j++;//每次順轉一度
    }
    else if( pos1[1] < j )
    {
      servo1.write(j);
      j--;//每次逆轉一度
    }
    else if( pos1[1] == j )
    {
      pos1[0] = pos1[1];
    }
    
    if( pos2[1] > k )
    {    
      servo2.write(k);
      k++;
    }
    else if( pos2[1] < k )
    {
      servo2.write(k);
      k--;
    }
    else if( pos2[1] == k )
    {
      pos2[0] = pos2[1];
    }
    delay(15);//給15ms讓馬達轉一度
  }
  if( pos1[0] == pos1[1] && pos2[0] == pos2[1] )//當馬達旋轉到指定角度時,顯示在螢幕上
  {
    Serial.println("Arrive!");
    Serial.print(pos1[0]);
    Serial.print(",");
    Serial.println(pos2[0]);
  }  
}
 

void my_Read_Degree()
{
  while(Serial.available() > 0) //當用電腦輸入字元時, Serial.available() > 0, 不輸入時, Serial.available() = -1
  {
    W[i] = Serial.read();
    Serial.println(W[i]);    
    if( W[i] == ',' )
    {
      flag = i;
    }
    i++;//這裡的i用來計算輸入的字元數
  }  
 
  if( i > 0 )
  {// j = 7,表示我輸入的角度最多可以是 xxx,yyy共七個字元
    for( j = 0; j < 7; j++)//先平移第一個輸入角度
    {
      w[ j + 3 - flag ] = W[j];
    }
    for( j = 0; j < 7; j++)
    {      
      W[j] = w[j];
    }
    for( j = 0; j < 7 ; j++)
    {
      if( j == 4 )
      {
        int val = i - flag -1 ;
        if( val == 1 )//如果第二個角度是一位數
        {
          w[4] = '0';//補兩個0
          w[5] = '0';
          w[6] = W[4];
        }
        else if( val == 2 )//如果第二個角度是二位數
        {
          w[4] = '0';//補一個零
          w[5] = W[4];
          w[6] = W[5];
        }//三位數就不用補零了
      }
    }
    for( j = 0; j < 7; j++)
    {
      N[j] = w[j] - 48; //(char -> int ), 0的ASCII是48
    }    
    pos1[1] = 100*N[0] + 10*N[1] + N[2];//轉換並計算舵機一的角度
    pos2[1] = 100*N[4] + 10*N[5] + N[6];//轉換並計算舵機二的角度
    if( pos1[1] > 180 || pos2[1] > 180 || pos1[1] < 0 || pos2[1] < 0 ) //如果輸入的角度超過180度或小於0度,則使得輸入無效
    {
      Serial.println("Incorrect angle!");
      Serial.print(pos1[1]);
      Serial.print(",");
      Serial.println(pos2[1]);
      pos1[1] = pos1[0];
      pos2[1] = pos2[0];
    }
    else//如果輸入得正確,就把角度儲存下來,主程式運行到motor)時,就可以用了
    {
      Serial.print("Recive angle: ");
      Serial.print(pos1[1]);
      Serial.print(",");
      Serial.println(pos2[1]);
    }
    for( j = 0; j < 7; j++) //每次輸入完角度,要把角度重新歸零,否則下一次輸入時會輸入錯誤(什麼時候該儲存,儲存多少個字元的問題)
    {
      w[j] = '0';
      W[j] = '0';
    }
    i = 0;
    j = 0;
  }
}