Hello, please give me some advice on my project about speed control of motors.
I am using a Sabertooth motor driver and an Arduino board to control two motors. The motors have hall effect encoders mounted to the tail. I also use a Dual LS7366R Quadrature Encoder Buffer to count the pulses of the encoders. The buffer board communicates with the Arduino Mega using SPI. I found the sample code for the LS7366R board here https://github.com/SuperDroidRobots/Encoder-Buffer-Breakout
Below is the code, I can get the encoder counts from the serial port and the values seem very clean. I need to calculate the rpm from those values. The idea I think is to take a certain time interval (like 50 ms) and using (new_count - old_count)/interval, the idea seems straigthforward but I just could not get it work. All the examples I can find are directing connecting the encoders to the Arduino and using interrupts to count the pulses of the encoders, like the one in Arduino Playground - ReadingRPM. I have the LS7366R to do the job and the Arduino can directly read values, I need to do a time derivative of those values. Anyone has than idea how to implement that, using Timers or anything? It may seem easy but it really bothers me for a few days.
#include <SabertoothSimplified.h> // Sabertooth motor driver Library
#include <Sabertooth.h>
#include "Arduino.h" // Arduino
#include <avr/io.h> // General definations of registers
#include <util/delay.h> // Delay functions
#include <SoftwareSerial.h> // Serial Library
#include <SPI.h> // SPI Library
// ****************************************************
// Hardware Pin Definitions
// ****************************************************
#define SAB_RX 25
#define SAB_PORT 43 // SaberTooth S1
#define SAB_ESTOP 45 // SaberTooth S2
#define POT A8 // Pot to control the speed of the motors
// Slave Select pins for encoders 1 and 2
#define slaveSelectEnc1 49
#define slaveSelectEnc2 48
// ****************************************************
// These hold the current encoder count.
// Updated with current number once per loop
// ****************************************************
signed long encoder1count = 0;
signed long encoder2count = 0;
// Define variables and constants
int val = 0;
int driveMotorValue = 0;
// ****************************************************
// Initialize software serial for the motor controller
// ****************************************************
SoftwareSerial SWSerial(SAB_RX, SAB_PORT);
SabertoothSimplified DriveMotor(SWSerial);
void setup()
{
// Initialize serial
Serial.begin(9600);
SWSerial.begin(9600);
Serial.flush(); // Clear
while (!Serial) {} // Flash L4 if we are waiting for a serial connection
// Initialize connected I/O
pinMode (SAB_ESTOP, OUTPUT);
allStop(); // Stop mptors
SPI.begin();
pinMode(slaveSelectEnc1, OUTPUT);
pinMode(slaveSelectEnc2, OUTPUT);
initEncoders();
clearEncoderCount();
}
void loop()
{
// receive values from the POT, map the values to -127~127
//val = analogRead(POT);
//val = map(val, 0, 1023, -127, 127);
//driveMotorValue = constrain(val, -127, 127);
driveMotorValue = 30;
DriveMotor.motor(1, driveMotorValue);
encoder1count = readEncoder(1);
DriveMotor.motor(2, driveMotorValue);
encoder2count = readEncoder(2);
Serial.print(driveMotorValue);
Serial.print("Enc1: "); Serial.print(encoder1count); Serial.print(" Enc2: ");
Serial.println(encoder2count);
}
void allStop()
{
int motorZero = 0;
//Serial.println("All Stop");
digitalWrite(SAB_ESTOP, LOW);
DriveMotor.motor(1,motorZero); // Zero motors
}
void initEncoders()
{
// Raise select pins
// Communication begins when you drop the individual select signal
digitalWrite(slaveSelectEnc1, HIGH);
digitalWrite(slaveSelectEnc2, HIGH);
// digitalWrite(SAB_ESTOP,HIGH);
// Initialize encoder 1
// Clock division factor: 0
// Negative index input
// free-running count mode
// x4 quadrature count mode (four counts per quadrature cycle)
// NOTE: For more information on commands, see datasheet
digitalWrite(slaveSelectEnc1, LOW); // Begin SPI conversation
SPI.transfer(0x88); // Write to MDR0
SPI.transfer(0x03); // Configure to 4 byte mode
digitalWrite(slaveSelectEnc1, HIGH); // Terminate SPI conversation
// Initialize encoder 2
// Clock division factor: 0
// Negative index input
// free-running count mode
// x4 quadrature count mode (four counts per quadrature cycle)
// NOTE: For more information on commands, see datasheet
digitalWrite(slaveSelectEnc2, LOW); // Begin SPI conversation
SPI.transfer(0x88); // Write to MDR0
SPI.transfer(0x03); // Configure to 4 byte mode
digitalWrite(slaveSelectEnc2, HIGH); // Terminate SPI conversation
}
// ****************************************************
// Reads the Encoders to retrieve the updated value.
// RETURNS: long
// ****************************************************
long readEncoder(int encoder)
{
long count_value;
unsigned int count_1, count_2, count_3, count_4;
// Read encoder 1
if (encoder == 1) {
digitalWrite(slaveSelectEnc1, LOW); // Begin SPI conversation
SPI.transfer(0x60); // Request count
count_1 = SPI.transfer(0x00); // Read highest order byte
count_2 = SPI.transfer(0x00);
count_3 = SPI.transfer(0x00);
count_4 = SPI.transfer(0x00); // Read lowest order byte
digitalWrite(slaveSelectEnc1, HIGH); // Terminate SPI conversation
}
// Read encoder 2
else if (encoder == 2) {
digitalWrite(slaveSelectEnc2, LOW); // Begin SPI conversation
SPI.transfer(0x60); // Request count
count_1 = SPI.transfer(0x00); // Read highest order byte
count_2 = SPI.transfer(0x00);
count_3 = SPI.transfer(0x00);
count_4 = SPI.transfer(0x00); // Read lowest order byte
digitalWrite(slaveSelectEnc2, HIGH); // Terminate SPI conversation
}
// Calculate encoder count
count_value= ((long)count_1<<24) + ((long)count_2<<16) + ((long)count_3<<8 ) +
(long)count_4;
return count_value;
}
void clearEncoderCount() {
// Set encoder1's data register to 0
digitalWrite(slaveSelectEnc1, LOW); // Begin SPI conversation
// Write to DTR
SPI.transfer(0x98);
// Load data
SPI.transfer(0x00); // Highest order byte
SPI.transfer(0x00);
SPI.transfer(0x00);
SPI.transfer(0x00); // lowest order byte
digitalWrite(slaveSelectEnc1, HIGH); // Terminate SPI conversation
delayMicroseconds(100); // provides some breathing room between SPI conversations
// Set encoder1's current data register to center
digitalWrite(slaveSelectEnc1, LOW); // Begin SPI conversation
SPI.transfer(0xE0);
digitalWrite(slaveSelectEnc1, HIGH); // Terminate SPI conversation
// Set encoder2's data register to 0
digitalWrite(slaveSelectEnc2, LOW); // Begin SPI conversation
// Write to DTR
SPI.transfer(0x98);
// Load data
SPI.transfer(0x00); // Highest order byte
SPI.transfer(0x00);
SPI.transfer(0x00);
SPI.transfer(0x00); // lowest order byte
digitalWrite(slaveSelectEnc2, HIGH); // Terminate SPI conversation
delayMicroseconds(100); // provides some breathing room between SPI conversations
// Set encoder2's current data register to center
digitalWrite(slaveSelectEnc2, LOW); // Begin SPI conversation
SPI.transfer(0xE0);
digitalWrite(slaveSelectEnc2, HIGH); // Terminate SPI conversation
}
