Arduino 控制小車加機械臂

　　是菜鳥的作品，實作了機械臂的控制，和小車前後左右的控制。連接配接 WiFi 後可實作TCP 通信。主要子產品用到了一個Arduino nuo 闆、一個紅闆、一個 TD-06 WiFi子產品、四個舵機和兩個小車驅動電機。

　　實物圖如下

　　代碼如下

#include <Servo.h>

//--------------------------------------------------舵機引腳
#define PIN_SERV7 7 //舵機号（IO口）
#define PIN_SERV8 8 //舵機号（IO口）
#define PIN_SERV9 9 //舵機号（IO口）
#define PIN_SERV10 10 //舵機号（IO口）
//--------------------------------------------------小車引腳
#define input1 3 // 定義uno的pin 3 向 input1 輸出
#define input2 4 // 定義uno的pin 4 向 input2 輸出----//3、4引腳控制左輪
#define input3 5 // 定義uno的pin 5 向 input3 輸出
#define input4 6 // 定義uno的pin 6 向 input4 輸出----//5、6引腳控制右輪
//-----------------------------------------------------------------------------------
//兩驅動小車

Servo myservo_7, myservo_8, myservo_9, myservo_10;

//----------------------小車控制函數--------------------------------------------------

char Forward() //前進
{
  digitalWrite(input1, HIGH); //給高電平1
  digitalWrite(input2, LOW); //給低電平0
  digitalWrite(input3, HIGH); //給高電平1
  digitalWrite(input4, LOW); //給低電平0
  while(1)
  {
    if(Serial.available())      // 如果要選擇新的模式：逆時針轉，停止
    {
        char mode = Serial.read();
        Stop();
        return mode;            // 傳回新的指令
    }
   }
}

char Backward() //後退
{
  digitalWrite(input1, LOW);
  digitalWrite(input2, HIGH);
  digitalWrite(input3, LOW);
  digitalWrite(input4, HIGH);
  while(1)
  {
    if(Serial.available())      // 如果要選擇新的模式：逆時針轉，停止
    {
        char mode = Serial.read();
        Stop();
        return mode;            // 傳回新的指令
    }
   }
}

void Stop()//------------------停止
{
  digitalWrite(input1, LOW);
  digitalWrite(input2, LOW);
  digitalWrite(input3, LOW);
  digitalWrite(input4, LOW);
}

char  TurnRight()//-------------右轉
{
  digitalWrite(input1, HIGH);
  digitalWrite(input2, LOW);
  digitalWrite(input3, HIGH);
  while(1)
  {
    digitalWrite(input4, LOW);
    delay(80);
    digitalWrite(input4, HIGH);
    delay(20);
    if(Serial.available())      // 如果要選擇新的模式：逆時針轉，停止
    {
        char mode = Serial.read();
        Stop();
        return mode;            // 傳回新的指令
    }
  }
}

char TurnLeft()//--------------左轉
{
  digitalWrite(input2, LOW);//左輪
  digitalWrite(input3, HIGH);//右輪
  digitalWrite(input4, LOW);
  while(1)
  {
    digitalWrite(input1, HIGH);
    delay(80);
    digitalWrite(input1, LOW);
    delay(20);
    if(Serial.available())      // 如果要選擇新的模式：逆時針轉，停止
    {
        char mode = Serial.read();
        Stop();
        return mode;            // 傳回新的指令
    }
  }
}


//------------------------舵機控制函數-------------------------------------------------

char clockwise(Servo myservo, int stop_angle=0) //順時針    角度減小   後 下 抓 順
{                                               //                      2, 4, 5, 7   
    int start_angle = myservo.read();//讀取舵機目前角度
    
    for(int i=start_angle; i>stop_angle; i--)
    {
        myservo.write(i);           //使舵機轉動至指定角度
        //Serial.println(i);
        delay(30);
        if(Serial.available())      // 如果要選擇新的模式：逆時針轉，停止
        {
            char mode = Serial.read();
            return mode;            // 傳回新的指令
        }
    }
    return '9';
}


char anticlockwise(Servo myservo, int stop_angle=180)//逆時針       角度增大  前 上 放 逆
{                                                    //                        1, 3, 6, 8
    int start_angle = myservo.read();         //讀取舵機目前角度
    
    for(int i=start_angle; i<stop_angle; i++)
    {
        myservo.write(i); 
        //Serial.println(i);
        delay(30);
        if(Serial.available())        // 如果要選擇新的模式：順時針轉，停止
        {
            char mode = Serial.read();
            return mode;
        }
    }
    return '9';
}
//-----------------------------------------------------------------------------------
void setup()
{
  Serial.begin(9600);        //初始化序列槽
  myservo_7.attach(PIN_SERV7);// 将Servo變量附加到引腳，将7号引腳設定為7号舵機控制端口
  myservo_8.attach(PIN_SERV8);
  myservo_9.attach(PIN_SERV9); 
  myservo_10.attach(PIN_SERV10);

  pinMode(input1, OUTPUT);//小車序列槽設定為輸出模式
  pinMode(input2, OUTPUT);
  pinMode(input3, OUTPUT);
  pinMode(input4, OUTPUT);
  //Serial.println("Test start ");

}
//-----------------------------------------------------------------------------------
void loop()
{
    if(Serial.available())            // 等待接收控制信号（非阻塞）
    {
        char mode_9g = Serial.read(); // 讀取模式選擇指令
        int jump=1;
        while(1)
        {
            switch(mode_9g)
            {
                case '1':       //-----前 角度增大
              {   
                mode_9g = anticlockwise(myservo_8);// 93/110 --> 180     
                break;
              }
            case '2':       //-----後 角度減小
              {   
                mode_9g = clockwise(myservo_8, 110);// 180 --> 110      
                break;
              }
            case '3':       //-----上 角度增大
              {   
                mode_9g = anticlockwise(myservo_9);// 93/65/80 -->180
                break;
              }
            case '4':       //-----下 角度減小
              {   
                int zz=myservo_8.read();
                if (zz>115)
                  mode_9g = clockwise(myservo_9, 65);// 180 --> 65
                else
                  mode_9g = clockwise(myservo_9, 85);// 93/180 --> 80
                break;
              }
            case '5':       //-----抓 角度減小
              {
                mode_9g = clockwise(myservo_10,90);// 93/180 --> 90
                break;
              }
            case '6':       //-----放 角度增大
              {
                mode_9g = anticlockwise(myservo_10);// 93/85 --> 180
                break;
              }
            case '7':       //-----底座右轉(順) 角度減小
              {
                mode_9g = clockwise(myservo_7); // 93/180 --> 0
                break;
              }
            case '8':       //----底座左轉(逆) 角度增大
              {
                mode_9g = anticlockwise(myservo_7);// 93/0 --> 180
                break;
              }
            case 'a':       //左轉
              {
                mode_9g = TurnLeft();
                break;
              }
            case 'w':       //前進
              {
                mode_9g = Forward();
                break;
              }
            case 'd':       //右轉
              {
                mode_9g = TurnRight();
                break;
              }
            case 's':       //後退
              {
                mode_9g = Backward();
                break;
              }
            case 'p':       //停止
              {
                Stop();
                jump = 0;
                break;
              }
            default:
              {
                 if(mode_9g=='9') // 模式 9 結束轉動 或 因轉到極限位置而停止
                    Serial.println("over");
                 else
                    Serial.println("Input error");
                 jump = 0;
                 Stop();
                 break;
               }
            }
            if (jump==0)//跳出 while 以接收新的模式指令 
            { 
               break;
            }
        }
    }
}