/***************************************************************
  Description: Motor driver definitions;
  Author: www.corvin.cn
  Hisotry: 20180209: init this file;
*************************************************************/
#include "motor_driver.h"

/* Maximum PWM signal */
#define MAX_PWM    255

//three motors control pin define
static const int A_IN1  = 26;
static const int A_IN2  = 28;
static const int A_PWM  = 4;   //A wheel pwm pin

static const int B_IN1  = 30;
static const int B_IN2  = 32;
static const int B_PWM  = 6;   //B wheel pwm pin

static const int C_IN1  = 24;
static const int C_IN2  = 22;
static const int C_PWM  = 5;   //C wheel pwm pin

static boolean direcA = FORWARDS;
static boolean direcB = FORWARDS;
static boolean direcC = FORWARDS;

/* Wrap the motor driver initialization,
   set all the motor control pins to outputs **/
void initMotorsPinMode(void)
{
  pinMode(A_IN1, OUTPUT);
  pinMode(A_IN2, OUTPUT);
  pinMode(A_PWM, OUTPUT);

  pinMode(B_IN1, OUTPUT);
  pinMode(B_IN2, OUTPUT);
  pinMode(B_PWM, OUTPUT);

  pinMode(C_IN1, OUTPUT);
  pinMode(C_IN2, OUTPUT);
  pinMode(C_PWM, OUTPUT);
}

/*
   get wheel run direction
*/
boolean directionWheel(int wheel)
{
  if (wheel == A_WHEEL)
  {
    return direcA;
  }
  else if (wheel == B_WHEEL)
  {
    return direcB;
  }
  else
  {
    return direcC;
  }
}

// A convenience function for setting both motor speeds
void setMotorSpeeds(int ASpeed, int BSpeed, int CSpeed)
{
  if (ASpeed > MAX_PWM)
  {
    ASpeed = MAX_PWM;
  }
  else if (ASpeed < -MAX_PWM)
  {
    ASpeed = -1 * MAX_PWM;
  }
  
  if (BSpeed > MAX_PWM)
  {
    BSpeed = MAX_PWM;
  }
  else if (BSpeed < -MAX_PWM)
  {
    BSpeed = -1 * MAX_PWM;
  }
  
  if (CSpeed > MAX_PWM)
  {
    CSpeed = MAX_PWM;
  }
  else if (CSpeed < -MAX_PWM)
  {
    CSpeed = -1 * MAX_PWM;
  }

  if (ASpeed >= 0) //drive motor A
  {
    direcA = FORWARDS;
    digitalWrite(A_IN1, LOW);
    digitalWrite(A_IN2, HIGH);
    analogWrite(A_PWM, ASpeed);
  }
  else if (ASpeed < 0)
  {
    direcA = BACKWARDS;
    digitalWrite(A_IN1, HIGH);
    digitalWrite(A_IN2, LOW);
    analogWrite(A_PWM, -ASpeed);
  }

  if (BSpeed >= 0) //drive motor B
  {
    direcB = FORWARDS;
    digitalWrite(B_IN1, LOW);
    digitalWrite(B_IN2, HIGH);
    analogWrite(B_PWM, BSpeed);
  }
  else if (BSpeed < 0)
  {
    direcB = BACKWARDS;
    digitalWrite(B_IN1, HIGH);
    digitalWrite(B_IN2, LOW);
    analogWrite(B_PWM, -BSpeed);
  }

  if (CSpeed >= 0) //drive motor C
  {
    direcC = FORWARDS;
    digitalWrite(C_IN1, LOW);
    digitalWrite(C_IN2, HIGH);
    analogWrite(C_PWM, CSpeed);
  }
  else if (CSpeed < 0)
  {
    direcC = BACKWARDS;
    digitalWrite(C_IN1, HIGH);
    digitalWrite(C_IN2, LOW);
    analogWrite(C_PWM, -CSpeed);
  }
}