//藍芽控制，需1.手機藍芽配業，2.app BlueTooth
// 馬達新配置
#include <SoftwareSerial.h>
SoftwareSerial BTserial(12,13); //12接藍芽的TX，13接藍芽的RX

const int posR=5 ;   // in1 , ~pwm，右正
const int negR=4;    // in2
const int posL=6;    // in3 , ~pwm，左正
const int negL=7;    // in4

int speedR=120;
int speedL=115;         //原120，因右偏，降左輪
int speedRback=120;
int speedLback=120;       
char ctrlChar='S';      //控制字元預設為S，停止

void setup() 
{ pinMode(posR,OUTPUT);     //設4支馬達控制腳
  pinMode(negR,OUTPUT);
  pinMode(posL,OUTPUT);
  pinMode(negL,OUTPUT); 
//  Serial.begin(9600);
  BTserial.begin(9600);    //開啟藍芽連線
  } 
  
void loop() 
{
  if (BTserial.available()) {      //若藍芽有資料
     ctrlChar = BTserial.read();   //讀入藍芽資料 
  //   Serial.println(ctrlChar);
  }
     switch (ctrlChar) {           
        case 'F':
          Forward(speedR,speedL);
          break;
       case 'B':
          back(speedRback,speedLback);
         break;
       case 'R':
          right(speedRback,speedLback);
         break;
       case 'L':
          left(speedRback,speedLback);
         break;  
       case 'S':
         stop1();
         break;  
       case '7':
          FL(speedR,speedL);
         break;  
       case '9':
          FR(speedR,speedL);
         break;  
       case '1':
          BL(speedR,speedL);
         break;  
       case '3':
          BR(speedR,speedL);
         break;                     
        }  
}

void Forward(byte RmotorSpeed, byte LmotorSpeed)
{
    analogWrite(posR,255-RmotorSpeed);  //右輪反轉 
    digitalWrite(negR,HIGH);            //右輪反轉  
    analogWrite(posL,LmotorSpeed);
    digitalWrite(negL,LOW);   
}  
void back(byte RmotorSpeed, byte LmotorSpeed)
{
    analogWrite(posR,RmotorSpeed);     //右輪反轉 
    digitalWrite(negR,LOW);            //右輪反轉  
    analogWrite(posL,255-LmotorSpeed);
    digitalWrite(negL,HIGH);   
}
  
void stop1()
{
    analogWrite(posR,0);    
    digitalWrite(negR,LOW);    
    analogWrite(posL,0);
    digitalWrite(negL,LOW);   
}  

void right(byte RmotorSpeed, byte LmotorSpeed)
{
    analogWrite(posR,255-RmotorSpeed);  //右輪反轉 
    digitalWrite(negR,HIGH);            //右輪反轉  
    analogWrite(posL,255-LmotorSpeed);
    digitalWrite(negL,HIGH);   
}  

void left(byte RmotorSpeed, byte LmotorSpeed)
{
    analogWrite(posR,RmotorSpeed+25);  //右輪反轉 
    digitalWrite(negR,LOW);         //右輪反轉  
    analogWrite(posL,LmotorSpeed+5);
    digitalWrite(negL,LOW);   
}  

void FL(byte RmotorSpeed, byte LmotorSpeed)
{
    analogWrite(posR,255-80);  //右輪反轉 
    digitalWrite(negR,HIGH);            //右輪反轉  
    analogWrite(posL,LmotorSpeed+60);
    digitalWrite(negL,LOW);   
}

void FR(byte RmotorSpeed, byte LmotorSpeed)
{
    analogWrite(posR,255-RmotorSpeed-80);  //右輪反轉 
    digitalWrite(negR,HIGH);            //右輪反轉  
    analogWrite(posL,80);
    digitalWrite(negL,LOW);   
}

void BL(byte RmotorSpeed, byte LmotorSpeed)
{
    analogWrite(posR,RmotorSpeed-60);     //右輪反轉 
    digitalWrite(negR,LOW);            //右輪反轉  
    analogWrite(posL,255-LmotorSpeed-80);
    digitalWrite(negL,HIGH);   
}

void BR(byte RmotorSpeed, byte LmotorSpeed)
{
    analogWrite(posR,RmotorSpeed+80);     //右輪反轉 
    digitalWrite(negR,LOW);            //右輪反轉  
    analogWrite(posL,255-LmotorSpeed+60);
    digitalWrite(negL,HIGH);   
}