Combining 2 VNH5019 to control 3 motors?

maksudcan · February 1, 2014, 2:11pm

Dear Pololu Forum members,

I want to combine 2 VNH5019 motor controllers to control 3 motors. I edited the original VNH5019 .cpp file and .h file to use a third motor M3. However, when I try to control the third motor, it wont do anything and when I set the speed for motor 1, it controls motor 1 and 3 and go so on.
Is there a way to combine these 2 VNH5019 motor controllers to control 3 motors?
This is the code that I edited and expanded with a Motor 3.
.cpp file:

#include "DualVNH5019MotorShield.h"

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

DualVNH5019MotorShield::DualVNH5019MotorShield()
{
  //Pin map
  _INA1 = 22;
  _INB1 = 24;
  _EN1DIAG1 = 26;
  _CS1 = A0; 
  _INA2 = 28;
  _INB2 = 30;
  _EN2DIAG2 = 32;
  _CS2 = A1;
  _INA3 = 36;
  _INB3 = 38;
  _EN3DIAG3 = 40;
  _CS1 = A2;
}

DualVNH5019MotorShield::DualVNH5019MotorShield(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)
{
  //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;
  _INA3 = INA3;
  _INB3 = INB3;
  _EN3DIAG3 = EN3DIAG3;
  _CS3 = CS3;
}

// Public Methods //////////////////////////////////////////////////////////////
void DualVNH5019MotorShield::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);
  #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
}
// Set speed for motor 1, speed is a number betwenn -400 and 400
void DualVNH5019MotorShield::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;
  #else
  analogWrite(_PWM1,speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
  #endif
  if (speed == 0)
  {
    digitalWrite(_INA1,LOW);   // Make the motor coast no
    digitalWrite(_INB1,LOW);   // matter which direction it is spinning.
  }
  else if (reverse)
  {
    digitalWrite(_INA1,LOW);
    digitalWrite(_INB1,HIGH);
  }
  else
  {
    digitalWrite(_INA1,HIGH);
    digitalWrite(_INB1,LOW);
  }
}

// Set speed for motor 2, speed is a number betwenn -400 and 400
void DualVNH5019MotorShield::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;
  #else
  analogWrite(_PWM2,speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
  #endif 
  if (speed == 0)
  {
    digitalWrite(_INA2,LOW);   // Make the motor coast no
    digitalWrite(_INB2,LOW);   // matter which direction it is spinning.
  }
  else if (reverse)
  {
    digitalWrite(_INA2,LOW);
    digitalWrite(_INB2,HIGH);
  }
  else
  {
    digitalWrite(_INA2,HIGH);
    digitalWrite(_INB2,LOW);
  }
}

// Set speed for motor 3, speed is a number betwenn -400 and 400
void DualVNH5019MotorShield::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__)
  OCR1C = speed;
  #else
  analogWrite(_PWM3,speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
  #endif 
  if (speed == 0)
  {
    digitalWrite(_INA3,LOW);   // Make the motor coast no
    digitalWrite(_INB3,LOW);   // matter which direction it is spinning.
  }
  else if (reverse)
  {
    digitalWrite(_INA3,LOW);
    digitalWrite(_INB3,HIGH);
  }
  else
  {
    digitalWrite(_INA3,HIGH);
    digitalWrite(_INB3,LOW);
  }
}

// Set speed for motor 1 and 2
void DualVNH5019MotorShield::setSpeeds(int m1Speed, int m2Speed)
{
  setM1Speed(m1Speed);
  setM2Speed(m2Speed);
}

// Set speed for motor 1 and 3
void DualVNH5019MotorShield::setSpeeds3(int m1Speed, int m3Speed)
{
  setM1Speed(m1Speed);
  setM3Speed(m3Speed);
}

// Set speed for motor 2 and 3 
void DualVNH5019MotorShield::setSpeeds2(int m2Speed, int m3Speed)
{
  setM2Speed(m2Speed);
  setM3Speed(m3Speed);
}

// Set speed for motor 1, 2 and 3 
void DualVNH5019MotorShield::setSpeedsAll(int m1Speed, int m2Speed, int m3Speed)
{
  setM1Speed(m1Speed);
  setM2Speed(m2Speed);
  setM3Speed(m3Speed);
}



// Brake motor 1, brake is a number between 0 and 400
void DualVNH5019MotorShield::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;
  #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 DualVNH5019MotorShield::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;
  #else
  analogWrite(_PWM2,brake * 51 / 80); // default to using analogWrite, mapping 400 to 255
  #endif
}

