//测速模块
int SpeedNumLeft = 12; //连接到pin 12
int SpeedNmuRight = 13; //连接到pin 13
//左轮驱动
int MotorLeftPinA = 2;
int MotorLeftPinB = 4;
int MotorLeftEn = 3;
//右轮驱动
int MotorRightPinA = 7;
int MotorRightPinB = 8;
int MotorRightEn = 6;

int Motor;
int Speed = 100;
int dir;

int i;
int j;
int x;
int y;

int val;

int n = 0; //码盘读数（即状态跳变次数）
int m = 0; //码盘读数（即状态跳变次数）
int flagL = 0; //因为初始读数可能为0或1，该标志是用来读取初始状态的
int flagR = 0; //因为初始读数可能为0或1，该标志是用来读取初始状态的

void setup()
{
    Serial.begin(9600);

    pinMode(MotorLeftPinA, OUTPUT);
    pinMode(MotorLeftPinB, OUTPUT);
    pinMode(MotorLeftEn, OUTPUT);

    pinMode(MotorRightPinA, OUTPUT);
    pinMode(MotorRightPinB, OUTPUT);
    pinMode(MotorRightEn, OUTPUT);

    pinMode(SpeedNumLeft, INPUT);
    pinMode(SpeedNmuRight, INPUT);  
}

//电机驱动函数
void MOTORDRIVE(char motor, int speed, int dir)
{
    if(motor == 'L')    
    {
        analogWrite(MotorLeftEn, speed);
        if(dir == 1)
        {
            digitalWrite(MotorLeftPinA, HIGH);
            digitalWrite(MotorLeftPinB, LOW);
        }
        else
        {
            digitalWrite(MotorLeftPinA, LOW);
            digitalWrite(MotorLeftPinB, HIGH);
        }
    }

    if(motor == 'R')    
    {
        analogWrite(MotorRightEn, speed);
        if (dir == 1)
        {
            digitalWrite(MotorRightPinA, HIGH);
            digitalWrite(MotorRightPinB, LOW);
        }
        else
        {
            digitalWrite(MotorRightPinA, LOW);
            digitalWrite(MotorRightPinB, HIGH);
        }
    }
}

//码盘函数
void SpeedNum(char motor)
{
    if(motor == 'L')
    {
        i = digitalRead(SpeedNumLeft); //读取码盘状态（0或1）
        if(!flagL){ //获取初始状态读数
            j = i;
            flagL = 1;
        }

        if(i != j){ //每次状态发生跳变的时候，读数增加1
            n += 1;
            j = i;
            Serial.print("Left:");
            Serial.println(n);
            Serial.print("Speed:");
            Serial.println(Speed);
        }
    }

    if(motor == 'R')
    {
        x = digitalRead(SpeedNmuRight); //读取码盘状态（0或1）
        if(!flagR){ //获取初始状态读数
            j = y;
            flagR = 1;
        }

        if(x != y){ //每次状态发生跳变的时候，读数增加1
            m += 1;
            y = x;
            Serial.print("Right:");
            Serial.println(m);
            Serial.print("Speed:");
            Serial.println(Speed);
        }
    }
}

//前进
void Forward()
{
    MOTORDRIVE('R',Speed,1);
    MOTORDRIVE('L',Speed,1);
}
//后退
void Backward()
{
    MOTORDRIVE('R',100,0);
    MOTORDRIVE('L',100,0);
}
//停止
void Stop()
{
    MOTORDRIVE('R',0,1);
    MOTORDRIVE('L',0,1);    
}
//右转
void RTurn()
{
    Stop();
    MOTORDRIVE('L',100,1);
}
//左转
void LTurn()
{
    Stop();
    MOTORDRIVE('R',100,1);
}
//加速
void SpeedUp()
{
    Speed += 10;
}
//减速
void SpeedDown()
{
    Speed -= 10;
}

void loop()
{
    SpeedNum('R');
    SpeedNum('L');
    if(Serial.available())
    {
        val = Serial.read();
        if(val == 'f'){
            Forward();
        }
        if(val == 's'){
            Stop();
        }
        if(val == 'b')
        {
            Backward();
        }
        if(val == 'l')
        {
            LTurn();
        }
        if(val == 'r')
        {
            RTurn();
        }
        if(val == 'o')
        {
            SpeedUp();
        }
        if(val == 'p')
        {
            SpeedDown();
        }
    }
}