#include "Arduino.h"
#include "QuadEncoder.h"
#include "TeensyTimerTool.h"
using namespace TeensyTimerTool;
#define PUL 5
#define DIR 6
#define ENA 7
#define IDX 4
#define A 0
#define B 1
#define chatty 0
uint32_t mCurPosValue;
uint32_t old_position = 0;
uint32_t mCurPosValue1;
uint32_t old_position1 = 0;
uint32_t timeoutpos;
volatile uint32_t val1, val2;
int myacc;
// imperial 32 TPI
int N = 293;
int D = 2000;
//// imperial 8 TPI
//int N = 293;
//int D = 500;
int bb = 0;
bool CW = false;
bool CCW = true;
bool mydir = CW;
uint16_t steplen = 5; // step pulse length in microseconds
unsigned long mytime;
unsigned long oldtime;
int encoder_resolution = 4096;
double instspeed;
double sps;
volatile bool timeout;
OneShotTimer t1(PIT); // for stepper pulse
OneShotTimer t2(PIT); // for measuring speed
QuadEncoder myEnc1(1, A, B, 0, IDX); // Encoder on channel 1 of 4 available
// Phase A (pin0), PhaseB(pin1), Pullups Req(0), Index Z (pin4)
/*
* N = leadscrewTPI * basestep * microstep [threads x steps / inch]
* D = TPI * encoder_res [ threads * pulses / ( inch x spindle rev ) ]
* numsteps = pulses (encoder count) * N/D // [ pulses x steps /( pulses / rev ) ]=[ steps * rev ]
*
* Issue is N/D is rational, so we use a method like Bresenham to calculated when to emit a step
* Didge has a method, which I am trying to understand. ESPels by greenail has a method using Didge,
* but it is a slow crawl through his code, since it is not exhaustively documented.
*/
void pulseme() // turns off stepper pulse after time out
{
digitalWriteFast(PUL, LOW);
digitalWriteFast(LED_BUILTIN, LOW);
}
void doincrement( bool adir ) // sets direction and turns on stepper pulse
{
digitalWriteFast(DIR, adir); // set direction
digitalWriteFast(PUL, HIGH);
digitalWriteFast(LED_BUILTIN, HIGH);
t1.trigger(steplen); // switch off after steplen us (5us)
}
void myms() // xxms timer callback for estimating spindle speed and step rate
{
val2 = myEnc1.read(); // read encoder for current position
// we know the time was 20 ms
double cps = 50.0 * ( (double)val2 - (double)val1)/(double)encoder_resolution;; // rev per second
int counts = (int)val2 - (int)val1;
sps = cps * (double)encoder_resolution * (double)N/(double)D;
if (cps != 0.0) // check for non-zero velocity
{
if (abs(cps) < 10000.0) // hack to avoid over underflow
{
Serial.printf("Counts in 20ms: %i\n", counts);
Serial.printf("Speed estimate: %7.2f RPM \n", cps*60.0);
Serial.printf("Stepper pulse/sec estimate: %7.2f\n", sps);
}
}
timeout = true;
}
void setup() {
while(!Serial && millis() < 4000);
for(int i=5; i<8; i++) pinMode(i, OUTPUT); // init these pins to outputs
pinMode(LED_BUILTIN, OUTPUT);
t1.begin(pulseme); // set up t1 callback function
t2.begin(myms); // set up t2 callback function
digitalWriteFast(DIR, CW); // sets direction of rotation LOW =CW, HIGH=CCW
digitalWriteFast(ENA, LOW); // active low signal, enables the stepper controller
myEnc1.setInitConfig(); // set up the HW encoder
myEnc1.init(); // initialize the HW encoder
myacc = 0;
instspeed = 0.0;
timeout = true; // so we start measuring speed right away
}
void loop()
{
mCurPosValue = myEnc1.read(); // gets all values in hold registers in encoder
if (timeout) // if no timeout, skip this
{
timeout = false; // determine how many counts we get in 20ms, this is the base for a speed estimate
t2.trigger(20ms); // set timer to go off in 20ms
val1 = mCurPosValue; // save current position
}
if(mCurPosValue != old_position)
{
bb = (int16_t) myEnc1.getHoldDifference(); // needed to cast to get print to work right
if (chatty) Serial.printf("inc: %d, myacc; %d\n", (int16_t)bb, (int16_t)myacc, (int16_t)D);
myacc = myacc + bb*N;
if ( bb >= 0)
{
if (chatty) Serial.printf("myacc : %d\n", (int16_t)myacc);
if (myacc >= D) // works for positive accumulation only
{
myacc = myacc - D;
mydir = CW;
doincrement(mydir);
}
}
else
{
if (chatty) Serial.printf("myacc : %d\n", (int16_t)myacc);
if (myacc <= D) // for negative accumulation
{
myacc = myacc + D;
mydir = CCW;
doincrement(mydir);
}
}
if (chatty)
{
Serial.printf("myacc: %ld\r\n", (int16_t)myacc); // print out our accumulator
/* Print out all the encoder position values. */
Serial.printf("Current position value1: %ld\r\n", mCurPosValue);
Serial.printf("Position differential value1: %d\r\n", (int16_t)myEnc1.getHoldDifference());
//Serial.printf("Get Revolution value: %d\r\n", (int16_t)myEnc1.getRevolution());
Serial.printf("Position HOLD revolution value1: %d\r\n", (int16_t)myEnc1.getHoldRevolution());
//Serial.printf("Index Counter: %d\r\n", myEnc1.indexCounter);
Serial.println();
}
}
old_position = mCurPosValue; // update for next cycle
}
