#include "DualVNH5019MotorShieldMegaSeveralCards.h"

// Constructors ////////////////////////////////////////////////////////////////

DualVNH5019MotorShieldMegaSeveralCards::DualVNH5019MotorShieldMegaSeveralCards()
{
  //Pin map
  _INA1 = 41;
  _INB1 = 40;
  _EN1DIAG1 = 38;
  _CS1 = A2; 
  _INA2 = 42;
  _INB2 = 47;
  _EN2DIAG2 = 43;
  _CS2 = A3;
  
   //Pin map2
   _INA3 = 48;
   _INB3 = 51;
   _EN3DIAG3 = 49;
   _CS3 = A0;
   _INA4 = 53;
   _INB4 = 52;
   _EN4DIAG4 = 50;
   _CS4 = A1;
   
    //Pin map3
    _INA5 = 35;
    _INB5 = 34;
    _EN5DIAG5 = 32;
    _CS5 = A5;
    _INA6 = 36;
    _INB6 = 39;
    _EN6DIAG6 = 37;
    _CS6 = A4;
  
  
  
  
  
  
  
}

DualVNH5019MotorShieldMegaSeveralCards::DualVNH5019MotorShieldMegaSeveralCards(     unsigned char INA1, unsigned char INB1, unsigned char EN1DIAG1, unsigned char CS1, 
                                               unsigned char INA2, unsigned char INB2, unsigned char EN2DIAG2, unsigned char CS2,
											   unsigned char INA3, unsigned char INB3, unsigned char EN3DIAG3, unsigned char CS3,
											   unsigned char INA4, unsigned char INB4, unsigned char EN4DIAG4, unsigned char CS4,
											   unsigned char INA5, unsigned char INB5, unsigned char EN5DIAG5, unsigned char CS5,
											   unsigned char INA6, unsigned char INB6, unsigned char EN6DIAG6, unsigned char CS6)
{
  //Pin map
  //PWM1 and PWM2 cannot be remapped because the library assumes PWM is on timer1
  _INA1 = INA1;
  _INB1 = INB1;
  _EN1DIAG1 = EN1DIAG1;
  _CS1 = CS1;
  _INA2 = INA2;
  _INB2 = INB2;
  _EN2DIAG2 = EN2DIAG2;
  _CS2 = CS2;
    //Pin map2
    _INA3 = INA3;
    _INB3 = INB3;
    _EN3DIAG3 = EN3DIAG3;
    _CS3 = CS3;
    _INA4 = INA4;
    _INB4 = INB4;
    _EN4DIAG4 = EN4DIAG4;
    _CS4 = CS4;
    
    //Pin map3
    _INA5 = 35;
    _INB5 = 34;
    _EN5DIAG5 = 32;
    _CS5 = A5;
    _INA6 = 36;
    _INB6 = 39;
    _EN6DIAG6 = 37;
    _CS6 = A4;
}

