I am using the PID control library to command a powertrain using position feedback. My position signal comes from an optical encoder that is sensing at 20000 CPR (counts per revolution). The encoder channels (A and B) are tied to interrupts to ensure that each pulse is captured during rotation. During my initial tests, before including data writes to an SD Card, the Teensy seems to be keeping up with the encoder, event during rapid movement. In spite of frequent interrupts, it appears that the Teensy 3.5 is running fast enough to run the rest of my code below 1ms/loop cycle when the data acquisition portion of the code (lines 229 - 251) is commented out. It is also worth mentioning that my SD card has pretty decent performance (SanDisk Ultra, class 10). I've printed results from the bench example sketch below.
EDIT: After running this code, I have noticed that I am using the SD library in my control/DAQ program and not SDfat, so I suppose that may be a cause of my latentcy.
The beginning of my main loop is a series of "If/else" statements forming a position recipe that I need the powertrain to follow. During the loop cycle, I also have code to write the current millis time step, setpoint, endcoder postion, and error (SP - EP) to the SD card on each loop cycle. My goal is to use this system as both a closed loop controller to the powertrain as well as a data logger of the system performance (for both tuning the PID as well as future data acquisition). In spite of my noob skills, everything is working fairly well with one big exception. What I have noticed in the output is an output error every 5800ms. Teh controller seems to send a command signal to the motor controller and gets stuck in the full "on" mode while some other process is takes the wheel for about 500ms. This problem seems to repeat every 5.8 seconds. My first guess is that a write cycle is occurring and killing my performance? The truth is, I know I should probably be using a data buffer but I haven't set one up yet, as I don't know how to set them up properly yet. In any event, I also know that I am casting my int data to char() before writing, and maybe this is causing delays as well? I am still very new to writing my own code to write to SD, so I am very open to constructive criticism. Below you'll find my full controller source code. I guess the question that I'm trying to answer is "is it obvious from my implementation below why this might be happening?" and "how can I optimize my code to prevent it?"
Also, here is a link to an image of my plotted results, showing the large spikes at repeating 5800ms intervals.
https://imgur.com/a/c1Wk9
EDIT: After running this code, I have noticed that I am using the SD library in my control/DAQ program and not SDfat, so I suppose that may be a cause of my latentcy.
Code:
SdFatSdioEX uses extended multi-block transfers without DMA.
SdFatSdio uses a traditional DMA SDIO implementation.
Note the difference is speed and busy yield time.
Type '1' for SdFatSdioEX or '2' for SdFatSdio
(using '2', SdFatSdioEX)
size,write,read
bytes,KB/sec,KB/sec
512,14197.91,15565.36
1024,14461.42,15814.28
2048,14421.82,16110.50
4096,15260.28,16312.63
8192,14606.97,16365.62
16384,15397.14,16458.03
32768,14951.92,16460.90
totalMicros 7618593
yieldMicros 187990
yieldCalls 176
yieldMaxUsec 1314
kHzSdClk 40000
Done
Type '1' for SdFatSdioEX or '2' for SdFatSdio
(using '2', SdFatSdio)
size,write,read
bytes,KB/sec,KB/sec
512,419.59,1944.20
1024,715.84,2843.06
2048,1518.90,4802.62
4096,4063.64,8758.51
8192,5787.70,10988.64
16384,9049.69,14955.97
32768,12173.33,16874.68
totalMicros 54154586
yieldMicros 53148423
yieldCalls 81366
yieldMaxUsec 140545
kHzSdClk 40000
DoneThe beginning of my main loop is a series of "If/else" statements forming a position recipe that I need the powertrain to follow. During the loop cycle, I also have code to write the current millis time step, setpoint, endcoder postion, and error (SP - EP) to the SD card on each loop cycle. My goal is to use this system as both a closed loop controller to the powertrain as well as a data logger of the system performance (for both tuning the PID as well as future data acquisition). In spite of my noob skills, everything is working fairly well with one big exception. What I have noticed in the output is an output error every 5800ms. Teh controller seems to send a command signal to the motor controller and gets stuck in the full "on" mode while some other process is takes the wheel for about 500ms. This problem seems to repeat every 5.8 seconds. My first guess is that a write cycle is occurring and killing my performance? The truth is, I know I should probably be using a data buffer but I haven't set one up yet, as I don't know how to set them up properly yet. In any event, I also know that I am casting my int data to char() before writing, and maybe this is causing delays as well? I am still very new to writing my own code to write to SD, so I am very open to constructive criticism. Below you'll find my full controller source code. I guess the question that I'm trying to answer is "is it obvious from my implementation below why this might be happening?" and "how can I optimize my code to prevent it?"
Code:
#include <TimerOne.h>
#include <PID_v1.h>
#include <SD.h>
#include <SPI.h>
// Set up elapsedMillis variables
elapsedMillis delTime;
int mState = 1;
int cnt = 0;
int dither = 275;
int rap45 = 2500;
int rap90 = 5000;
int ramp = 5000;
// Set up logger variables+
const int chipSelect = BUILTIN_SDCARD;
long startTime = 0;
long tm = 0;
//Define PID Variables
double Setpoint, Input, Output;
double Kp = 0.5, Ki = 0.0, Kd = 0;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
// Quadrature encoder
const int encoderInterruptA = 24;
const int encoderInterruptB = 25;
const int encoderPinA = 24;
const int encoderPinB = 25;
volatile bool encoderASet;
volatile bool encoderBSet;
volatile long encoderTicks = 0;
// Define PWM output pins
const int forward = 27;
const int reverse = 28;
const int spd = 29;
void setup()
{
// Quadrature encoder setup
pinMode(encoderPinA, INPUT); // sets pin A as input
digitalWrite(encoderPinA, LOW); // turn on pullup resistors
pinMode(encoderPinB, INPUT); // sets pin B as input
digitalWrite(encoderPinB, LOW); // turn on pullup resistors
attachInterrupt(digitalPinToInterrupt(encoderPinA), HandleInterruptA, CHANGE);
attachInterrupt(digitalPinToInterrupt(encoderPinB), HandleInterruptB, CHANGE);
//setup direction and speed pins
pinMode(forward, OUTPUT);
digitalWrite(forward, LOW);
pinMode(reverse, OUTPUT);
digitalWrite(reverse, LOW);
pinMode(spd, OUTPUT);
digitalWrite(spd, LOW);
//turn the PID on
myPID.SetOutputLimits(-255, 255);
myPID.SetMode(AUTOMATIC);
// Open serial communications and wait for port to open:
Serial.begin(115200);
while (!Serial) {
; // wait for serial port to connect. Needed for Leonardo only
}
// see if the card is present and can be initialized:
Serial.print("Initializing SD card...");
if (!SD.begin(chipSelect)) {
Serial.println("Card failed, or not present");
// don't do anything more:
return;
}
Serial.println("card initialized.");
// setup base timer
startTime = millis();
}
void loop()
{
if (mState == 1) { // State 1: 30x 5 deg dither cycles
if (delTime <= 250) {
if (delTime <= 125) { // ramp up if <125ms, ramp down if >125ms
Setpoint = map((int)delTime,0,125,0,dither);
}
else {
Setpoint = map((int)delTime,125,250,dither,0);
}
}
else {
cnt++;
delTime = 0;
if (cnt >= 30) { // handle dither cycle count
cnt = 0;
mState = 2;
delTime = 0;
}
}
}
else if (mState == 2) { // State 2: 45 deg Rapids (2x)
if (delTime <= 500) {
if (delTime <= 250) { // ramp up if <250ms, ramp down if >250ms
Setpoint = map((int)delTime,0,250,0,rap45);
}
else {
Setpoint = map((int)delTime,250,500,rap45,0);
}
}
else {
cnt++;
delTime = 0;
if (cnt >= 2) { // handle rapids cycle count
cnt = 0;
mState = 3;
delTime = 0;
}
}
}
else if (mState == 3) { // State 3: 90 deg fast rapids (1x)
if (delTime <= 500) {
if (delTime <= 250) { // ramp up if <250ms, ramp down if >250ms
Setpoint = map((int)delTime,0,250,0,rap90);
}
else {
Setpoint = map((int)delTime,250,500,rap90,0);
}
}
else {
cnt++;
delTime = 0;
if (cnt >= 1) { // handle rapids cycle count
cnt = 0;
mState = 4;
delTime = 0;
}
}
}
else if (mState == 4) { // State 4: 90 deg fast ramp (1x)
if (delTime <= 5000) {
if (delTime <= 2500) { // ramp up if <2500ms, ramp down if >2500ms
Setpoint = map((int)delTime,0,2500,0,ramp);
}
else {
Setpoint = map((int)delTime,2500,5000,ramp,0);
}
}
else {
cnt++;
delTime = 0;
if (cnt >= 1) { // handle rapids cycle count
cnt = 0;
mState = 5;
delTime = 0;
}
}
}
else if (mState == 5) { // State 5: 90 deg slow rapids (2x)
if (delTime <= 1000) {
if (delTime <= 500) { // ramp up if <500ms, ramp down if >500ms
Setpoint = map((int)delTime,0,500,0,rap90);
}
else {
Setpoint = map((int)delTime,500,1000,rap90,0);
}
}
else {
cnt++;
delTime = 0;
if (cnt >= 2) { // handle rapids cycle count
cnt = 0;
mState = 6;
delTime = 0;
}
}
}
else if (mState == 6) { // State 6: 90 deg slow ramp (1x)
if (delTime <= 10000) {
if (delTime <= 5000) { // ramp up if <125ms, ramp down if >125ms
Setpoint = map((int)delTime,0,5000,0,ramp);
}
else {
Setpoint = map((int)delTime,5000,10000,ramp,0);
}
}
else {
cnt++;
delTime = 0;
if (cnt >= 1) { // handle rapids cycle count
cnt = 0;
mState = 7;
delTime = 0;
}
}
}
else if (mState == 7) { // State7: 90 deg slow rapids (2x)
if (delTime <= 1000) {
if (delTime <= 500) { // ramp up if <500ms, ramp down if >500ms
Setpoint = map((int)delTime,0,500,0,rap90);
}
else {
Setpoint = map((int)delTime,500,1000,rap90,0);
}
}
else {
cnt++;
delTime = 0;
if (cnt >= 1) { // handle rapids cycle count
cnt = 0;
mState = 8;
delTime = 0;
}
}
}
noInterrupts();
Input = encoderTicks;
interrupts();
myPID.Compute();
// send data to SD card
// make a string for assembling the data to log:
String dataString = "";
// read three sensors and append to the string:
tm = millis()-startTime;
dataString += String(tm);
dataString += ",";
dataString += String(Setpoint);
dataString += ",";
dataString += String(encoderTicks);
dataString += ",";
dataString += String(Setpoint - encoderTicks);
// open the file. note that only one file can be open at a time,
// so you have to close this one before opening another.
File dataFile = SD.open("datalog2.txt", FILE_WRITE);
// if the file is available, write to it:
if (dataFile) {
dataFile.println(dataString);
dataFile.close();
// print to the serial port too:
Serial.println(dataString);
}
// if the file isn't open, pop up an error:
else {
Serial.println("error opening datalog.txt");
}
gearBox(Output);
}
// gearBox motion function
void gearBox(double sign4l)
{
// H-bridge output to motor
if(sign4l > 0)
{
// go go forward
digitalWrite(forward, HIGH);
digitalWrite(reverse, LOW);
analogWrite(spd, sign4l);
}
else if(sign4l < 0)
{
// go reverse
digitalWrite(forward, LOW);
digitalWrite(reverse, HIGH);
analogWrite(spd, abs(sign4l));
}
else
{
// no go
digitalWrite(forward, LOW);
digitalWrite(reverse, LOW);
analogWrite(spd,0);
}
}
// Interrupt service routines for the quadrature encoder
void HandleInterruptA()
{
// Test transition
encoderASet = digitalRead(encoderPinA); // read the input pin
encoderBSet = digitalRead(encoderPinB); // read the input pin
// and adjust counter + if A leads B
if(encoderASet)
{
encoderTicks += encoderBSet ? -1 : +1;
}
else
{
encoderTicks -= encoderBSet ? -1 : +1;
}
}
// Interrupt service routines for the quadrature encoder
void HandleInterruptB()
{
// Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
encoderASet = digitalRead(encoderPinA); // read the input pin
encoderBSet = digitalRead(encoderPinB); // read the input pin
// and adjust counter + if B leads A
if(encoderBSet)
{
encoderTicks -= encoderASet ? -1 : +1;
}
else
{
encoderTicks += encoderASet ? -1 : +1;
}
}Also, here is a link to an image of my plotted results, showing the large spikes at repeating 5800ms intervals.
https://imgur.com/a/c1Wk9