/*     ----------------------------------------
 *     |        Joel's Ardu-bot code          |
 *     ----------------------------------------
 *
 */

int standbyPin = 12; 
int leftMotor1 = 2;       // correspond to A01 and A02 on TB6612FNG
int leftMotor2 = 3;
int leftMotorPWM = 9;     // control speed of left motor
int rightMotor1 = 4;      // correspond to B01 and B02 on TB6612FNG
int rightMotor2 = 5;
int rightMotorPWM = 10;   // control speed of right motor

/*
 * setup() - this function runs once when you turn your Arduino on
 * We set the motors pin to be an output (turning the pin high (+5v) or low (ground) (-))
 * rather than an input (checking whether a pin is high or low)
 */
void setup()
{
  pinMode(standbyPin, OUTPUT);
  pinMode(leftMotor1, OUTPUT);
  pinMode(leftMotor2, OUTPUT);
  pinMode(leftMotorPWM, OUTPUT);
  pinMode(rightMotor1, OUTPUT);
  pinMode(rightMotor2, OUTPUT);
  pinMode(rightMotorPWM, OUTPUT);
  wakeUp();
  delay(3000);
}


/*
 * loop() - this function will start after setup finishes and then repeat
 * we call a few functions to test the contrtols
 */

void loop()                     // run over and over again
{
  widdershins();  // spin left
  delay(2000);    // wait a bit
  allStop();
  delay(1000);
  deosil();       // spin right
  delay(2000);    // wait a bit
  allStop();
  
  delay(5000);
}

/*
 * widdershins() - spins the 'bot counter-clockwise
 * left wheel backward, right wheel forward
 */
void widdershins(){
  leftBackward();
  rightForward();
}
 
/*
 * deosil() - spins the 'bot clockwise
 * left wheel forward, right wheel backward
 */
void deosil(){
  leftForward();
  rightBackward();
}

/*
 * shortBrake() - stops the 'bot!
 * 
 */
void shortBrake(){
  leftShortBrake();
  rightShortBrake();
}

/*
 * leftForward() - left wheel forward
 * 
 */
void leftForward(){
  digitalWrite(leftMotor1, LOW);
  digitalWrite(leftMotor2, HIGH);
  digitalWrite(leftMotorPWM, HIGH);
}

/*
 * rightForward() - right wheel forward
 * 
 */
void rightForward(){
  digitalWrite(rightMotor1, HIGH);
  digitalWrite(rightMotor2, LOW);
  digitalWrite(rightMotorPWM, HIGH);
}

/*
 * leftBackward() - left wheel backward
 * 
 */
void leftBackward(){
  digitalWrite(leftMotor1, HIGH);
  digitalWrite(leftMotor2, LOW);
  digitalWrite(leftMotorPWM, HIGH);
}

/*
 * rightBackward() - right wheel backward
 * 
 */
void rightBackward(){
  digitalWrite(rightMotor1, LOW);
  digitalWrite(rightMotor2, HIGH);
  digitalWrite(rightMotorPWM, HIGH);
}

/*
 * allStop() - both wheels stop
 *
 */
void allStop(){
  leftStop();
  rightStop();
}

/*
 * leftStop() - left wheel stop
 * 
 */
void leftStop(){
  digitalWrite(leftMotor1, LOW);
  digitalWrite(leftMotor2, LOW);
  digitalWrite(leftMotorPWM, HIGH);
}

/*
 * rightStop() - right wheel stop
 * 
 */
void rightStop(){
  digitalWrite(rightMotor1, LOW);
  digitalWrite(rightMotor2, LOW);
  digitalWrite(rightMotorPWM, HIGH);
}

/*
 * leftShortBrake() - left wheel short brake
 * 
 */
void leftShortBrake(){
  digitalWrite(leftMotor1, HIGH);
  digitalWrite(leftMotor2, HIGH);
  digitalWrite(leftMotorPWM, LOW);
}

/*
 * rightShortBrake() - right wheel short brake
 * 
 */
void rightShortBrake(){
  digitalWrite(rightMotor1, HIGH);
  digitalWrite(rightMotor2, HIGH);
  digitalWrite(rightMotorPWM, LOW);
}

/*
 * standbyMode() - puts motor driver in standby mode
 *
 */
void standbyMode(){
  digitalWrite(standbyPin, LOW);
}

/*
 * wakeUp() - wakes motor driver from standby mode
 *
 */
void wakeUp(){
  leftStop();
  rightStop();
  digitalWrite(standbyPin, HIGH);
}