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;
            }
        }
    }
}