// For Motor Driver L298N
#include <SoftwareSerial.h>
SoftwareSerial mySerial(12, 13); // TX,RX
#define MR1 3  // IN1 
#define MR2 5  // IN2 
#define ML1 9  // IN4 
#define ML2 10 // IN3 

/*
int MR1 = 3;  // IN1
int MR2 = 5;  // IN2
int ML1 = 9;  // IN4
int ML2 = 10; // IN3
*/

int data=0;
int Speed=0;
boolean maju=true;
int kec[11]={0,80,100,120,140,160,180,200,220,240,255}; //array kecepatan

void setup(){
  mySerial.begin(9600); 
  pinMode(MR1,OUTPUT);
  pinMode(MR2,OUTPUT);
  pinMode(ML1,OUTPUT);
  pinMode(ML2,OUTPUT);
}

void motorOut(unsigned char lpwm, unsigned char rpwm, boolean arrow){

  if(arrow==false){
    digitalWrite(ML1,HIGH);
    digitalWrite(MR1,LOW);
    analogWrite(ML2,255-lpwm);
    analogWrite(MR2,rpwm);
    }
  else{
    digitalWrite(ML1,LOW);
    digitalWrite(MR1,HIGH);
    analogWrite(ML2,lpwm);
    analogWrite(MR2,255-rpwm);
    }
}

void loop(){
  
  /* Commands/Characters sent from APP Bluetooth RC Controller (ANDROID)
  Forward ->F
  Back  -> B
  Left  -> L
  Right -> R
  Forward Left  -> G
  Forward Righ  -> I
  Back Left -> H
  Back Right  -> J
  Stop  -> S
  Speed 10  -> 1
  Speed 20  -> 2
  Speed 30  -> 3
  Speed 40  -> 4
  Speed 50  -> 5
  Speed 60  -> 6
  Speed 70  -> 7
  Speed 80  -> 8
  Speed 90  -> 9
  Speed 100 -> q
  Stop All  -> D */
  
  if(mySerial.available()>0){
    data=mySerial.read();
    
    //penyimpan data kecepatan
    if (data =='0') { Speed=0; }
    else if (data =='1') { Speed=1;}
    else if (data =='2') { Speed=2;}
    else if (data =='3') { Speed=3;}
    else if (data =='4') { Speed=4;}
    else if (data =='5') { Speed=5;}
    else if (data =='6') { Speed=6;}
    else if (data =='7') { Speed=7;}
    else if (data =='8') { Speed=8;}
    else if (data =='9') { Speed=9;}
    else if (data =='q') { Speed=10;}
    
    if (data=='S')
    {
      motorOut(0,0,false);
    } // S=Stop
    
    if (data=='F')
    {
      motorOut(kec[Speed],kec[Speed],true);
    } // F=Maju
    
    if (data=='I')
    {
      motorOut(kec[Speed],((kec[Speed])/2),true);
    } // I=Maju sambil belok kanan
    
    if (data=='G')
    {
      motorOut(((kec[Speed])/2),kec[Speed],true);
    } // G=Maju sambil belok kiri
    
    if (data=='R')
    {
      motorOut(kec[Speed],0,true);
    } // R=Belok kanan
    
    if (data=='L')
    {
      motorOut(0,kec[Speed],true);
    } // L=Belok kiri
    
    if (data=='B')
    {
      motorOut(kec[Speed],kec[Speed],false);
    } // B=Mundur
    
    if (data=='H')
    {
      motorOut(((kec[Speed])/2),kec[Speed],false);
    } // H=Mundur sambil belok kiri
    
    if (data=='J')
    {
      motorOut(kec[Speed],((kec[Speed])/2),false);
    } // J=Mundur sambil belok kanan
  }
}