Arduino 藍芽遙控車

/*
藍芽接收
10 11
馬達 ABCD四顆
左前A
右前B
左後C
右後D

 左   頭   右
    A    B
    C    D
      尾


使用兩顆L298N控制4個馬達
Z=X,Y
Z=馬達 X=正轉 Y=逆轉
A=2,3
B=4,5
C=6,7
D=8,9


副程式 列表
AM_F  A馬達正轉 1,0
AM_R  A馬達逆轉 0,1
AM_S  A馬達停止 0,0

BM_F  B馬達正轉 1,0
BM_R  B馬達逆轉 0,1
BM_S  B馬達停止 0,0

CM_F  C馬達正轉 1,0
CM_R  C馬達逆轉 0,1
CM_S  C馬達停止 0,0

DM_F  D馬達正轉 1,0
DM_R  D馬達逆轉 0,1
DM_S  D馬達停止 0,0


藍芽傳輸內容:
    a=前進上緣訊號=97
    b=前進下緣訊號=98
    c=後退上緣訊號=99
    d=後退下緣訊號=100
    e=左轉上緣訊號=101
    f=左轉下緣訊號=102
    g=右轉上緣訊號=103
    h=右轉下緣訊號=104

*/
#include <SoftwareSerial.h>  //導入函式庫
#include <Wire.h>   //導入函式庫
SoftwareSerial BT(10,11);

//A馬達控制用副程式
int AM_F()
{
    digitalWrite(2,HIGH);
    digitalWrite(3,LOW);
}
int AM_R()
{
    digitalWrite(2,LOW);
    digitalWrite(3,HIGH);
}
int AM_S()
{
    digitalWrite(2,LOW);
    digitalWrite(3,LOW);
}

//B馬達控制用副程式
int BM_F()
{
    digitalWrite(4,HIGH);
    digitalWrite(5,LOW);
}
int BM_R()
{
    digitalWrite(4,LOW);
    digitalWrite(5,HIGH);
}
int BM_S()
{
    digitalWrite(4,LOW);
    digitalWrite(5,LOW);
}

//C馬達控制用副程式
int CM_F()
{
    digitalWrite(6,HIGH);
    digitalWrite(7,LOW);
}
int CM_R()
{
    digitalWrite(6,LOW);
    digitalWrite(7,HIGH);
}
int CM_S()
{
    digitalWrite(6,LOW);
    digitalWrite(7,LOW);
}

//D馬達控制用副程式
int DM_F()
{
    digitalWrite(8,HIGH);
    digitalWrite(9,LOW);
}
int DM_R()
{
    digitalWrite(8,LOW);
    digitalWrite(9,HIGH);
}
int DM_S()
{
    digitalWrite(8,LOW);
    digitalWrite(9,LOW);
}




void setup()
{
    for(int pin=2;pin<=9;pin++)  //設定腳位2~9都是輸出(給馬達用)
    {
            pinMode(pin,OUTPUT);
    }
    Serial.begin(9600);
    BT.begin(9600);
}

void loop()
{
    if( BT.available() == 1) //如果藍芽裝置讀到了值
    {
        Serial.print("Receive:");//序列阜顯示Receive:
        Serial.print(BT.read());//序列阜顯示收到的值
        Serial.print("\n");//序列阜換行
        int btread = BT.read();
        if ((btread== 98 )||(btread == 100)||(btread == 102)||(btread == 104))
        {
            btread = 98;
        }
        if (btread == 97)  //前進時 ABCD都正轉
        {
            AM_F();
            BM_F();
            CM_F();
            DM_F();
        }
        else if (btread == 99) //後退時 ABCD都逆轉
        {
            AM_R();
            BM_R();
            CM_R();
            DM_R();
        }
        else if (btread == 101)  //左轉時 BD正 AC逆
        {
            BM_F();
            DM_F();
            AM_R();
            CM_R();
        }
        else if (btread == 103)  //右轉時 AC正 BD逆
        {
            AM_F();
            CM_F();
            BM_R();
            DM_R();
        }
        else if (btread == 98)  //如果按鈕放開 所有馬達停止
        {
            AM_S();
            BM_S();
            CM_S();
            DM_S();
        }
    }
}