PWM Frequency - Dual VNH5019 Shield & Arduino Library

I recently purchased the Dual VNH5019 Motor Driver Shield for Arduino.

It works perfectly with my Arduino Uno using the pololu Arduino library for the board. My motors operate very quietly, there is no PWM type buzzing, I assume this is because of the 20kHz “ultrasonic” operation.

However, using the Arduino Mega2560 R3 everything functions as with the Arduino Uno except that the motors make a distinct buzzing sound well within audible range. This buzzing sounds very much like a low kHz PWM type buzzing. I have the shield attached as with my Arduino Uno so M1PWM & M2PWM are attached to pins 9 & 10 on the Arduino.

Am I correct in thinking that this buzzing is due to timers on the Mega2560 R3 differing from those on the UNO? For example, I think that pins 9 & 10 are now on timer2 rather than timer1 as with the Uno (according to: arduino.cc/forum/index.php?topic=72092.0).

If this is correct would it be possible for the library to be updated to detect the ATmega2560 and alter timer2 in place of timer1?

Alternatively, since your library alters timer1 I could connect M1PWM & M2PWM to pins 11 & 12 which I believe are now controlled by timer1.

Hi, Bob.

Yes, that buzzing sound is most likely the PWM whine. Unfortunately, timer 2 is an 8-bit timer and there is no good configuration that gives you a frequency close to an ultrasonic PWM while being below the driver’s 20kHz maximum. If you want an ultrasonic PWM, you will need to remap the PWM pins to one of the 16-bit timers on the Mega (like timer 1 as you suggest), and modify the library accordingly. Please note that the nSF pin is currently mapped to pin 12.

Please let me know if you need assistance with the remapping or code modifications, because I would be glad to help.

- Ryan

Hi Ryan

I’m about to do the same thing, VNH5109 board on a Mega, glad I read this post.

I’m certainly no guru in C/C++, so I’d greatly appreciate if you could update your library to recognise and handle the Mega.

Will I also need to make/change jumpers on the driver board ?

(bump)

Hi Ryan

I don’t know about Bob, but I definitely need assistance with the remapping / code modifications to suit the Mega.

I found this arduino.cc/forum/index.php?topic=77168.0 but I’m unclear on a couple of things:
1/ how do I “reassign MIPWM to Arduino digital pin 11 and M2PWM to Arduino digital pin 12”? - is that in software or hardware ?
2/ I’d really prefer if the ‘official’ library was updated on github github.com/pololu/Dual-VNH5019-Motor-Shield rather than me trying to track my own personal versions, since I’m likely to use both types of boards. Seems there are several people suffering from the same thing, thanks to Arduino being inconsistent with their pin assignments.

thanks in advance,
Michael

Hi, Michael.

Just to be clear, the motor shield works with the Arduino Mega 2560 R3 by default without any extra configuration or modifications. The issue in this thread is that some people prefer to get an ultrasonic PWM frequency. Have you already determined that the low-frequency PWM is unacceptable to you?

- Ryan

Hi Ryan

I tried it yesterday, and its borderline at best, but only because I’ve got such a noisy motor. The pwm squeal is almost as loud as the motor now, and the person I’m building this for wants it to be quiet, so I know once they source a quieter motor they will be complaining about the noise.

I guess my other option is to mount the board separately (not as a shield) and connect via jumpers to the row of General-Purpose Motor Driver connections, but I’d really prefer to use it as a shield plugged into my mega. I’m just unclear how to do that, I gather I need to:

  1. cut some traces and use jumpers on the pin re-assignment block so pwm’s come from pins connected to a 16 bit timer (since default 9 & 10 are connected to an 8 bit timer on the Mega), and
  2. change the library to reflect pin and timer re-assignment.

I’m drifting a long way from the “buy a shield, plug and play” dream. Reading mythic-beasts.com/~markt/ATmega-timers.html suggests maybe timer3 on pins 3 & 5 (otherwise free with the VNH5019 shield) could be a good choice for minimum change. Will this work ?

thanks

Sorry for the delay in replying; I know it can be frustrating when you expect things to be plug and play and they do not perform up to your expectations. We had to make some trade off with the pin assignment because Arduino unfortunately did not connect the same kind of timer to the pins of the Mega.

I think that using Timer 3 should work, but I have not had a chance to try it myself yet.

Yes, your understand of reassignment is correct. There is a section in the user’s guide called “Remapping the Arduino Connections”, which has more information.