// Public Methods //////////////////////////////////////////////////////////////
void DualVNH5019MotorShieldMegaSeveralCards::init()
{
// Define pinMode for the pins and set the frequency for timer1.

  pinMode(_INA1,OUTPUT);
  pinMode(_INB1,OUTPUT);
  pinMode(_PWM1,OUTPUT);
  pinMode(_EN1DIAG1,INPUT);
  pinMode(_CS1,INPUT);
  pinMode(_INA2,OUTPUT);
  pinMode(_INB2,OUTPUT);
  pinMode(_PWM2,OUTPUT);
  pinMode(_EN2DIAG2,INPUT);
  pinMode(_CS2,INPUT);
  
  pinMode(_INA3,OUTPUT);
  pinMode(_INB3,OUTPUT);
  pinMode(_PWM3,OUTPUT);
  pinMode(_EN3DIAG3,INPUT);
  pinMode(_CS3,INPUT);
  pinMode(_INA4,OUTPUT);
  pinMode(_INB4,OUTPUT);
  pinMode(_PWM4,OUTPUT);
  pinMode(_EN4DIAG4,INPUT);
  pinMode(_CS4,INPUT);
  
  pinMode(_INA5,OUTPUT);
  pinMode(_INB5,OUTPUT);
  pinMode(_PWM5,OUTPUT);
  pinMode(_EN5DIAG5,INPUT);
  pinMode(_CS5,INPUT);
  pinMode(_INA6,OUTPUT);
  pinMode(_INB6,OUTPUT);
  pinMode(_PWM6,OUTPUT);
  pinMode(_EN6DIAG6,INPUT);
  pinMode(_CS6,INPUT);
  #if defined(__AVR_ATmega168__)|| defined(__AVR_ATmega328P__)
  // Timer 1 configuration
  // prescaler: clockI/O / 1
  // outputs enabled
  // phase-correct PWM
  // top of 400
  //
  // PWM frequency calculation
  // 16MHz / 1 (prescaler) / 2 (phase-correct) / 400 (top) = 20kHz
  TCCR1A = 0b10100000; 
 TCCR1B = 0b00010001; 
 ICR1 = 400;
 
  
  #endif
  #if defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
 //  Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
  TCCR1A = 0b10100000; // Mega pin 5
  TCCR1B = 0b00010001; // Mega pin 3
  ICR1 = 400;
     
  #endif  
}
// Set speed for motor 1, speed is a number between -400 and 400
void DualVNH5019MotorShieldMegaSeveralCards::setM1Speed(int speed)
{
  unsigned char reverse = 0;
  
  if (speed < 0)
  {
    speed = -speed;  // Make speed a positive quantity
    reverse = 1;  // Preserve the direction
  }
  if (speed > 400)  // Max PWM dutycycle
    speed = 400;
  #if defined(__AVR_ATmega168__)|| defined(__AVR_ATmega328P__)
  OCR1A = speed;
  #elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  // Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
  OCR1A = speed;
  #else
  analogWrite(_PWM1,speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
  #endif
  if (reverse)
  {
    digitalWrite(_INA1,LOW);
    digitalWrite(_INB1,HIGH);
  }
  else
  {
    digitalWrite(_INA1,HIGH);
    digitalWrite(_INB1,LOW);
  }
}

// Set speed for motor 2, speed is a number between -400 and 400
void DualVNH5019MotorShieldMegaSeveralCards::setM2Speed(int speed)
{
  unsigned char reverse = 0;
  
  if (speed < 0)
  {
    speed = -speed;  // make speed a positive quantity
    reverse = 1;  // preserve the direction
  }
  if (speed > 400)  // Max 
    speed = 400;
  #if defined(__AVR_ATmega168__)|| defined(__AVR_ATmega328P__)
  OCR1B = speed;
  #elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  // Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
  OCR1B = speed;
  #else
  analogWrite(_PWM2,speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
  #endif 
  if (reverse)
  {
    digitalWrite(_INA2,LOW);
    digitalWrite(_INB2,HIGH);
  }
  else
  {
    digitalWrite(_INA2,HIGH);
    digitalWrite(_INB2,LOW);
  }
}
// Set speed for motor 3, speed is a number between -400 and 400
void DualVNH5019MotorShieldMegaSeveralCards::setM3Speed(int speed)
{
	unsigned char reverse = 0;
	
	if (speed < 0)
	{
		speed = -speed;  // make speed a positive quantity
		reverse = 1;  // preserve the direction
	}
	if (speed > 400)  // Max
	speed = 400;
	#if defined(__AVR_ATmega168__)|| defined(__AVR_ATmega328P__)
	OCR1B = speed;
	#elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
	// Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
	OCR1B = speed;
	#else
	analogWrite(_PWM3,speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
	#endif
	if (reverse)
	{
		digitalWrite(_INA3,LOW);
		digitalWrite(_INB3,HIGH);
	}
	else
	{
		digitalWrite(_INA3,HIGH);
		digitalWrite(_INB3,LOW);
	}
}
// Set speed for motor 4, speed is a number between -400 and 400
void DualVNH5019MotorShieldMegaSeveralCards::setM4Speed(int speed)
{
	unsigned char reverse = 0;
	
	if (speed < 0)
	{
		speed = -speed;  // make speed a positive quantity
		reverse = 1;  // preserve the direction
	}
	if (speed > 400)  // Max
	speed = 400;
	#if defined(__AVR_ATmega168__)|| defined(__AVR_ATmega328P__)
	OCR1B = speed;
	#elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
	// Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
	OCR1B = speed;
	#else
	analogWrite(_PWM4,speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
	#endif
	if (reverse)
	{
		digitalWrite(_INA4,LOW);
		digitalWrite(_INB4,HIGH);
	}
	else
	{
		digitalWrite(_INA4,HIGH);
		digitalWrite(_INB4,LOW);
	}
}
// Set speed for motor 5, speed is a number between -400 and 400
void DualVNH5019MotorShieldMegaSeveralCards::setM5Speed(int speed)
{
	unsigned char reverse = 0;
	
	if (speed < 0)
	{
		speed = -speed;  // make speed a positive quantity
		reverse = 1;  // preserve the direction
	}
	if (speed > 400)  // Max
	speed = 400;
	#if defined(__AVR_ATmega168__)|| defined(__AVR_ATmega328P__)
	OCR1B = speed;
	#elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
	// Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
	OCR1B = speed;
	#else
	analogWrite(_PWM5,speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
	#endif
	if (reverse)
	{
		digitalWrite(_INA5,LOW);
		digitalWrite(_INB5,HIGH);
	}
	else
	{
		digitalWrite(_INA5,HIGH);
		digitalWrite(_INB5,LOW);
	}
}
// Set speed for motor 6, speed is a number betwenn -400 and 400
void DualVNH5019MotorShieldMegaSeveralCards::setM6Speed(int speed)
{
	unsigned char reverse = 0;
	
	if (speed < 0)
	{
		speed = -speed;  // make speed a positive quantity
		reverse = 1;  // preserve the direction
	}
	if (speed > 400)  // Max
	speed = 400;
	#if defined(__AVR_ATmega168__)|| defined(__AVR_ATmega328P__)
	OCR1B = speed;
	#elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
	// Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
	OCR1B = speed;
	#else
	analogWrite(_PWM6,speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
	#endif
	if (reverse)
	{
		digitalWrite(_INA6,LOW);
		digitalWrite(_INB6,HIGH);
	}
	else
	{
		digitalWrite(_INA6,HIGH);
		digitalWrite(_INB6,LOW);
	}
}


// Set speed for motor 1 and 2
void DualVNH5019MotorShieldMegaSeveralCards::setSpeeds(int m1Speed, int m2Speed, int m3Speed, int m4Speed, int m5Speed, int m6Speed)
{
  setM1Speed(m1Speed);
  setM2Speed(m2Speed);
   setM3Speed(m3Speed);
   setM4Speed(m4Speed);
 setM5Speed(m5Speed);
 setM6Speed(m6Speed);

}

// Brake motor 1, brake is a number between 0 and 400
void DualVNH5019MotorShieldMegaSeveralCards::setM1Brake(int brake)
{
  // normalize brake
  if (brake < 0)
  {
    brake = -brake;
  }
  if (brake > 400)  // Max brake
    brake = 400;
  digitalWrite(_INA1, LOW);
  digitalWrite(_INB1, LOW);
  #if defined(__AVR_ATmega168__)|| defined(__AVR_ATmega328P__)
  OCR1A = brake;
  #elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  // Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
  OCR1A = brake;
  #else
  analogWrite(_PWM1,brake * 51 / 80); // default to using analogWrite, mapping 400 to 255
  #endif
}

// Brake motor 2, brake is a number between 0 and 400
void DualVNH5019MotorShieldMegaSeveralCards::setM2Brake(int brake)
{
  // normalize brake
  if (brake < 0)
  {
    brake = -brake;
  }
  if (brake > 400)  // Max brake
    brake = 400;
  digitalWrite(_INA2, LOW);
  digitalWrite(_INB2, LOW);
  #if defined(__AVR_ATmega168__)|| defined(__AVR_ATmega328P__)
  OCR1B = brake;
  #elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  // Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
  OCR1B = brake;
  #else
  analogWrite(_PWM2,brake * 51 / 80); // default to using analogWrite, mapping 400 to 255
  #endif
}

// Brake motor 1 and 2, brake is a number between 0 and 400
void DualVNH5019MotorShieldMegaSeveralCards::setBrakes(int m1Brake, int m2Brake)
{
  setM1Brake(m1Brake);
  setM2Brake(m2Brake);
}

// Return motor 1 current value in milliamps.
unsigned int DualVNH5019MotorShieldMegaSeveralCards::getM1CurrentMilliamps()
{
  // 5V / 1024 ADC counts / 144 mV per A = 34 mA per count
  return analogRead(_CS1) * 34;
}

// Return motor 2 current value in milliamps.
unsigned int DualVNH5019MotorShieldMegaSeveralCards::getM2CurrentMilliamps()
{
	// 5V / 1024 ADC counts / 144 mV per A = 34 mA per count
	return analogRead(_CS2) * 34;
}
// Return motor 3 current value in milliamps.
unsigned int DualVNH5019MotorShieldMegaSeveralCards::getM3CurrentMilliamps()
{
	// 5V / 1024 ADC counts / 144 mV per A = 34 mA per count
	return analogRead(_CS3) * 34;
}
// Return motor 4 current value in milliamps.
unsigned int DualVNH5019MotorShieldMegaSeveralCards::getM4CurrentMilliamps()
{
	// 5V / 1024 ADC counts / 144 mV per A = 34 mA per count
	return analogRead(_CS4) * 34;
}
// Return motor 5 current value in milliamps.
unsigned int DualVNH5019MotorShieldMegaSeveralCards::getM5CurrentMilliamps()
{
	// 5V / 1024 ADC counts / 144 mV per A = 34 mA per count
	return analogRead(_CS5) * 34;
}
// Return motor 6 current value in milliamps.
unsigned int DualVNH5019MotorShieldMegaSeveralCards::getM6CurrentMilliamps()
{
	// 5V / 1024 ADC counts / 144 mV per A = 34 mA per count
	return analogRead(_CS6) * 34;
}

// Return error status for motor 1 
unsigned char DualVNH5019MotorShieldMegaSeveralCards::getM1Fault()
{
  return !digitalRead(_EN1DIAG1);
}

// Return error status for motor 2 
unsigned char DualVNH5019MotorShieldMegaSeveralCards::getM2Fault()
{
  return !digitalRead(_EN2DIAG2);
}
// Return error status for motor 3
unsigned char DualVNH5019MotorShieldMegaSeveralCards::getM3Fault()
{
	return !digitalRead(_EN3DIAG3);
	
}// Return error status for motor 4 
unsigned char DualVNH5019MotorShieldMegaSeveralCards::getM4Fault()
{
  return !digitalRead(_EN4DIAG4);
  
}// Return error status for motor 5
unsigned char DualVNH5019MotorShieldMegaSeveralCards::getM5Fault()
{
  return !digitalRead(_EN5DIAG5);
  
}// Return error status for motor 6 
unsigned char DualVNH5019MotorShieldMegaSeveralCards::getM6Fault()
{
  return !digitalRead(_EN6DIAG6);
}