Modified VNH5019 shield library for 20kHz PWM with Mega

A number of people have been asking for a version of the DualVNH5019MotorShield library that can do 20kHz PWM frequencies with the Arduino Mega. To address this, we have made a modified version of our DualVNH5019MotorShield library that lets you use the Dual VNH5019 Motor Driver Shield with an Arduino Mega to drive motors with a 20kHz PWM. Using the 20kHz PWM should eliminate the whining sound heard when PWMing at lower frequencies, like the Arduino’s standard PWM frequency of 500Hz. This library uses pins 11 and 12 of the Arduino, which are connected to one of the Mega’s 16bit timers, timer 1.

To use this library you will need to either make some hardware modifications, or use the driver without plugging it in as a shield. As mentioned above, M1PWM and M2PWM were moved to pins 11 and 12 respectively. In addition, M2EN/DIAG was moved from pin 12 on the Arduino to pin 5. All other connections between the board and shield are the same.

Hardware modifications can be made by cutting traces on the shield and running a wire from a different Arduino pin to the appropriate trace, as explained in the user’s guide.

The new version of the library should be compatible with code written to use the old version, but the include statement will need to be changed.

DualVNH5019MotorShieldMega.h

#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); // User-defined pin selection. 
    
    // PUBLIC METHODS
    void init(); // Initialize TIMER 1 (or timer 3 for mega) set the PWM to 20kHZ. 
    void setM1Speed(int speed); // Set speed for M1.
    void setM2Speed(int speed); // Set speed for M2.
    void setSpeeds(int m1Speed, int m2Speed); // Set speed for both M1 and M2.
    void setM1Brake(int brake); // Brake M1. 
    void setM2Brake(int brake); // Brake M2.
    void setBrakes(int m1Brake, int m2Brake); // Brake both M1 and M2.
    unsigned int getM1CurrentMilliamps(); // Get current reading for M1. 
    unsigned int getM2CurrentMilliamps(); // Get current reading for M2.
    unsigned char getM1Fault(); // Get fault reading from M1.
    unsigned char getM2Fault(); // Get fault reading from M2.
    
  private:
    unsigned char _INA1;
    unsigned char _INB1;
  #if defined(__AVR_ATmega168__)|| defined(__AVR_ATmega328P__)
    static const unsigned char _PWM1 = 9;
    static const unsigned char _PWM2 = 10;
  #endif
  #if defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
    static const unsigned char _PWM1 = 11;
    static const unsigned char _PWM2 = 12;
  #endif  
    unsigned char _EN1DIAG1;
    unsigned char _CS1;
    unsigned char _INA2;
    unsigned char _INB2;
    unsigned char _EN2DIAG2;
    unsigned char _CS2;
    
};

#endif

DualVNH5019MotorShieldMega.cpp

#include "DualVNH5019MotorShieldMega.h"

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

DualVNH5019MotorShield::DualVNH5019MotorShield()
{
  //Pin map
  _INA1 = 2;
  _INB1 = 4;
  _EN1DIAG1 = 6;
  _CS1 = A0; 
  _INA2 = 7;
  _INB2 = 8;
  _EN2DIAG2 = 5;
  _CS2 = A1;
}

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)
{
  //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;
}

// 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);
  #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 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;
  #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 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;
  #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 1 and 2
void DualVNH5019MotorShield::setSpeeds(int m1Speed, int m2Speed)
{
  setM1Speed(m1Speed);
  setM2Speed(m2Speed);
}

// 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;
  #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 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;
  #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 DualVNH5019MotorShield::setBrakes(int m1Brake, int m2Brake)
{
  setM1Brake(m1Brake);
  setM2Brake(m2Brake);
}

// 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 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);
}

DualVNH5019MotorShieldMega.zip (3.06 KB)

1 Like

Thanks Claire.

We especially appreciate you staying back late on a Friday to sort this out.

I was just having a look myself, and it appears my previous attempt to get it working on Timer 3 (pins 3 & 5, chosen to minimise the number of track cuts and jumpers required) was foiled by some Arduino bug; I loaded this simple test code

int speedA=50;
int speedB=100;
int speedC=250;

void setup(){
//  Mega pins 5, 2 & 3 respectively for channels A, B & C
  TCCR3A = 0b10100000; 
  TCCR3B = 0b00010001; 
  ICR3 = 400;
  pinMode(2,OUTPUT);
  pinMode(3,OUTPUT);
  pinMode(5,OUTPUT);
}

