#include <Servo.h>

#include <AFMotor.h>

Servo servo1;
Servo servo2;
AF_DCMotor M1(1, MOTOR12_1KHZ); // create motor #1, 64KHz pwm
AF_DCMotor M2(2, MOTOR12_1KHZ); // create motor #2, 64KHz pwm
AF_DCMotor M3(3, MOTOR34_1KHZ); // create motor #3, 8KHz pwm
AF_DCMotor M4(4, MOTOR34_1KHZ); // create motor #4, 8KHz pwm
int spd1 = 255; //lt(0-255)
int ser1a = 180; //og(SER1AF)0-180
int ser1b = 0; //ǳ(SER1AF)0-180
int ser2a = 180; //og(SER2AF)0-180
int ser2b = 0; //ǳ(SER2AF)0-180
int ser1 = 90; //@}lSER1AF
int ser2 = 90; //@}lSER2AF

void setup() {
  Serial.begin(9600);  // } Serial port, qTtv 9600 bps
  servo1.attach(10);
  servo2.attach(9);
  servo1.write(ser1);
  servo2.write(ser2);

}

void loop() {
  if (Serial.available() >= 0) { // ˬdO_ƥiŪ
    int x = Serial.read();
    switch (x) {
      case '1':
        forward1();
        break;
      case '2':
        back1();
        break;
      case '3':
        left1();
        break;
      case '4':
        right1();
        break;
      case '5':
        FC();
        break;
      case '6':
        RC();
        break;
      case '0':
        STOP();
        break;
      case 'a':
        servo1.write(ser1a);
        break;
      case 'b':
        servo1.write(ser1b);
        break;
      case 'c':
        servo2.write(ser2a);
        break;
      case 'd':
        servo2.write(ser2b);
        break;
    }
  }
}

void forward1()  // @eiNOk@_
{
  M1.run(FORWARD);
  M2.run(FORWARD);
  M3.run(FORWARD);
  M4.run(FORWARD);

  M1.setSpeed(spd1);
  M2.setSpeed(spd1);
  M3.setSpeed(spd1);
  M4.setSpeed(spd1);
}

void back1()   // @eiNOk@_
{
  M2.run(BACKWARD);
  M1.run(BACKWARD);
  M3.run(BACKWARD);
  M4.run(BACKWARD);

  M1.setSpeed(spd1);
  M2.setSpeed(spd1);
  M3.setSpeed(spd1);
  M4.setSpeed(spd1);
}

void left1()// @NOkA
{
  M1.run(FORWARD);
  M2.run(BACKWARD);
  M3.run(FORWARD);
  M4.run(BACKWARD);


  M1.setSpeed(spd1);
  M2.setSpeed(spd1);
  M3.setSpeed(spd1);
  M4.setSpeed(spd1);
}
void right1() // @kNOkA
{
  M1.run(BACKWARD);
  M2.run(FORWARD);
  M3.run(BACKWARD);
  M4.run(FORWARD);


  M1.setSpeed(spd1);
  M2.setSpeed(spd1);
  M3.setSpeed(spd1);
  M4.setSpeed(spd1);
}
void FC() // @kNOkA
{
  M1.run(BACKWARD);
  M2.run(FORWARD);
  M3.run(FORWARD);
  M4.run(BACKWARD);

  M1.setSpeed(spd1);
  M2.setSpeed(spd1);
  M3.setSpeed(spd1);
  M4.setSpeed(spd1);
}

void RC()  // @kNOkA
{
  M1.run(FORWARD);
  M2.run(BACKWARD);
  M3.run(BACKWARD);
  M4.run(FORWARD);

  M1.setSpeed(spd1);
  M2.setSpeed(spd1);
  M3.setSpeed(spd1);
  M4.setSpeed(spd1);
}
void STOP()  // @kNOkA
{
  M1.run(RELEASE);
  M2.run(RELEASE);
  M3.run(RELEASE);
  M4.run(RELEASE);
}