Here is a general description of the changes you’d have to make to the library code:

  • In DualVNH5019MotorShield.h: _PWM1 and _PWM2 would need to changed to the newly assigned pins[
  • In DualVNH5019MotorShield.cpp: All the code inside the “#if defined(AVR_ATmega168)|| defined(AVR_ATmega328P)” blocks would have to be used instead of analogWrite, and if you used timer 3 instead of Timer 1, you’d have to change registers like TCCR1A to TCCR3A.

I would like to make a better way to configure the library, but I have not been able to do that yet.

- Ryan

Hi Ryan

thanks for the reply, no drama with the delay since I’ve been dragged off onto other jobs for the last week, so haven’t had a chance to even read your reply until now.

Guess I’ll have a shot at modding the software myself, and will post the result (hopefully success!)

OK, I think I’ve got it sorted for the Mega on pins 3 & 5, but this same library wont work with both Mega and other boards. Someone that knows more about compiler directives may be able to sort it.

Here is the code that works with the Mega:

DualVNH5019MotorShield.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 = 3;
    static const unsigned char _PWM2 = 5;
  #endif  
    unsigned char _EN1DIAG1;
    unsigned char _CS1;
    unsigned char _INA2;
    unsigned char _INB2;
    unsigned char _EN2DIAG2;
    unsigned char _CS2;
    
};

#endif

DualVNH5019MotorShield.cpp

#include "DualVNH5019MotorShield.h"

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

DualVNH5019MotorShield::DualVNH5019MotorShield()
{
  //Pin map
  _INA1 = 2;
  _INB1 = 4;
  _EN1DIAG1 = 6;
  _CS1 = A0; 
  _INA2 = 7;
  _INB2 = 8;
  _EN2DIAG2 = 12;
  _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
  TCCR3A = 0b10100000; // Mega pin 5
  TCCR3B = 0b00010001; // Mega pin 3
  ICR3 = 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;
  #elseif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  // Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
  OCR3A = 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;
  #elseif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  // Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
  OCR3C = 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;
  #elseif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  // Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
  OCR3A = 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;
  #elseif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  // Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
  OCR3C = 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);
}

Here is my attempt to have a single library that works with Uno/Duemilenova and Mega:

The problem is that only one channel lights up, pin 5 gets pwm, but pin 3 doesn’t. I’ve run out of time to get this working ‘universally’, I just have to run with 2 versions of the library (the original and the one above).

Would love to see someone solve this.

DualVNH5019MotorShield.cpp

#include "DualVNH5019MotorShield.h"

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

DualVNH5019MotorShield::DualVNH5019MotorShield()
{
  //Pin map
  _INA1 = 2;
  _INB1 = 4;
  _EN1DIAG1 = 6;
  _CS1 = A0; 
  _INA2 = 7;
  _INB2 = 8;
  _EN2DIAG2 = 12;
  _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
  TCCR3A = 0b10100000; 
  TCCR3B = 0b00010001; 
  ICR3 = 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;
  #elseif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  // Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
    #if defined(OCR3A)//
    OCR3A = speed;
    #endif
  #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;
  #elseif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  // Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
    #if defined OCR3C
    OCR3C = speed;
    #endif
  #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;
  #elseif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  // Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
    #if defined OCR3A
    OCR3A = brake;
    #endif
  #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;
  #elseif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  // Mega board specific stuff here - assumes assigning timer3, using pins 3 &5
    #if defined OCR3C
    OCR3C = brake;
    #endif
  #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);
}

Hi effgee,

Would this mean that instead of connecting the motor driver to pins 9 & 10 (for PWM) one would connect them to pins 3 & 5 instead leaving all other pin connections as they are? Sorry if the question is extremely obvious but I am a beginner with regards to this subject.

Many thanks in advance,
siattasm

That’s what I did, i.e. cut the tracks and solder in jumper wires. However, I never did get it working properly, ended up damaging the drive card somehow (it now flags a motor fault if I exceed about 1500mA on channel 2). For a while I was using a stepper motor instead of the brushed motor on channel 2, hence how I thought I had it working OK (since I wasn’t using the problematic 2nd channel).

At my (low) level of expertise, I simply ran out of time to chase it down, so I ended up living with audible pwm noise. This was offset by the fact I ended up running at 100% duty cycle except during ramp up/ramp down. If you’re a beginner, ask yourself do you really need ultrasonic (i.e. noiseless) pwm. If not, the shield should work off the bat, but just hum when driving.

If I get some time I’ll have another look at it, but that is at least a week away.

Maybe if enough people ask for this a Pololu guru will step in and either solve it or say it can’t be done. Its not Pololu’s fault it doesn’t work, they’ve at least provided jumers to facilitate pin changes. It’s really a result of careless(?) pin re-assignment by the Arduino designers when laying out the Mega. Maybe it was unavoidable due to track routing restrictions/lack of layers on the pcb, but it sucks that sketches using timers aren’t portable between boards, seems to go against the idea of the simplified IDE.

A modified version of the VNH5019 shield library that allows for a 20kHz PWM frequency with the Arduino Mega is available in this thread.

-Claire

A post was split to a new topic: Using the VNH5019 shield