209 lines
7.1 KiB
C++
209 lines
7.1 KiB
C++
/* Example of driving servomotor and reading encoder signals */
|
|
|
|
#include <SPI.h> /* Needed to communicate with LS7366R (Counter Click) */
|
|
|
|
/* 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 DC Motor driver */
|
|
#define enA 13 /* PWM output, also visible as LED */
|
|
#define in1 8 /* H bridge selection input 1 */
|
|
#define in2 9 /* H bridge selection input 2 */
|
|
#define minPercent -100.0
|
|
#define maxPercent 100.0
|
|
|
|
/* Used to to initiate SPI communication to LS7366R chip (Counter click) */
|
|
#define chipSelectPin 10
|
|
|
|
/* Size of buffer used to store received characters */
|
|
#define numChars 32
|
|
|
|
/* Intervals in milliseconds for user-defined timed loops */
|
|
#define printInterval 1000
|
|
|
|
/* Global variables used in serial input */
|
|
char receivedChars[numChars]; // an array to store the received data
|
|
float dataNumber = 0;
|
|
boolean newData = false;
|
|
|
|
/* Global variable used for motor control */
|
|
double percentSpeed;
|
|
|
|
/* Global variables used for loop timing */
|
|
unsigned long prevMillisPrint = 0; /* stores last time values were printed */
|
|
|
|
/* 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.println("Enter PWM duty cycle as a percentage (positive for forward, negative for reverse");
|
|
|
|
/* 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);
|
|
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
unsigned long currentMillis = millis();
|
|
|
|
// Print out value to serial monitor at interval specified by printInterval variable
|
|
if (currentMillis - prevMillisPrint >= printInterval) {
|
|
// save the last time you printed output
|
|
prevMillisPrint = currentMillis;
|
|
printLoop();
|
|
}
|
|
|
|
// Check if new data has been input via serial monitor
|
|
recvWithEndMarker();
|
|
if(convertNewNumber()) // Update value read from serial line
|
|
{
|
|
percentSpeed=dataNumber;
|
|
driveMotorPercent(percentSpeed); // Send new speed value to motor driver
|
|
}
|
|
}
|
|
|
|
void driveMotorPercent(double percentSpeed)
|
|
/* Output PWM and H bridge signals based on positive or negative duty cycle % */
|
|
{
|
|
percentSpeed = constrain(percentSpeed, -100, 100); // Value must be in range -100 to +100
|
|
int regVal = map(percentSpeed, -100, 100, -255, 255); // Scale value to range -255 to +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)); // depending on whether speed is positive or negative
|
|
}
|
|
|
|
void printLoop()
|
|
/* Print count and control information */
|
|
{
|
|
/* Sample counter chip and output position and requested speed */
|
|
long encoderCountFromLS7366R = readEncoderCountFromLS7366R();
|
|
|
|
Serial.print("Count from LS7366R = ");
|
|
Serial.print(encoderCountFromLS7366R);
|
|
Serial.print(" Percent speed = ");
|
|
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)
|
|
/* Initialiseds 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;
|
|
}
|
|
}
|