I am trying to use the PID library with the angular feedback from the absolute encoders which I can now read data from.
I am using a Pololu Dual VNH5019 motor controller shield which I have used without PID before. I have the code to read from two optical encoders but have implemented only one PID to begin with.
Here is the problem, using some arbitary angle as the setpoint, when the program is started it just makes the motor run at the maximum speed in the reverse direction. The Pololu shield takes a speed value from -400 to 400. Negative numbers are reverse direction.
As I understand it, the PID outputs 0 to 255, is that correct? In that case the motor can only run forward? I'm not certain this is the case.
Would anyone more experienced be able to shed light on this? I'd like the PID to output between -400 and 400, if this is possible.
Here is the code
/*
Code to read data from two Hengstler AC58 16 bit SSI Encoders
and to provide basic motor control with the Dual VNH5019 Shield.
*** Only one PID set up so far!***
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 to print angle to serial port every XXXms.
{
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
Serial.print(AZspeed); //Check to see updated motor speed and angle
Serial.print(" ");
Serial.println(angleAZ);
}
//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;
}
When the program has started, the motor turns at -400 (max reverse speed), but the angle is not changing which implies that the loop is not looping.
Hope someone can help, thank you.