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]