Pololu Robotics & Electronics
Menu
My account Comments or questions? About Pololu Contact Ordering information Distributors

Pololu Forum

Is it possible to use the DualVNH5019 Library with PID


#1

Using an Arduino Mega, I am trying to control a motor with angular feedback from an absolute encoder. I have the Dual VNH5019 motor controller with a modified library for 20kHz PWM. The PID library is the standard Arduino library.

The problem I am having is after the PID.Compute(); call, I have md.setM1Speed(AZspeed). Where AZspeed is the azimuth motor speed that is the output of the PID.Compute() function.

With this motor command, the motor stays on indefinitely and the program does not loop to calculate the next output value. Please can someone advise how to get around this?

I have seen analogWrite (PWM value), but I’d like to use the Pololu library for convenience.

Thank you.

Here is the code that I have so far:

[code]/*
Code to read data from two Hengstler AC58 16 bit SSI Encoders
and to provide basic motor control with the Dual VNH5019 Shield.

Written in Arduino 1.6.5 for Mega
12/08/2015 

*/
#include “DualVNH5019MotorShieldMega.h”
#include <PID_v1.h>

DualVNH5019MotorShield md;

double AZspeed=0;
double angleAZsetpoint=0;
double kpAZ=10, kiAZ=0, kdAZ=0;

const int CLOCK_PIN_AZ = 50; // Azimuth Encoder Clock Pin
const int DATA_PIN_AZ = 51; // Azimuth Encoder Data Pin
const int CLOCK_PIN_EL = 52; // Elevation Encoder Clock Pin
const int DATA_PIN_EL = 53; // Elevation` Encoder Data Pin

const int BIT_COUNT = 16; //for 16 bit encoder

unsigned int readingAZ = 0; // Initialising variables to store reading and angles
double angleAZ = 0;
float previousreadingAZ=0;
float previousreadingEL=0;
unsigned int readingEL = 0;
double angleEL = 0;
boolean directionAZ = false;
boolean directionEL = false;

PID AZPID(&angleAZ, &AZspeed, &angleAZsetpoint, kpAZ, kiAZ, kdAZ, DIRECT);

void setup() {

pinMode(DATA_PIN_AZ, INPUT);
pinMode(CLOCK_PIN_AZ, OUTPUT);
pinMode(DATA_PIN_EL, INPUT);
pinMode(CLOCK_PIN_EL, OUTPUT);

digitalWrite(CLOCK_PIN_AZ, HIGH); //Initialising both encoder clock pins to High
digitalWrite(CLOCK_PIN_EL, HIGH);

md.init(); //Initialise Motor Driver
AZPID.SetMode(AUTOMATIC);
AZPID.SetSampleTime(200);
AZPID.SetOutputLimits(-400,400);
angleAZsetpoint=50;

Serial.begin(19200); // Start serial comms. This will be replaced with Ethernet later

}

void loop() //Main loop
{

readingAZ = readPositionAZ(); //Get value from encoder
angleAZ = ((float)readingAZ / 65536.0) * 360.0; //Make that value in range of 0-360 degrees
AZPID.Compute(); //Perform PID calculation
md.setM1Speed(AZspeed); //Set motor speed according to PID calculation

}

//read the current angular position from Azimuth Encoder
unsigned long readPositionAZ() {
unsigned long sample1 = shiftInAZ(DATA_PIN_AZ, CLOCK_PIN_AZ, BIT_COUNT);
delayMicroseconds(25); // Clock must be high for 25 microseconds before a new sample can be taken
return sample1;
}

//read the current angular position from Elevation Encoder
unsigned long readPositionEL() {
unsigned long sample1 = shiftInEL(DATA_PIN_EL, CLOCK_PIN_EL, BIT_COUNT);
delayMicroseconds(25); // Clock must be high for 25 microseconds before a new sample can be taken
return sample1;
}

//read in a byte of data from the digital input of the Azimuth Encoder.
unsigned long shiftInAZ(const int DATA_PIN_AZ, const int CLOCK_PIN_AZ, const int bit_count) {
unsigned long data = 0;

noInterrupts(); //Switch off interrupts so that clocking in of data is not interuupted

for (int i=0; i<bit_count; i++) { //reads first bits
data <<= 1;
digitalWrite(CLOCK_PIN_AZ,LOW);
delayMicroseconds(1);
digitalWrite(CLOCK_PIN_AZ,HIGH);
data |= digitalRead(DATA_PIN_AZ);
}

interrupts();

digitalWrite(CLOCK_PIN_AZ,LOW); //clocks space bit
delayMicroseconds(1);
digitalWrite(CLOCK_PIN_AZ,HIGH);
delayMicroseconds(1);

return data;
}

//read in a byte of data from the digital input of the Elevation Encoder.
unsigned long shiftInEL(const int DATA_PIN_EL, const int CLOCK_PIN_EL, const int bit_count) {
unsigned long data = 0;

noInterrupts(); //Switch off interrupts so that clocking in of data is not interuupted

for (int i=0; i<bit_count; i++) { //reads first bits
data <<= 1;
digitalWrite(CLOCK_PIN_EL,LOW);
delayMicroseconds(1);
digitalWrite(CLOCK_PIN_EL,HIGH);
data |= digitalRead(DATA_PIN_EL);
}

interrupts();

digitalWrite(CLOCK_PIN_EL,LOW); //clocks space bit
delayMicroseconds(1);
digitalWrite(CLOCK_PIN_EL,HIGH);
delayMicroseconds(1);

return data;
}[/code]


#2

Hello.

It is not clear to me why you think the setM1Speed method is causing the issue in your code. The setM1Speed method does set the speed (between -400 and 400) that your motor (connected to M1) will continuously run at, but if you call the method again with a new value, the speed of your motor will be set to that new value.

Unfortunately, I am not familiar with the Arduino PID library that you are using, but I suspect that AZspeed might not be changing after calling the Compute method. You might try printing the value of AZspeed to the Serial Monitor after calling the PID library’s Compute method in your main loop to verify that the value changes. Also, can you post a screenshot of your Serial Monitor showing the results after running your modified sketch?

- Amanda


#3

I got the motor to respond to the PID output. The problem was too embarrassing to share :wink:

The PID is able to give an output of -400 to 400 and the motor does indeed rotate in both directions. Next is to tune the PID.

Thanks.

Edit: The input to the PID was looking at the wrong encoder that was stationary!