2019-12-02 14:07:28 -07:00
|
|
|
#include "MovementEncoder.h"
|
2015-10-29 16:50:09 -06:00
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
MovementEncoder::MovementEncoder()
|
2017-04-19 08:12:12 -06:00
|
|
|
{
|
|
|
|
//lastCalcLog = 0;
|
|
|
|
|
|
|
|
pinChannelA = 0;
|
|
|
|
pinChannelB = 0;
|
|
|
|
|
|
|
|
position = 0;
|
|
|
|
encoderType = 0; // default type
|
2018-01-03 22:27:03 -07:00
|
|
|
scalingFactor = 10000;
|
2017-04-19 08:12:12 -06:00
|
|
|
|
|
|
|
curValChannelA = false;
|
|
|
|
curValChannelA = false;
|
|
|
|
prvValChannelA = false;
|
|
|
|
prvValChannelA = false;
|
|
|
|
|
|
|
|
readChannelA = false;
|
|
|
|
readChannelAQ = false;
|
|
|
|
readChannelB = false;
|
|
|
|
readChannelBQ = false;
|
2018-01-29 13:59:02 -07:00
|
|
|
|
|
|
|
mdlEncoder = _MDL_X1;
|
2015-10-29 16:50:09 -06:00
|
|
|
}
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
void MovementEncoder::test()
|
2017-04-19 08:12:12 -06:00
|
|
|
{
|
|
|
|
/*
|
2015-11-06 15:22:17 -07:00
|
|
|
Serial.print("R88 ");
|
2015-11-02 14:15:49 -07:00
|
|
|
Serial.print("position ");
|
2015-10-29 16:50:09 -06:00
|
|
|
Serial.print(position);
|
2015-11-02 14:15:49 -07:00
|
|
|
Serial.print(" channel A ");
|
|
|
|
Serial.print(prvValChannelA);
|
2015-11-06 15:22:17 -07:00
|
|
|
Serial.print(" -> ");
|
2015-11-02 14:15:49 -07:00
|
|
|
Serial.print(curValChannelA);
|
|
|
|
Serial.print(" channel B ");
|
|
|
|
Serial.print(prvValChannelB);
|
2015-11-06 15:22:17 -07:00
|
|
|
Serial.print(" -> ");
|
2015-11-02 14:15:49 -07:00
|
|
|
Serial.print(curValChannelB);
|
2016-09-09 11:18:16 -06:00
|
|
|
Serial.print("\r\n");
|
2017-03-20 16:08:23 -06:00
|
|
|
*/
|
2015-10-29 16:50:09 -06:00
|
|
|
}
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
void MovementEncoder::loadPinNumbers(int channelA, int channelB, int channelAQ, int channelBQ)
|
2017-04-19 08:12:12 -06:00
|
|
|
{
|
|
|
|
pinChannelA = channelA;
|
|
|
|
pinChannelB = channelB;
|
|
|
|
pinChannelAQ = channelAQ;
|
|
|
|
pinChannelBQ = channelBQ;
|
2015-10-29 16:50:09 -06:00
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
readChannels();
|
|
|
|
shiftChannels();
|
2015-10-29 16:50:09 -06:00
|
|
|
}
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
void MovementEncoder::loadSettings(int encType, long scaling, int invert)
|
2017-04-19 08:12:12 -06:00
|
|
|
{
|
|
|
|
encoderType = encType;
|
|
|
|
scalingFactor = scaling;
|
2017-04-23 04:17:32 -06:00
|
|
|
if (invert == 1)
|
|
|
|
{
|
|
|
|
encoderInvert = -1;
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
encoderInvert = 1;
|
|
|
|
}
|
2017-05-02 12:35:16 -06:00
|
|
|
|
2017-05-03 13:33:26 -06:00
|
|
|
// encoderType = 0; // TEVE 2017-04-20 Disabling the differential channels. They take too much time to read.
|
2017-03-18 15:52:10 -06:00
|
|
|
}
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
void MovementEncoder::loadMdlEncoderId(MdlSpiEncoders encoder)
|
2018-01-29 13:59:02 -07:00
|
|
|
{
|
|
|
|
mdlEncoder = encoder;
|
|
|
|
}
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
void MovementEncoder::setPosition(long newPosition)
|
2017-04-19 08:12:12 -06:00
|
|
|
{
|
2020-02-13 12:08:20 -07:00
|
|
|
#if defined(RAMPS_V14) || defined(FARMDUINO_V10) || defined(FARMDUINO_EXP_V20)
|
2018-01-31 01:29:27 -07:00
|
|
|
position = newPosition;
|
|
|
|
#endif
|
|
|
|
|
2020-02-13 12:08:20 -07:00
|
|
|
#if defined(FARMDUINO_V14) || defined(FARMDUINO_V30)
|
2018-01-31 01:29:27 -07:00
|
|
|
if (newPosition == 0)
|
|
|
|
{
|
|
|
|
position = newPosition;
|
|
|
|
|
|
|
|
const byte reset_cmd = 0x00;
|
|
|
|
|
2020-02-13 12:08:20 -07:00
|
|
|
SPI.beginTransaction(SPISettings(SPI_CLOCK_DIV4, MSBFIRST, SPI_MODE0));
|
2018-01-31 01:29:27 -07:00
|
|
|
digitalWrite(NSS_PIN, LOW);
|
2020-02-13 12:08:20 -07:00
|
|
|
delayMicroseconds(2);
|
2018-01-31 01:29:27 -07:00
|
|
|
SPI.transfer(reset_cmd | (mdlEncoder << mdl_spi_encoder_offset));
|
2020-02-13 12:08:20 -07:00
|
|
|
delayMicroseconds(5);
|
2018-01-31 01:29:27 -07:00
|
|
|
digitalWrite(NSS_PIN, HIGH);
|
2020-02-13 12:08:20 -07:00
|
|
|
SPI.endTransaction();
|
2018-01-31 01:29:27 -07:00
|
|
|
}
|
|
|
|
#endif
|
2015-10-29 16:50:09 -06:00
|
|
|
}
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
long MovementEncoder::currentPosition()
|
2017-04-19 08:12:12 -06:00
|
|
|
{
|
2018-01-31 01:29:27 -07:00
|
|
|
|
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
// Apply scaling to the output of the encoder, except when scaling is zero or lower
|
2018-01-03 22:27:03 -07:00
|
|
|
if (scalingFactor == 10000 || scalingFactor <= 0)
|
2017-04-19 08:12:12 -06:00
|
|
|
{
|
2017-04-23 04:17:32 -06:00
|
|
|
return position * encoderInvert;
|
2017-04-19 08:12:12 -06:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2020-02-13 12:08:20 -07:00
|
|
|
#if defined(FARMDUINO_V14) || defined(FARMDUINO_V30)
|
2019-04-24 20:01:07 -06:00
|
|
|
floatScalingFactor = scalingFactor / 40000.0;
|
|
|
|
return position * floatScalingFactor * encoderInvert;
|
2018-04-25 22:56:46 -06:00
|
|
|
#endif
|
2019-04-24 20:01:07 -06:00
|
|
|
floatScalingFactor = scalingFactor / 10000.0;
|
|
|
|
return position * floatScalingFactor * encoderInvert;
|
2017-04-19 08:12:12 -06:00
|
|
|
}
|
2015-10-29 16:50:09 -06:00
|
|
|
}
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
long MovementEncoder::currentPositionRaw()
|
2017-05-05 15:01:18 -06:00
|
|
|
{
|
|
|
|
return position * encoderInvert;
|
|
|
|
}
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
void MovementEncoder::checkEncoder(bool channelA, bool channelB, bool channelAQ, bool channelBQ)
|
2017-05-28 14:54:58 -06:00
|
|
|
{
|
2018-01-29 13:59:02 -07:00
|
|
|
#if defined(RAMPS_V14) || defined(FARMDUINO_V10)
|
|
|
|
shiftChannels();
|
|
|
|
setChannels(channelA, channelB, channelAQ, channelBQ);
|
|
|
|
processEncoder();
|
|
|
|
#endif
|
|
|
|
|
2020-02-13 12:08:20 -07:00
|
|
|
#if defined(FARMDUINO_V14) || defined(FARMDUINO_V30)
|
2018-01-29 13:59:02 -07:00
|
|
|
processEncoder();
|
|
|
|
#endif
|
|
|
|
|
2017-05-28 14:54:58 -06:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2017-05-05 15:01:18 -06:00
|
|
|
/* Check the encoder channels for movement according to this specification
|
2015-10-29 16:50:09 -06:00
|
|
|
________ ________
|
|
|
|
Channel A / \ / \
|
|
|
|
_____/ \________/ \________
|
|
|
|
________ ________
|
|
|
|
Channel B / \ / \
|
|
|
|
__________/ \________/ \____
|
|
|
|
__
|
|
|
|
Channel I / \
|
|
|
|
____________________/ \___________________
|
|
|
|
|
|
|
|
rotation ----------------------------------------------------->
|
|
|
|
|
|
|
|
*/
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
void MovementEncoder::processEncoder()
|
2017-04-19 08:12:12 -06:00
|
|
|
{
|
|
|
|
|
2018-01-29 13:59:02 -07:00
|
|
|
#if defined(RAMPS_V14) || defined(FARMDUINO_V10)
|
2017-04-19 08:12:12 -06:00
|
|
|
|
2018-01-29 13:59:02 -07:00
|
|
|
// Detect edges on the A channel when the B channel is high
|
|
|
|
if (curValChannelB == true && prvValChannelA == false && curValChannelA == true)
|
|
|
|
{
|
|
|
|
//delta--;
|
|
|
|
position--;
|
|
|
|
}
|
|
|
|
if (curValChannelB == true && prvValChannelA == true && curValChannelA == false)
|
|
|
|
{
|
|
|
|
//delta++;
|
|
|
|
position++;
|
|
|
|
}
|
2017-04-19 08:12:12 -06:00
|
|
|
|
2018-01-29 13:59:02 -07:00
|
|
|
#endif
|
2017-05-14 14:09:07 -06:00
|
|
|
|
2018-01-29 13:59:02 -07:00
|
|
|
// If using farmduino, revision 1.4, use the SPI interface to read from the Motor Dynamics Lab chip
|
2020-02-13 12:08:20 -07:00
|
|
|
#if defined(FARMDUINO_V14) || defined(FARMDUINO_V30)
|
2018-01-29 13:59:02 -07:00
|
|
|
const byte read_cmd = 0x0F;
|
|
|
|
int readSize = 4;
|
|
|
|
long encoderVal = 0;
|
2017-04-22 13:47:15 -06:00
|
|
|
|
2020-02-13 12:08:20 -07:00
|
|
|
SPI.beginTransaction(SPISettings(SPI_CLOCK_DIV4, MSBFIRST, SPI_MODE0));
|
2018-01-29 13:59:02 -07:00
|
|
|
digitalWrite(NSS_PIN, LOW);
|
2020-02-13 12:08:20 -07:00
|
|
|
delayMicroseconds(2);
|
2018-01-29 13:59:02 -07:00
|
|
|
SPI.transfer(read_cmd | (mdlEncoder << mdl_spi_encoder_offset));
|
2020-02-13 12:08:20 -07:00
|
|
|
delayMicroseconds(5);
|
2017-04-19 08:12:12 -06:00
|
|
|
|
2018-01-29 13:59:02 -07:00
|
|
|
for (int i = 0; i < readSize; ++i)
|
|
|
|
{
|
|
|
|
encoderVal <<= 8;
|
|
|
|
encoderVal |= SPI.transfer(0x01);
|
|
|
|
}
|
|
|
|
|
|
|
|
digitalWrite(NSS_PIN, HIGH);
|
2020-02-13 12:08:20 -07:00
|
|
|
SPI.endTransaction();
|
2018-01-29 13:59:02 -07:00
|
|
|
position = encoderVal;
|
|
|
|
#endif
|
2017-04-19 08:12:12 -06:00
|
|
|
|
2015-10-29 16:50:09 -06:00
|
|
|
}
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
void MovementEncoder::readChannels()
|
2017-04-19 08:12:12 -06:00
|
|
|
{
|
2017-04-15 15:32:01 -06:00
|
|
|
|
2018-01-29 13:59:02 -07:00
|
|
|
#if defined(RAMPS_V14) || defined(FARMDUINO_V10)
|
2017-04-19 08:12:12 -06:00
|
|
|
// read the new values from the coder
|
2017-04-15 15:32:01 -06:00
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
readChannelA = digitalRead(pinChannelA);
|
|
|
|
readChannelB = digitalRead(pinChannelB);
|
2017-04-20 12:31:09 -06:00
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
if (encoderType == 1)
|
|
|
|
{
|
2017-04-23 12:50:33 -06:00
|
|
|
readChannelAQ = digitalRead(pinChannelAQ);
|
|
|
|
readChannelBQ = digitalRead(pinChannelBQ);
|
2017-03-18 15:52:10 -06:00
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
// differential encoder
|
|
|
|
if ((readChannelA ^ readChannelAQ) && (readChannelB ^ readChannelBQ))
|
|
|
|
{
|
|
|
|
curValChannelA = readChannelA;
|
|
|
|
curValChannelB = readChannelB;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2017-04-17 13:25:27 -06:00
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
// encoderType <= 0
|
|
|
|
// non-differential incremental encoder
|
|
|
|
curValChannelA = readChannelA;
|
|
|
|
curValChannelB = readChannelB;
|
|
|
|
}
|
2018-01-29 13:59:02 -07:00
|
|
|
#endif
|
|
|
|
|
2017-05-02 12:35:16 -06:00
|
|
|
}
|
2017-03-20 16:08:23 -06:00
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
void MovementEncoder::setChannels(bool channelA, bool channelB, bool channelAQ, bool channelBQ)
|
2017-05-02 12:35:16 -06:00
|
|
|
{
|
|
|
|
// read the new values from the coder
|
2017-03-27 13:58:25 -06:00
|
|
|
|
2017-05-02 12:35:16 -06:00
|
|
|
if (encoderType == 1)
|
|
|
|
{
|
|
|
|
// differential encoder
|
|
|
|
if ((channelA ^ channelAQ) && (channelB ^ channelBQ))
|
|
|
|
{
|
|
|
|
curValChannelA = channelA;
|
|
|
|
curValChannelB = channelB;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
// encoderType <= 0
|
|
|
|
// non-differential incremental encoder
|
|
|
|
curValChannelA = channelA;
|
|
|
|
curValChannelB = channelB;
|
|
|
|
}
|
2015-10-29 16:50:09 -06:00
|
|
|
}
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
void MovementEncoder::shiftChannels()
|
2017-04-19 08:12:12 -06:00
|
|
|
{
|
2017-04-15 15:32:01 -06:00
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
// Save the current enoder status to later on compare with new values
|
2017-04-15 15:32:01 -06:00
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
prvValChannelA = curValChannelA;
|
|
|
|
prvValChannelB = curValChannelB;
|
2015-10-29 16:50:09 -06:00
|
|
|
}
|
2019-10-22 12:06:55 -06:00
|
|
|
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
void MovementEncoder::setEnable(bool enable)
|
2019-10-22 12:06:55 -06:00
|
|
|
{
|
|
|
|
encoderEnabled = enable;
|
|
|
|
}
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
void MovementEncoder::setStepDecay(float stepDecay)
|
2019-10-22 12:06:55 -06:00
|
|
|
{
|
|
|
|
encoderStepDecay = stepDecay;
|
|
|
|
}
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
void MovementEncoder::setMovementDirection(bool up)
|
2019-10-22 12:06:55 -06:00
|
|
|
{
|
|
|
|
encoderMovementUp = up;
|
|
|
|
}
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
float MovementEncoder::getMissedSteps()
|
2019-10-22 12:06:55 -06:00
|
|
|
{
|
|
|
|
return missedSteps;
|
|
|
|
}
|
|
|
|
|
2019-12-02 14:07:28 -07:00
|
|
|
void MovementEncoder::checkMissedSteps()
|
2019-10-22 12:06:55 -06:00
|
|
|
{
|
2020-02-13 12:08:20 -07:00
|
|
|
#if !defined(FARMDUINO_EXP_V20)
|
2019-10-22 12:06:55 -06:00
|
|
|
if (encoderEnabled)
|
|
|
|
{
|
|
|
|
bool stepMissed = false;
|
|
|
|
|
|
|
|
// Decrease amount of missed steps if there are no missed step
|
|
|
|
if (missedSteps > 0)
|
|
|
|
{
|
|
|
|
(missedSteps) -= (encoderStepDecay);
|
|
|
|
}
|
|
|
|
|
|
|
|
// Check if the encoder goes in the wrong direction or nothing moved
|
|
|
|
if ((encoderMovementUp && encoderLastPosition > currentPositionRaw()) ||
|
|
|
|
(!encoderMovementUp && encoderLastPosition < currentPositionRaw()))
|
|
|
|
{
|
|
|
|
stepMissed = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (stepMissed && missedSteps < 32000)
|
|
|
|
{
|
|
|
|
(missedSteps)++;
|
|
|
|
}
|
|
|
|
|
|
|
|
encoderLastPosition = currentPositionRaw();
|
|
|
|
//axis->setLastPosition(axis->currentPosition());
|
|
|
|
}
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
2019-10-22 12:17:50 -06:00
|
|
|
/*
|
2020-02-11 13:32:04 -07:00
|
|
|
#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30)
|
2019-10-22 12:06:55 -06:00
|
|
|
|
|
|
|
if (encoderEnabled) {
|
|
|
|
if (axis->stallDetected()) {
|
|
|
|
// In case of stall detection, count this as a missed step
|
|
|
|
(*missedSteps)++;
|
2019-10-22 12:17:50 -06:00
|
|
|
//axis->setCurrentPosition(*lastPosition);
|
2019-10-22 12:06:55 -06:00
|
|
|
}
|
|
|
|
else {
|
|
|
|
// Decrease amount of missed steps if there are no missed step
|
|
|
|
if (missedSteps > 0)
|
|
|
|
{
|
|
|
|
(missedSteps) -= (encoderStepDecay);
|
|
|
|
}
|
2019-10-22 12:17:50 -06:00
|
|
|
setPosition(axis->currentPosition());
|
2019-10-22 12:06:55 -06:00
|
|
|
//axis->setLastPosition(axis->currentPosition());
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|
2019-10-22 12:17:50 -06:00
|
|
|
*/
|
2020-02-13 12:08:20 -07:00
|
|
|
}
|