int data = 0;          // 接收到的 data byte

String inputString = "";         // 用来储存接收到的内容

boolean newLineReceived = false; // 前一次数据结束标志

#define STOP    177 //停止识别信号

#define FORWARD   178 //前进识别信号

#define TURNLEFT   180 //左转识别信号

#define BACKWARD   181 //后退识别信号

#define TURNRIGHT 182 //右转识别信号

#define SERVO1 241   //舵机1识别信号

#define SERVO2 242   //舵机2识别信号

#define SERVO3   243   //舵机3识别信号

#define SERVO4 179   //舵机4识别信号

int a=1;   //爪子开合

Servo servo_[4];   //创建4个舵机

int servo_port[4]={3,4,7,8}; //4个舵机引脚端口

Servo servo_D;   //方向舵机

int leftMotor1 = 5; //电机

int leftMotor2 = 6;//电机

int rightMotor1 = 9;//电机

int rightMotor2 = 10;//电机

void setup()

{

Serial.begin(9600); //波特率9600

for(int i =0;i<4;i++){servo_[i].attach(servo_port[i]);}//舵机关联引脚

servo_D.attach(11);   //舵机关联引脚

pinMode(leftMotor1, OUTPUT); //定义电机引脚模式为输出

pinMode(leftMotor2, OUTPUT);

pinMode(rightMotor1, OUTPUT);

pinMode(rightMotor2, OUTPUT);

}

void loop() {if (newLineReceived) {          //主程序

/*motorRun((byte)inputString[5]);*/

motorRun((byte)inputString[3]);            //调用swittch 选择判断

inputString = "";   // clear the string       //清空字符串 防止累计叠加下次读值

newLineReceived = false;                // 字符串完整标志

}}

void motorRun(int cmd)

{   switch(cmd){

    case FORWARD:Serial.println("FORWARD");Forwards();break;

    case BACKWARD:Serial.println("BACKWARD");Back();break;

    case TURNLEFT:Serial.println("TURN   LEFT");Left();break;

    case TURNRIGHT:Serial.println("TURN   RIGHT");Right();break;

    case STOP:Serial.println("STOP");Stop();break;

    case SERVO1:servo_[0].write((byte)inputString[4]);break;

    case SERVO2:servo_[1].write((byte)inputString[4]);break;

    case SERVO3:servo_[2].write((byte)inputString[4]);break;

    case SERVO4:if(a==1){servo_[3].write(90);a=0;}else{servo_[3].write(140);a=1;}

    default:Stop();

  }

}  

void Forwards()            //前进

{

servo_D.write(90);

digitalWrite(leftMotor1,1 );

digitalWrite(leftMotor2, 0);

digitalWrite(rightMotor1, 1);

digitalWrite(rightMotor2, 0);

}

void Back()               //后退

{

digitalWrite(leftMotor1,0);

digitalWrite(leftMotor2, 1);

digitalWrite(rightMotor1, 0);

digitalWrite(rightMotor2, 1);

}

void Left()                //左转

{

servo_D.write(130);

digitalWrite(leftMotor1,1 );

digitalWrite(leftMotor2, 0);

digitalWrite(rightMotor1, 1);

digitalWrite(rightMotor2, 0);/*

analogWrite(leftMotor1, 100);

analogWrite(leftMotor2, LOW);

analogWrite(rightMotor1, 255);

analogWrite(rightMotor2, LOW);*/

}

void Right()                 //右转

{

servo_D.write(50);

digitalWrite(leftMotor1,1 );

digitalWrite(leftMotor2, 0);

digitalWrite(rightMotor1, 1);

digitalWrite(rightMotor2, 0);

/*analogWrite(leftMotor1, 255);

analogWrite(leftMotor2, LOW);

analogWrite(rightMotor1, 100);

analogWrite(rightMotor2, LOW);*/

}

void Stop()                //停止

{

digitalWrite(leftMotor1, LOW);

digitalWrite(leftMotor2, LOW);

digitalWrite(rightMotor1, LOW);

digitalWrite(rightMotor2, LOW);

}

void serialEvent()           //串口事件   对串口传输的信息进行处理

{while (Serial.available()) {    //判断缓冲器状态。回传有多少位元组（bytes）的资料尚未被read()函式读取，如果回传值是0代表所有序列埠上资料都已经被read()函式读取。

data = Serial.read();            //一个字节一个字节地读，下一句是读到的放入字符串数组中组成一个完成的数据包

inputString += (char) data;     // 全双工串口可以不用在下面加延时，半双工则要加的//

if (data ==170) {

newLineReceived = true;

     }

  }

}