void loop(){
  OCR3A = speedA;
  OCR3B = speedB;
  OCR3C = speedC;
}

and put my scope on pins 2, 3 & 5.
Pin5 shows 12.5% duty cycle, 20kHz pwm, Pin2 shows 25%, but pin 3 (timer3C) just sits on zero volts. I suspect its a shortcoming of the Arduino IDE.

OK, now I get it.

On reading up the ATmega docs at atmel.com/devices/atmega2560 … =documents, I worked out that channel C simply wan’t being enabled for pwm.
To correct this, we need to write a different magic number to timer control register A:

  TCCR3A = 0b10101000; 

This gem should allow successful running ultrasonic pwm on pins 3 & 5, which aren’t already used, so only 2 jumper wires and 2 track cuts are needed.

Thank you for the work on this. I was unable to make this work. I’m trying to use this shield and an XBee shield. I have remapped the RX/TX lines of the XBee (2,3) to (10,13) which works fine with softwareSerial. I bent the stackable header leads of 2,3,5 on the XBee out of the way. I remapped pins 11,12 to 2,5 on the motorDriver board, and bent pins 2,5 out of the way on the motorDriver board. I changed the include to be for the Mega, and placed the libraries in the Arduino library folder.

From here, I thought I’d be able to use the same commands as before (dm.setM1Speed(0)) but the motor just spins as if set to full speed. Did I miss something above?

It sounds like you did not remap the right pins for using the library given above. Could you list what each of the pins on your Arduino is connected to? Also, could you post a picture of the remapping?

-Claire

Thank you for the very fast reply. Here’s the exact remapping that I did:

The motor driver shield sits on the Mega. The Xbee shield sits on the motor driver shield.

XBee shield: Bent the through stackable header pins 2,3,5 out of the way. With solid core wire, via the stackable headers, mapped pins 10,13 to pins 2,3 on the XBee shield for RX/TX using SoftwareSerial.

MotorDriver shield: Bent through stackable header pins 2,5 out of way. With solid core wire, mapped pins 11,12 to 2,5 on the motor driver shield.

So… now…
pins 2 and 5 from the MEGA do not continue up the boards at all.
pins 2,3, and 5 do not continue up from motor driver shield to xbee shield.

I thought that this was right, as based on the libraries, the motor driver shield needs pins 2,3,5 for PWM and pins 11,12 now remap for pins 2,5.

Normally, the Xbee needs pins 2,3 for RX/TX, but those are now remapped to pins 10,13.

I’ll post a photo as soon as my phone(camera) comes back online.

To use the DualVNH5019MotorShieldMega library you should connect the M1PWM and M2PWM lines from the shield to pins 11 and 12 on the Arduino and the M2EN/DIAG line to pin 5 on the Arduino. This can be done by cutting the traces going to pins 9, 10, and 12 on the bottom of the shield and soldering a wire to connect the PWM pins to pins 11 and 12 and the M2EN/DIAG pin to pin 5 on the outside edge of the shield as described in the “Remapping the Arduino Connections” section of the user’s guide.

If you are able to post a picture of your setup, I might be able to help you further.

However, please not that this modification is only for using the modified version of our VNH5019 library that is given at the top of this thread, and you do not need to use this library to use the VNH5019 shield with an Arduino Mega. This library should only be used if you want to run your motors with a 20kHz PWM to eliminate the motor noise.

-Claire

Thank you for the explicit clarifications. I got a little confused by the thread but the original and these instructions make more sense and work just fine for me now. I wanted this ultrasonic PWM library so that I could remove the loudly audible PWM whine coming from my motors. I’ve just used simple H-bridge circuits in the past and there are nicely built functions to set the PWM frequency on Arduino PWM pins, but this motor driver shield is very handy and quite powerful. Thanks for the speedy and clear help. Cheers.

Hello,

I’m working with the VNH5019 on a Mega 1280. I’ve found that when I set the speed to 399, I only get around 62 to 64% Duty Cycle. When I set it to 400, I get 100% Duty Cycle. What am I missing? Were did my other 40% go?

