248 lines
7.8 KiB
C++
248 lines
7.8 KiB
C++
|
|
/* Example of driving servomotor using PID closed loop control */
|
|
|
|
#include <SPI.h> /* Needed to communicate with LS7366R (Counter Click) */
|
|
#include <PID_v1.h>
|
|
|
|
/* Serial input aspects are based closely upon:
|
|
http://forum.arduino.cc/index.php?topic=396450
|
|
Example 4 - Receive a number as text and convert it to an int
|
|
Modified to read a float */
|
|
|
|
/* LS7366R aspects very loosely based on concepts used in controlling
|
|
the Robogaia 3-axis encoder shield though implementation is very different
|
|
https://www.robogaia.com/3-axis-encoder-conter-arduino-shield.html */
|
|
|
|
/* Pins used for L298 driver */
|
|
const int enA = 13; /* PWM output, also visible as LED */
|
|
const int in1 = 8; /* H bridge selection input 1 */
|
|
const int in2 = 9; /* H bridge selection input 2 */
|
|
const float minPercent = -100.0;
|
|
const float maxPercent = 100.0;
|
|
|
|
/* Used to to initiate SPI communication to LS7366R chip (Counter click) */
|
|
const int chipSelectPin = 10;
|
|
|
|
/* Size of buffer used to store received characters */
|
|
enum {numChars = 32};
|
|
|
|
/* Intervals in milliseconds for user-defined timed loops */
|
|
const int printInterval = 1000;
|
|
const int controlInterval = 20;
|
|
|
|
/* Global variables used in serial input */
|
|
char receivedChars[numChars]; // an array to store the received data
|
|
float dataNumber = 0;
|
|
boolean newData = false;
|
|
|
|
/* Global variables used for motor control and encoder reading */
|
|
double percentSpeed = 0;
|
|
double encoderPosnMeasured = 0;
|
|
double positionSetPoint = 0;
|
|
|
|
/* PID */
|
|
double Kp = 0.1;
|
|
double Ki = 0.1;
|
|
double Kd = 0.05;
|
|
|
|
PID myPID(&encoderPosnMeasured, &percentSpeed, &positionSetPoint, Kp, Ki, Kd, DIRECT);
|
|
|
|
/* Global variables used for loop timing */
|
|
unsigned long prevMillisPrint = 0; /* stores last time values were printed */
|
|
unsigned long prevMillisControl = 0;
|
|
|
|
/* Overlapping regions of memory used to convert four bytes to a long integer */
|
|
union fourBytesToLong
|
|
{
|
|
long result;
|
|
unsigned char bytes [4];
|
|
};
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(9600);
|
|
Serial.print("Kp"); Serial.print(Kp);
|
|
Serial.print("Ki"); Serial.print(Ki);
|
|
Serial.print("Kd"); Serial.println(Kd);
|
|
Serial.println("Enter desired motor position: ");
|
|
|
|
/* Set up and initialise pin used for selecting LS7366R counter: hi=inactive */
|
|
pinMode(chipSelectPin, OUTPUT);
|
|
digitalWrite(chipSelectPin, HIGH);
|
|
|
|
SetUpLS7366RCounter();
|
|
|
|
delay(100);
|
|
|
|
/* Configure control pins for L298 H bridge */
|
|
pinMode(enA, OUTPUT);
|
|
pinMode(in1, OUTPUT);
|
|
pinMode(in2, OUTPUT);
|
|
|
|
/* Set initial rotation direction */
|
|
digitalWrite(in1, LOW);
|
|
digitalWrite(in2, HIGH);
|
|
|
|
delay(100);
|
|
positionSetPoint = 0;
|
|
encoderPosnMeasured=readEncoderCountFromLS7366R();
|
|
myPID.SetOutputLimits(-100,100);
|
|
myPID.SetMode(AUTOMATIC);
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
unsigned long currentMillis = millis();
|
|
|
|
// Call control loop at frequency controInterval
|
|
if (currentMillis - prevMillisControl >= controlInterval) {
|
|
// save the last time the control loop was called
|
|
prevMillisControl = currentMillis;
|
|
controlLoop();
|
|
}
|
|
|
|
// Call print loop at frequency of printInterval
|
|
if (currentMillis - prevMillisPrint >= printInterval) {
|
|
// save the last time you printed output
|
|
prevMillisPrint = currentMillis;
|
|
printLoop();
|
|
}
|
|
|
|
recvWithEndMarker();// Update value read from serial line
|
|
// If a valid number has been read this is set to the current required position
|
|
if(convertNewNumber()){
|
|
positionSetPoint=dataNumber;
|
|
}
|
|
}
|
|
|
|
void controlLoop()
|
|
{
|
|
// Get the current position from the encoder
|
|
encoderPosnMeasured=readEncoderCountFromLS7366R(); // Get current motor position
|
|
myPID.Compute(); // Use the PID library to compute new value for motor input
|
|
driveMotorPercent(percentSpeed); // Send value to motor
|
|
}
|
|
|
|
|
|
void driveMotorPercent(double percentSpeed)
|
|
/* Output PWM and H bridge signals based on positive or negative duty cycle % */
|
|
{
|
|
percentSpeed = constrain(percentSpeed, -100, 100);
|
|
int regVal = map(percentSpeed, -100, 100, -255, 255);
|
|
analogWrite(enA, (int)abs(regVal)); // Write value to speed control pin
|
|
digitalWrite(in1, regVal>0); // Set the value of direction control pins to true or false
|
|
digitalWrite(in2, !(regVal>0));
|
|
}
|
|
|
|
void printLoop()
|
|
/* Print count and control information */
|
|
{
|
|
double error;
|
|
Serial.print("Actual position: ");
|
|
Serial.print(encoderPosnMeasured);
|
|
Serial.print("\t");
|
|
Serial.print("Desired position: ");
|
|
Serial.print(positionSetPoint);
|
|
Serial.print("\t");
|
|
error = positionSetPoint - encoderPosnMeasured;
|
|
Serial.print("Error: ");
|
|
Serial.print(error);
|
|
Serial.print("\t");
|
|
// Serial.print(percentSpeed);
|
|
Serial.print("\r\n");
|
|
}
|
|
|
|
|
|
long readEncoderCountFromLS7366R()
|
|
/* Reads the LS7366R chip to obtain up/down count from encoder. Reads four
|
|
bytes separately then concverts them to a long integer using a union */
|
|
{
|
|
fourBytesToLong converter; /* Union of four bytes and a long integer */
|
|
|
|
digitalWrite(chipSelectPin,LOW); /* Make LS7366R active */
|
|
|
|
SPI.transfer(0x60); // Request count
|
|
converter.bytes[3] = SPI.transfer(0x00); /* Read highest order byte */
|
|
converter.bytes[2] = SPI.transfer(0x00);
|
|
converter.bytes[1] = SPI.transfer(0x00);
|
|
converter.bytes[0] = SPI.transfer(0x00); /* Read lowest order byte */
|
|
|
|
digitalWrite(chipSelectPin,HIGH); /* Make LS7366R inactive */
|
|
|
|
return converter.result;
|
|
}
|
|
|
|
void SetUpLS7366RCounter(void)
|
|
/* Initialises LS7366R hardware counter on Counter Click board to read quadrature signals */
|
|
{
|
|
/* Control registers in LS7366R - see LS7366R datasheet for this and subsequent control words */
|
|
unsigned char IR = 0x00, MRD0=0x00;
|
|
|
|
// SPI initialization
|
|
SPI.begin();
|
|
//SPI.setClockDivider(SPI_CLOCK_DIV16); // SPI at 1Mhz (on 16Mhz clock)
|
|
delay(10);
|
|
|
|
/* Configure as free-running 4x quadrature counter */
|
|
digitalWrite(chipSelectPin,LOW); /* Select chip and initialise transfer */
|
|
/* Instruction register IR */
|
|
IR |= 0x80; /* Write to register (B7=1, B6=0) */
|
|
IR |= 0x08; /* Select register MDR0: B5=0, B4=0, B3=1 */
|
|
SPI.transfer(IR); /* Write to instruction register */
|
|
/* Mode register 0 */
|
|
MRD0 |= 0x03; /* 4x quadrature count: B0=1, B1=1 */
|
|
/* B2=B3=0: free running. B4=B5=0: disable index. */
|
|
/* B6=0: asynchronous index. B7: Filter division factor = 1. */
|
|
SPI.transfer(MRD0);
|
|
digitalWrite(chipSelectPin,HIGH);
|
|
|
|
/* Clear the counter i.e. set it to zero */
|
|
IR = 0x00; /* Clear the instructino register IR */
|
|
digitalWrite(chipSelectPin,LOW); /* Select chip and initialise transfer */
|
|
IR |= 0x20; /* Select CNTR: B5=1,B4=0,B3=0; CLR register: B7=0,B6=0 */
|
|
SPI.transfer(IR); /* Write to instruction register */
|
|
digitalWrite(chipSelectPin,HIGH);
|
|
}
|
|
|
|
void recvWithEndMarker()
|
|
/* Receive data from serial port finishing with "newline" character.
|
|
Based on http://forum.arduino.cc/index.php?topic=396450 Example 4 */
|
|
{
|
|
static byte ndx = 0;
|
|
char endMarker = '\n';
|
|
char rc;
|
|
|
|
if (Serial.available() > 0) { // If data is available read value from serial monitor
|
|
rc = Serial.read();
|
|
|
|
if (rc != endMarker) { // Store character in buffer if not end marker
|
|
receivedChars[ndx] = rc;
|
|
ndx++;
|
|
if (ndx >= numChars) {
|
|
ndx = numChars - 1;
|
|
}
|
|
}
|
|
else { // Add end of string character and change flag to indicate new data is available
|
|
receivedChars[ndx] = '\0'; // terminate the string
|
|
ndx = 0;
|
|
newData = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
bool convertNewNumber()
|
|
/* Converts character string to floating point number only if there are new
|
|
data to convert, otherwise returns false */
|
|
{
|
|
if (newData) {
|
|
dataNumber = 0.0;
|
|
dataNumber = atof(receivedChars);
|
|
newData = false;
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|