Bluetooth Controlled PBOT Jr. Via Android Phone

JR_BT_ANDROID.ino

//*********************************************//
//                   P_BOT_JR                  //
//               JR_ BT_ANDROID                //
//                                             //
//  This enables a bot to avoid obstacles and  //
//          may also solve mazes.              //
//               Codes by:                     //
//        e-Gizmo Mechatronix Central          //
//         Taft, Manila, Philippines           //
//           http://www.egizmo.com             //
//                May 15,2016                  //
//*********************************************//

// INITIALIZATION:
#include <Servo.h>
Servo myservo;
int POS = 0;
int BUZZER = 12;
byte INPUT_FROM_ANDROID_APP;

int MOTOR_1_DIRECTION =  8;                // Motor control HIGH = FWD, LOW = BWD
int MOTOR_1_SPEED =      9;                // Speed input ( digitalWrite  LOW or 0 ) means fullspeed.
int MOTOR_2_SPEED =      10;               // Speed input ( digitalWrite  HIGH or 1 ) means fullstop.
int MOTOR_2_DIRECTION =  11;               // Motor control HIGH = FWD, LOW = BWD 

int NORMAL_SPEED = 150;                    // 255 means fullstop on analog read,
int FULL_STOP = 255;                       // 0 means fullspeed,feel free to play analog values from 0-255

// INPUTS AND OUTPUTS
void setup()
{
  Serial.begin(9600); 
  myservo.attach(6);

  pinMode(MOTOR_1_DIRECTION, OUTPUT);
  pinMode(MOTOR_1_SPEED, OUTPUT);
  pinMode(MOTOR_2_DIRECTION, OUTPUT);
  pinMode(MOTOR_2_SPEED, OUTPUT);
  pinMode(BUZZER, OUTPUT);
  myservo.write(0);
}

// MAIN PROGRAM
void loop() {
  if (Serial.available() > 0) {
    INPUT_FROM_ANDROID_APP = Serial.read();
    Serial.println(INPUT_FROM_ANDROID_APP);
  }
  if (INPUT_FROM_ANDROID_APP==1){
    FORWARD();
  }
  if (INPUT_FROM_ANDROID_APP==2){
    BACKWARD ();
  }
  if (INPUT_FROM_ANDROID_APP==3){
    LEFT_TURN();
  }
  if (INPUT_FROM_ANDROID_APP==4){
    RIGHT_TURN();
  }
  if (INPUT_FROM_ANDROID_APP==0){
    STOP ();
  }
  if (INPUT_FROM_ANDROID_APP==5){
    myservo.write(90);
    delay(15);
  }
  if (INPUT_FROM_ANDROID_APP==55){
    myservo.write(0);
    delay(15);
  }
  if (INPUT_FROM_ANDROID_APP==7){
    digitalWrite(BUZZER,HIGH);
  }
  if (INPUT_FROM_ANDROID_APP==77){
    digitalWrite(BUZZER,LOW);
  }
}

// PRESET FUNCTIONS 
void FORWARD()    
{
  digitalWrite(MOTOR_1_DIRECTION,HIGH);
  digitalWrite(MOTOR_2_DIRECTION,HIGH);
  analogWrite(MOTOR_1_SPEED,NORMAL_SPEED);
  analogWrite(MOTOR_2_SPEED,NORMAL_SPEED);
}
void STOP()    
{
  analogWrite(MOTOR_1_SPEED,FULL_STOP);
  analogWrite(MOTOR_2_SPEED,FULL_STOP);
  delay(10);
}
void BACKWARD()    
{
  digitalWrite(MOTOR_1_DIRECTION,LOW);
  digitalWrite(MOTOR_2_DIRECTION,LOW);
  analogWrite(MOTOR_1_SPEED,NORMAL_SPEED);
  analogWrite(MOTOR_2_SPEED,NORMAL_SPEED);
  delay(10);
}
void RIGHT_TURN()    
{
  digitalWrite(MOTOR_1_DIRECTION,HIGH);
  digitalWrite(MOTOR_2_DIRECTION,LOW);
  analogWrite(MOTOR_1_SPEED,NORMAL_SPEED);
  analogWrite(MOTOR_2_SPEED,255);
  delay(10);
}
void LEFT_TURN()    
{
  digitalWrite(MOTOR_1_DIRECTION,LOW);
  digitalWrite(MOTOR_2_DIRECTION,HIGH);
  analogWrite(MOTOR_1_SPEED,255);
  analogWrite(MOTOR_2_SPEED,NORMAL_SPEED);
  delay(10);
}
void KICK()
{
  myservo.write(90);
  delay(15);
  myservo.write(0); 
}  
 // PROGRAM ENDS AND LOOP BACK FOREVER                                                                                                                                                   // PARZ