//*********************************************//
//                                             //
//             MINIBOT_BT_CONTROLLED           //
//                                             //
//     This demo program will  controll the    //
// minibot using android phone via bluetooth.  //
//                                             //
//                Codes by:                    //
//        e-Gizmo Mechatronix Central          //
//         Taft, Manila, Philippines           //
//           http://www.egizmo.com             //
//                May 15,2016                  //
//*********************************************//

// INITIALIZATION:
#include <Servo.h>
byte INPUT_FROM_ANDROID_APP;

#define MOTOR_1_DIRECTION         8       // Motor control HIGH = FWD, LOW = BWD
#define MOTOR_1_SPEED             9       // Speed input
#define MOTOR_2_SPEED            10       // Speed input
#define MOTOR_2_DIRECTION        11       // Motor control HIGH = FWD, LOW = BWD    

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

  pinMode(MOTOR_1_DIRECTION, OUTPUT);
  pinMode(MOTOR_1_SPEED, OUTPUT);
  pinMode(MOTOR_2_DIRECTION, OUTPUT);
  pinMode(MOTOR_2_SPEED, OUTPUT);
}

// 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==19){
    FORWARD();
  }
  if (INPUT_FROM_ANDROID_APP==20){
    BACKWARD ();
  }
  if (INPUT_FROM_ANDROID_APP==22){
    SPIN_LEFT();
  }
  if (INPUT_FROM_ANDROID_APP==21){
    SPIN_RIGHT();
  }
  if (INPUT_FROM_ANDROID_APP==0){
    STOP ();
  }
}

// PRERSET FUNCTIONS
void FORWARD()
{
  digitalWrite(MOTOR_1_DIRECTION,HIGH);
  digitalWrite(MOTOR_2_DIRECTION,HIGH);
  digitalWrite(MOTOR_1_SPEED, HIGH);
  digitalWrite(10, HIGH);
  delay(100);
}   
void BACKWARD()
{
  digitalWrite(MOTOR_1_DIRECTION, LOW); 
  digitalWrite(MOTOR_1_SPEED, HIGH);
  digitalWrite(MOTOR_2_SPEED, HIGH);
  digitalWrite(MOTOR_2_DIRECTION, LOW);
  delay(100);
}
void SPIN_LEFT()
{
  digitalWrite(MOTOR_1_DIRECTION, HIGH); 
  digitalWrite(MOTOR_1_SPEED, HIGH);
  digitalWrite(MOTOR_2_SPEED, HIGH);
  digitalWrite(MOTOR_2_DIRECTION, LOW);
  delay(100);
}
void SPIN_RIGHT()
{
  digitalWrite(MOTOR_1_DIRECTION, LOW); 
  digitalWrite(MOTOR_1_SPEED, HIGH);
  digitalWrite(MOTOR_2_SPEED, HIGH);
  digitalWrite(MOTOR_2_DIRECTION, HIGH);
  delay(100);
}
void STOP()
{
  analogWrite(MOTOR_1_SPEED, 0);
  analogWrite(MOTOR_2_SPEED, 0); 
} 
 // PROGRAM ENDS AND LOOP BACK FOREVER                                                                                                                                                   // PARZ