//*********************************************//
//                   MINIBOT                   //
//                 MOTOR TEST                  //
//          this cde is for papa bear          //
//                  and nong                   //
//                 Codes by:                   //
//        e-Gizmo Mechatronix Central          //
//         Taft, Manila, Philippines           //
//           http://www.egizmo.com             //
//                May 15,2016                  //
//*********************************************//

// INITIALIZATION:
#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

int NORMAL_SPEED = 100; 
int PAUSE = 3000;
int PAUSED = 500;

// INPUTS AND OUTPUTS
void setup() 
{  
  // DEFAULT PINS OF THE MOTOR DRIVER (8,9,10,11)
  pinMode(MOTOR_1_DIRECTION, OUTPUT);       
  pinMode(MOTOR_1_SPEED, OUTPUT);
  pinMode(MOTOR_2_SPEED, OUTPUT);
  pinMode(MOTOR_2_DIRECTION, OUTPUT);
} 
// MAIN PROGRAM
void loop()  {
  FORWARD();
  delay(PAUSE);
  STOP();
  delay(PAUSED);
  BACKWARD();
  delay(PAUSE);
  STOP();
  delay(PAUSED);

  RIGHT_TURN();
  delay(PAUSE);
  STOP();
  delay(PAUSED);

  LEFT_TURN();
  delay(PAUSE);
  STOP();
  delay(PAUSED);
}

// 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()    
{
  digitalWrite(MOTOR_1_DIRECTION,LOW);
  digitalWrite(MOTOR_2_DIRECTION,LOW);
  analogWrite(MOTOR_1_SPEED,0);
  analogWrite(MOTOR_2_SPEED,0);
}

void BACKWARD()    
{
  digitalWrite(MOTOR_1_DIRECTION,LOW);
  digitalWrite(MOTOR_2_DIRECTION,LOW);
  analogWrite(MOTOR_1_SPEED,NORMAL_SPEED);
  analogWrite(MOTOR_2_SPEED,NORMAL_SPEED);
}

void RIGHT_TURN()    
{
  digitalWrite(MOTOR_1_DIRECTION,HIGH);
  digitalWrite(MOTOR_2_DIRECTION,LOW);
  analogWrite(MOTOR_1_SPEED,NORMAL_SPEED);
  analogWrite(MOTOR_2_SPEED,1);
}

void LEFT_TURN()    
{
  digitalWrite(MOTOR_1_DIRECTION,LOW);
  digitalWrite(MOTOR_2_DIRECTION,HIGH);
  analogWrite(MOTOR_1_SPEED,1);
  analogWrite(MOTOR_2_SPEED,NORMAL_SPEED);
}
// PROGRAM ENDS AND LOOP BACK FOREVER                                                                                                                                                   // PARZ