Okay. Update… I did some massive testing tonight with the Library… I changed the max PWM value to 625. I ran a looping test that count from 0 to 625. The duty cycle on my oscilloscope spike at a 100% when the counter hit 400 and 401… then resumed normal progression at 402 up through 625. I’m not sure what’s magical about the 400 and 401 values for the PWM… but, what ever the reason… those 2 numbers cause the issue… I wrote some extra code to make it skip those two numbers and my program works fine now. Anyone have ANY insight into why this may be? Where did that initial 400 value come from? I find it strange that that’s the default top PWM value and that’s the value that causes the duty cycle to spike… Could they’re be something strange with the timer values and some sort of strange relationship with 20Khz ??

Thanks, Connor

Hello, Connor.

Are you getting the same behavior with both channels?

- Ben

Yes, I am. I was trying to figure out where the “400” came from to begin with… I couldn’t figure that out… But, like I said, at 399 it’s round 62-64% duty, 400 and 401 it spikes to 100%, 402 it picks back up at the 64-65% mark and is good all the way up till it hits 625 which is 100% duty again… What’s stranger is… I would think it would be 512 or 1024 for the top value for the PWM depending on if it was a 9 bit or 10 bit counter for the clock…

A top value of 400 produces a 20 kHz PWM. What frequency do you see when you look at the PWM on your scope?

- Ben

Okay, Found the problem… issues is in the following areas…

#if defined(AVR_ATmega168)|| defined(AVR_ATmega328P)
OCR1B = speed;
#elseif defined(AVR_ATmega128) || defined(AVR_ATmega1280) || defined(AVR_ATmega2560)
// Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
OCR3B = speed;
#else
analogWrite(_PWM2,speed * 51 / 80); // default to using analogWrite, mapping 400 to 255
#endif

#elseif isn’t valid, it should be #elif

also, OCR3B should be OCR1B… The elseif problem is throughout the file, but the OCR3B is only on line 116.

I think it was falling through to the analogWrite part of the code… which would work correctly up to the 400 or 401 which = 255 when rounded up. analogWrite then must put the pin in a high state regardless of the PWM value… which is why it was acting strangely.

Continuing on with the math… 625 = 398, 626 & 627 = 399 and 628 & 629 = 400… Which was the cut off value I found…

So, that explains the strange behavior. Please update your libraries to reflect the bug fixes. :slight_smile:

One think that would be nice would be a easy way to switch the motor orders… In my setup. Do to wire lengths… I had to go through and swap all the pins… and the OCR1A and OCR1B… and of course, the _PWM1 and _PWM2 have no effect with this library, as we’re not using the analogWrite function… (or shouldn’t be)

Thanks, Billy

Hi, Billy.

Thank you for pointing those mistakes out; I really appreciate it. You are right about it not recognizing the elseif statement and falling through to the else statement. I have tested the library with both of the changes you indicated and updated it in my original post. I also fixed the second code block in that post, which used to have the header file instead of the C++ file in it.

-Claire

I wouldn’t say mistakes… I would say bugs. :slight_smile: Bugs happen… I’m a magnet for them… Glad to have figured it out… it was driving me crazy. I thought I had a problem with my PID loop.

Thanks, Billy

I would like to modify the library to disable some of the optional driver functions, like M2EN, to free up some of the PWM outputs on the MEGA. (I am new to modifying libraries).

Can I simply change the line in the .CPP file :

_EN2DIAG2 = 5;

To some unused digital output (not PWM):

_EN2DIAG2 = 30;

?

Chris

Hi, Chris.

Yes, that is the only change that you should have to make in the library to change the pin used for EN2DIAG2. However, you do not need to make any changes to the library if you use the alternate constructor documented on the libraries github page.

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)

This constructor lets you remap pins directly from your Arduino sketch; that way you do not have to remember to modify the library every time you want to change a pin. For your particular case, you could just find the line in your sketch that creates the shield object and change it to the following:

DualVNH5019MotorShield md(2, 4, 6, A0, 7, 8, 30, A1);

-Claire

In case you haven’t already done it, you will also have to physically disconnect the EN2DIAG2 pin from the Arduino pin it is connected to by default. You can do this by cutting the trace going to it on the shield, as shown in the Remapping the Arduino Connections section of the shield’s user’s guide. You can then rewire it to whatever pin on the Arduino you want to use.

-Claire

Hi guys

I went to use this library and found that, while all the required info is here, I found no single source to download it from; the zip file in the first post still has the bugs that are discussed, and the github site doesn’t have the Mega-20kHz version at all (as far as I can see).