#include <Servo.h>

//定义五中运动状态
#define STOP      0 // 停止
#define FORWARD   1 // 前进
#define BACKWARD  2 // 后退
#define TURNLEFT  3 // 左转
#define TURNRIGHT 4 // 右转


//定义需要用到的引脚
int leftMotor1 = 16; // D0 红线
int leftMotor2 = 5; // D1  褐线
int leftPWM = 4;

int rightMotor1 = 0; // D3  黑线
int rightMotor2 = 2; // D4  白线
int rightPWM = 14;

Servo servo;
void setup() {
  //设置控制电机的引脚为输出状态
  pinMode(leftMotor1, OUTPUT);
  pinMode(leftMotor2, OUTPUT);
  pinMode(rightMotor1, OUTPUT);
  pinMode(rightMotor2, OUTPUT);

  pinMode(leftPWM, OUTPUT);
  pinMode(rightPWM, OUTPUT);

  servo.attach(13); //D7
  servo.write(90);
}
 
void loop() {
  int cmd;
  for(cmd=0;cmd<5;cmd++) {
    analogWrite(leftPWM, 100);
    analogWrite(rightPWM, 100);
    motorRun(cmd);  
    delay(2000);
  } 
}


//运动控制函数
void motorRun(int cmd)
{
  switch(cmd){
    case FORWARD:
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, HIGH);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, HIGH);
      break;
     case BACKWARD:
      digitalWrite(leftMotor1, HIGH);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, HIGH);
      digitalWrite(rightMotor2, LOW);
      break;
     case TURNLEFT:
        servo.write(0);
//      digitalWrite(leftMotor1, HIGH);
//      digitalWrite(leftMotor2, LOW);
//      digitalWrite(rightMotor1, LOW);
//      digitalWrite(rightMotor2, HIGH);
      break;
     case TURNRIGHT:
        servo.write(180);
//      digitalWrite(leftMotor1, LOW);
//      digitalWrite(leftMotor2, HIGH);
//      digitalWrite(rightMotor1, HIGH);
//      digitalWrite(rightMotor2, LOW);
      break;
     default:
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, LOW);
  }
}