// Brake motor 3, brake is a number between 0 and 400
void DualVNH5019MotorShield::setM3Brake(int brake)
{
  // normalize brake
  if (brake < 0)
  {
    brake = -brake;
  }
  if (brake > 400)  // Max brake
    brake = 400;
  digitalWrite(_INA3, LOW);
  digitalWrite(_INB3, LOW);
  #if defined(__AVR_ATmega168__)|| defined(__AVR_ATmega328P__)
  OCR1C = brake;
  #else
  analogWrite(_PWM3,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 DualVNH5019MotorShield::setBrakes(int m1Brake, int m2Brake)
{
  setM1Brake(m1Brake);
  setM2Brake(m2Brake);
}

// Brake motor 2 and 3, brake is a number between 0 and 400
void DualVNH5019MotorShield::setBrakes2(int m2Brake, int m3Brake)
{
  setM2Brake(m2Brake);
  setM3Brake(m3Brake);
}

// Brake motor 1 and 3, brake is a number between 0 and 400
void DualVNH5019MotorShield::setBrakes3(int m1Brake, int m3Brake)
{
  setM1Brake(m1Brake);
  setM3Brake(m3Brake);
}

// Brake motor 1, 2 and 3, brake is a number between 0 and 400
void DualVNH5019MotorShield::setBrakesAll(int m1Brake, int m2Brake, int m3Brake)
{
  setM1Brake(m1Brake);
  setM2Brake(m2Brake);
  setM3Brake(m3Brake);
}

// Return motor 1 current value in milliamps.
unsigned int DualVNH5019MotorShield::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 DualVNH5019MotorShield::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 DualVNH5019MotorShield::getM3CurrentMilliamps()
{
  // 5V / 1024 ADC counts / 144 mV per A = 34 mA per count
  return analogRead(_CS3) * 34;
}


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

// Return error status for motor 2 
unsigned char DualVNH5019MotorShield::getM2Fault()
{
  return !digitalRead(_EN2DIAG2);
}

// Return error status for motor 3 
unsigned char DualVNH5019MotorShield::getM3Fault()
{
  return !digitalRead(_EN3DIAG3);
}

.h file

#ifndef DualVNH5019MotorShield_h
#define DualVNH5019MotorShield_h

#include <Arduino.h>

class DualVNH5019MotorShield
{
  public:  
    // CONSTRUCTORS
    DualVNH5019MotorShield(); // Default pin selection.
    DualVNH5019MotorShield(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 EN2DIAG3, unsigned char CS3);						   // User-defined pin selection. 
    
    // PUBLIC METHODS
    void init(); // Initialize TIMER 1, set the PWM to 20kHZ. 
    void setM1Speed(int speed); // Set speed for M1.
    void setM2Speed(int speed); // Set speed for M2.
	void setM3Speed(int speed); // Set speed for M2.
    void setSpeeds(int m1Speed, int m2Speed); // Set speed for both M1 and M2.
	void setSpeeds2(int m2Speed, int m3Speed); // Set speed for both M1 and M2.
	void setSpeeds3(int m1Speed, int m3Speed); // Set speed for both M1 and M2.
	void setSpeedsAll(int m1Speed, int m2Speed, int m3Speed); // Set speed for both M1 and M2.
    void setM1Brake(int brake); // Brake M1. 
    void setM2Brake(int brake); // Brake M2.
	void setM3Brake(int brake); // Brake M2.
    void setBrakes(int m1Brake, int m2Brake); // Brake both M1 and M2.
	void setBrakes2(int m2Brake, int m3Brake); // Brake both M1 and M2.
	void setBrakes3(int m1Brake, int m3Brake); // Brake both M1 and M2.
	void setBrakesAll(int m1Brake, int m2Brake, int m3Brake); // Brake both M1 and M2.
    unsigned int getM1CurrentMilliamps(); // Get current reading for M1. 
    unsigned int getM2CurrentMilliamps(); // Get current reading for M2.
	unsigned int getM3CurrentMilliamps(); // Get current reading for M2.
    unsigned char getM1Fault(); // Get fault reading from M1.
    unsigned char getM2Fault(); // Get fault reading from M2.
	unsigned char getM3Fault(); // Get fault reading from M2.
    
  private:
    unsigned char _INA1;
    unsigned char _INB1;
    static const unsigned char _PWM1 = 2;
    unsigned char _EN1DIAG1;
    unsigned char _CS1;
    unsigned char _INA2;
    unsigned char _INB2;
    static const unsigned char _PWM2 = 3;
    unsigned char _EN2DIAG2;
    unsigned char _CS2;
	unsigned char _INA3;
    unsigned char _INB3;
    static const unsigned char _PWM3 = 4;
    unsigned char _EN3DIAG3;
    unsigned char _CS3;
    
};

#endif

Can you guys help me out?

JeremyT · February 4, 2014, 12:22am

Hello.

I am sorry you are having problems driving your motors. Could you tell me about your setup? Which VNH5019 board and Arduino are you using? Could you clarify what happens when you set the speed for motor 1? Do motors 1 and 3 behave the same? Could you post pictures of your setup? Could you also post a diagram that shows how everything is connected?

Instead of using our library, you might try setting the speed for motor 3 by calling analogWrite() in your sketch.

- Jeremy

maksudcan · February 8, 2014, 2:23pm

Hi,

Thank you for replying, I solved the problem by changing some cables, they were not correctly put into the Arduino.

Greetings,

Maksud

JeremyT · February 10, 2014, 11:20pm

Thanks for letting us know. I am glad you got it working. Did you have to make any additional changes to the library?

- Jeremy