You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
369 lines
13 KiB
369 lines
13 KiB
/*
|
|
* Copyright (c) 2021, Erich Styger
|
|
*
|
|
* SPDX-License-Identifier: BSD-3-Clause
|
|
*/
|
|
|
|
#include "QuadCounter.h"
|
|
#include "McuUtility.h"
|
|
#include "McuShell.h"
|
|
#include "McuGPIO.h"
|
|
|
|
#define QuadCounter_GET_C1_PIN_LEFT() (McuGPIO_GetValue(C1_Left))
|
|
#define QuadCounter_GET_C1_PIN_RIGHT() (McuGPIO_GetValue(C1_Right))
|
|
#define QuadCounter_GET_C2_PIN_LEFT() (McuGPIO_GetValue(C2_Left))
|
|
#define QuadCounter_GET_C2_PIN_RIGHT() (McuGPIO_GetValue(C2_Right))
|
|
|
|
#define QuadCounter_GET_C1_PIN_LEFT() (McuGPIO_GetValue(C1_Left))
|
|
#define QuadCounter_GET_C1_PIN_RIGHT() (McuGPIO_GetValue(C1_Right))
|
|
#define QuadCounter_GET_C2_PIN_LEFT() (McuGPIO_GetValue(C2_Left))
|
|
#define QuadCounter_GET_C2_PIN_RIGHT() (McuGPIO_GetValue(C2_Right))
|
|
|
|
#if QuadCounter_SWAP_PINS
|
|
#define QuadCounter_GET_C1_C2_PINS_LEFT() ((QuadCounter_GET_C2_PIN_LEFT()?2:0)|(QuadCounter_GET_C1_PIN_LEFT()?1:0))
|
|
#define QuadCounter_GET_C1_C2_PINS_RIGHT() ((QuadCounter_GET_C2_PIN_RIGHT()?2:0)|(QuadCounter_GET_C1_PIN_RIGHT()?1:0))
|
|
#define QuadCounter_GET_C1_C2_PINS_SWAPPED_LEFT() ((QuadCounter_GET_C1_PIN_LEFT()?2:0)|(QuadCounter_GET_C2_PIN_LEFT()?1:0))
|
|
#define QuadCounter_GET_C1_C2_PINS_SWAPPED_RIGHT() ((QuadCounter_GET_C1_PIN_RIGHT()?2:0)|(QuadCounter_GET_C2_PIN_RIGHT()?1:0))
|
|
#else
|
|
#define QuadCounter_GET_C1_C2_PINS_LEFT() ((QuadCounter_GET_C1_PIN_LEFT()?2:0)|(QuadCounter_GET_C2_PIN_LEFT()?1:0))
|
|
#define QuadCounter_GET_C1_C2_PINS_RIGHT() ((QuadCounter_GET_C1_PIN_RIGHT()?2:0)|(QuadCounter_GET_C2_PIN_RIGHT()?1:0))
|
|
#define QuadCounter_GET_C1_C2_PINS_SWAPPED_LEFT() ((QuadCounter_GET_C2_PIN_LEFT()?2:0)|(QuadCounter_GET_C1_PIN_LEFT()?1:0))
|
|
#define QuadCounter_GET_C1_C2_PINS_SWAPPED_RIGHT() ((QuadCounter_GET_C2_PIN_RIGHT()?2:0)|(QuadCounter_GET_C1_PIN_RIGHT()?1:0))
|
|
#endif
|
|
|
|
static McuGPIO_Handle_t C1_Left, C2_Left, C1_Right, C2_Right;
|
|
#if QuadCounter_SWAP_PINS_AT_RUNTIME
|
|
static bool QuadCounter_swappedPins_Left = FALSE;
|
|
static bool QuadCounter_swappedPins_Right = FALSE;
|
|
#endif
|
|
|
|
/* The decoder has 4 different states, together with the previous state the table has 16 entries.
|
|
The value in the table (0,1,-1) indicates the steps taken since previous sample. */
|
|
#define QUAD_ERROR 3 /*!< Value to indicate an error in impulse detection. Has to be different from 0,1,-1 */
|
|
|
|
static const signed char QuadCounter_Quad_Table[4][4] =
|
|
{ /* prev new */
|
|
{ /* c1 c2 c1 c2 */
|
|
0, /* 0 0 0 0 no change or missed a step? */
|
|
1, /* 0 0 0 1 */
|
|
-1, /* 0 0 1 0 */
|
|
QUAD_ERROR /* 0 0 1 1 error, lost impulse */
|
|
},
|
|
{ /* c1 c2 c1 c2 */
|
|
-1, /* 0 1 0 0 */
|
|
0, /* 0 1 0 1 no change or missed a step? */
|
|
QUAD_ERROR, /* 0 1 1 0 error, lost impulse */
|
|
1 /* 0 1 1 1 */
|
|
},
|
|
{ /* c1 c2 c1 c2 */
|
|
1, /* 1 0 0 0 */
|
|
QUAD_ERROR, /* 1 0 0 1 error, lost impulse */
|
|
0, /* 1 0 1 0 no change or missed a step? */
|
|
-1 /* 1 0 1 1 */
|
|
},
|
|
{ /* c1 c2 c1 c2 */
|
|
QUAD_ERROR, /* 1 1 0 0 error, lost impulse */
|
|
-1, /* 1 1 0 1 */
|
|
1, /* 1 1 1 0 */
|
|
0 /* 1 1 1 1 no change or missed a step? */
|
|
}
|
|
};
|
|
static uint8_t QuadCounter_last_quadrature_value_left; /*! Value of left C1&C2 during last round. */
|
|
static uint8_t QuadCounter_last_quadrature_value_right; /*! Value of right C1&C2 during last round. */
|
|
|
|
static QuadCounter_QuadCntrType QuadCounter_currPos_Left = 0; /*!< Current left position */
|
|
static QuadCounter_QuadCntrType QuadCounter_currPos_Right = 0; /*!< Current right position */
|
|
|
|
static uint32_t QuadCounter_errorsLeft, QuadCounter_errorsRight;
|
|
|
|
uint32_t QuadCounter_GetErrorsLeft(void) {
|
|
return QuadCounter_errorsLeft;
|
|
}
|
|
|
|
uint32_t QuadCounter_GetErrorsRight(void) {
|
|
return QuadCounter_errorsRight;
|
|
}
|
|
|
|
/*
|
|
** ===================================================================
|
|
** Method : SetPos (component QuadCounter)
|
|
**
|
|
** Description :
|
|
** Sets the position information. Can be used as well to reset
|
|
** the position information.
|
|
** Parameters :
|
|
** NAME - DESCRIPTION
|
|
** pos - Position value to be set.
|
|
** Returns : Nothing
|
|
** ===================================================================
|
|
*/
|
|
void QuadCounter_SetPosLeft(QuadCounter_QuadCntrType pos)
|
|
{
|
|
QuadCounter_currPos_Left = pos;
|
|
}
|
|
|
|
void QuadCounter_SetPosRight(QuadCounter_QuadCntrType pos)
|
|
{
|
|
QuadCounter_currPos_Right = pos;
|
|
}
|
|
|
|
/*
|
|
** ===================================================================
|
|
** Method : GetPos (component QuadCounter)
|
|
**
|
|
** Description :
|
|
** Returns the current position based on the encoder tracking.
|
|
** Parameters : None
|
|
** Returns :
|
|
** --- - position
|
|
** ===================================================================
|
|
*/
|
|
QuadCounter_QuadCntrType QuadCounter_GetPosLeft(void)
|
|
{
|
|
return QuadCounter_currPos_Left;
|
|
}
|
|
|
|
QuadCounter_QuadCntrType QuadCounter_GetPosRight(void)
|
|
{
|
|
return QuadCounter_currPos_Right;
|
|
}
|
|
|
|
#if QuadCounter_SWAP_PINS_AT_RUNTIME
|
|
void QuadCounter_SwapPinsLeft(bool swap) {
|
|
QuadCounter_swappedPins_Left = swap;
|
|
}
|
|
|
|
void QuadCounter_SwapPinsRight(bool swap) {
|
|
QuadCounter_swappedPins_Right = swap;
|
|
}
|
|
#endif
|
|
|
|
/*
|
|
** ===================================================================
|
|
** Method : GetVal (component QuadCounter)
|
|
**
|
|
** Description :
|
|
** Returns the quadrature value (0, 1, 2 or 3)
|
|
** Parameters : None
|
|
** Returns :
|
|
** --- - Quadrature value (0-3)
|
|
** ===================================================================
|
|
*/
|
|
uint8_t QuadCounter_GetValLeft(void)
|
|
{
|
|
#if QuadCounter_SWAP_PINS_AT_RUNTIME
|
|
if (QuadCounter_swappedPins_Left) {
|
|
return QuadCounter_GET_C1_C2_PINS_SWAPPED_LEFT();
|
|
} else {
|
|
return QuadCounter_GET_C1_C2_PINS_LEFT();
|
|
}
|
|
#else
|
|
return QuadCounter_GET_C1_C2_PINS_LEFT();
|
|
#endif
|
|
}
|
|
|
|
uint8_t QuadCounter_GetValRight(void)
|
|
{
|
|
#if QuadCounter_SWAP_PINS_AT_RUNTIME
|
|
if (QuadCounter_swappedPins_Right) {
|
|
return QuadCounter_GET_C1_C2_PINS_SWAPPED_RIGHT();
|
|
} else {
|
|
return QuadCounter_GET_C1_C2_PINS_RIGHT();
|
|
}
|
|
#else
|
|
return QuadCounter_GET_C1_C2_PINS_RIGHT();
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
** ===================================================================
|
|
** Method : Sample (component QuadCounter)
|
|
**
|
|
** Description :
|
|
** Call this method to periodically sample the signals.
|
|
** Parameters : None
|
|
** Returns : Nothing
|
|
** ===================================================================
|
|
*/
|
|
void QuadCounter_Sample(void)
|
|
{
|
|
signed char new_step;
|
|
uint8_t c12; /* value of the two sensor input */
|
|
|
|
/* left */
|
|
c12 = QuadCounter_GetValLeft();
|
|
new_step = QuadCounter_Quad_Table[QuadCounter_last_quadrature_value_left][c12];
|
|
QuadCounter_last_quadrature_value_left = c12;
|
|
if (new_step == QUAD_ERROR) {
|
|
QuadCounter_errorsLeft++;
|
|
} else if (new_step != 0) {
|
|
QuadCounter_currPos_Left += new_step;
|
|
}
|
|
/* right */
|
|
c12 = QuadCounter_GetValRight();
|
|
new_step = QuadCounter_Quad_Table[QuadCounter_last_quadrature_value_right][c12];
|
|
QuadCounter_last_quadrature_value_right = c12;
|
|
if (new_step == QUAD_ERROR) {
|
|
QuadCounter_errorsRight++;
|
|
} else if (new_step != 0) {
|
|
QuadCounter_currPos_Right += new_step;
|
|
}
|
|
}
|
|
|
|
/*
|
|
** ===================================================================
|
|
** Method : ParseCommand (component QuadCounter)
|
|
**
|
|
** Description :
|
|
** Handler to process shell commands
|
|
** Parameters :
|
|
** NAME - DESCRIPTION
|
|
** cmd - Command string to be parsed
|
|
** * handled - Pointer to boolean. The handler
|
|
** sets this variable to TRUE if command was
|
|
** handled, otherwise let it untouched.
|
|
** io - Pointer to I/O structure
|
|
** Returns :
|
|
** --- - Error code
|
|
** ===================================================================
|
|
*/
|
|
/*!
|
|
* \brief Parses a command
|
|
* \param cmd Command string to be parsed
|
|
* \param handled Sets this variable to TRUE if command was handled
|
|
* \param io I/O stream to be used for input/output
|
|
* \return Error code, ERR_OK if everything was fine
|
|
*/
|
|
uint8_t QuadCounter_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io)
|
|
{
|
|
uint8_t res=ERR_OK;
|
|
|
|
if (McuUtility_strcmp((const char*)cmd, McuShell_CMD_HELP)==0 || McuUtility_strcmp((const char *)cmd, "QuadCounter help")==0) {
|
|
McuShell_SendHelpStr((const unsigned char*)"QuadCounter", (const unsigned char*)"QuadCounter command group\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((const unsigned char*)" help|status", (const unsigned char*)"Print help or status information\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((const unsigned char*)" reset", (const unsigned char*)"Reset the current position counter\r\n", io->stdOut);
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strcmp((const char*)cmd, McuShell_CMD_STATUS)==0 || McuUtility_strcmp((const char*)cmd, "QuadCounter status")==0) {
|
|
McuShell_SendStatusStr((const unsigned char*)"QuadCounter", (const unsigned char*)"Quadrature counter status", io->stdOut);
|
|
McuShell_SendStatusStr((const unsigned char*)" pos left", (const unsigned char*)"", io->stdOut);
|
|
McuShell_SendNum32s((int32_t)QuadCounter_currPos_Left, io->stdOut);
|
|
McuShell_SendStr((const unsigned char*)"\r\n", io->stdOut);
|
|
|
|
McuShell_SendStatusStr((const unsigned char*)" error left", (const unsigned char*)"", io->stdOut);
|
|
McuShell_SendNum32s((int32_t)QuadCounter_errorsLeft, io->stdOut);
|
|
McuShell_SendStr((const unsigned char*)"\r\n", io->stdOut);
|
|
|
|
McuShell_SendStatusStr((const unsigned char*)" left C1 C2", (const unsigned char*)"", io->stdOut);
|
|
if (QuadCounter_GET_C1_PIN_LEFT()) {
|
|
McuShell_SendStr((const unsigned char*)"1 ", io->stdOut);
|
|
} else {
|
|
McuShell_SendStr((const unsigned char*)"0 ", io->stdOut);
|
|
}
|
|
if (QuadCounter_GET_C2_PIN_LEFT()) {
|
|
McuShell_SendStr((const unsigned char*)"1\r\n", io->stdOut);
|
|
} else {
|
|
McuShell_SendStr((const unsigned char*)"0\r\n", io->stdOut);
|
|
}
|
|
McuShell_SendStatusStr((const unsigned char*)" pos right", (const unsigned char*)"", io->stdOut);
|
|
McuShell_SendNum32s((int32_t)QuadCounter_currPos_Right, io->stdOut);
|
|
McuShell_SendStr((const unsigned char*)"\r\n", io->stdOut);
|
|
|
|
McuShell_SendStatusStr((const unsigned char*)" error right", (const unsigned char*)"", io->stdOut);
|
|
McuShell_SendNum32s((int32_t)QuadCounter_errorsRight, io->stdOut);
|
|
McuShell_SendStr((const unsigned char*)"\r\n", io->stdOut);
|
|
|
|
McuShell_SendStatusStr((const unsigned char*)" right C1 C2", (const unsigned char*)"", io->stdOut);
|
|
if (QuadCounter_GET_C1_PIN_RIGHT()) {
|
|
McuShell_SendStr((const unsigned char*)"1 ", io->stdOut);
|
|
} else {
|
|
McuShell_SendStr((const unsigned char*)"0 ", io->stdOut);
|
|
}
|
|
if (QuadCounter_GET_C2_PIN_RIGHT()) {
|
|
McuShell_SendStr((const unsigned char*)"1\r\n", io->stdOut);
|
|
} else {
|
|
McuShell_SendStr((const unsigned char*)"0\r\n", io->stdOut);
|
|
}
|
|
*handled = TRUE;
|
|
} else if (McuUtility_strcmp((const char*)cmd, "QuadCounter reset")==0) {
|
|
QuadCounter_SetPosLeft(0);
|
|
QuadCounter_SetPosRight(0);
|
|
*handled = TRUE;
|
|
}
|
|
return res;
|
|
}
|
|
|
|
/*
|
|
** ===================================================================
|
|
** Method : Deinit (component QuadCounter)
|
|
**
|
|
** Description :
|
|
** Module de-initialization method
|
|
** Parameters : None
|
|
** Returns : Nothing
|
|
** ===================================================================
|
|
*/
|
|
void QuadCounter_Deinit(void)
|
|
{
|
|
C1_Left = McuGPIO_DeinitGPIO(C1_Left);
|
|
C2_Left = McuGPIO_DeinitGPIO(C2_Left);
|
|
C1_Right = McuGPIO_DeinitGPIO(C1_Right);
|
|
C2_Right = McuGPIO_DeinitGPIO(C2_Right);
|
|
}
|
|
|
|
void QuadCounter_EnablePullups(void) {
|
|
McuGPIO_SetPullResistor(C1_Left, McuGPIO_PULL_UP);
|
|
McuGPIO_SetPullResistor(C1_Right, McuGPIO_PULL_UP);
|
|
McuGPIO_SetPullResistor(C2_Left, McuGPIO_PULL_UP);
|
|
McuGPIO_SetPullResistor(C2_Right, McuGPIO_PULL_UP);
|
|
}
|
|
|
|
static void QuadCounter_PinInit(void) {
|
|
McuGPIO_Config_t gpioConfig;
|
|
|
|
/*
|
|
* Left: PTC16, PTC17
|
|
* Right: PTC10, PTC11
|
|
*/
|
|
McuGPIO_GetDefaultConfig(&gpioConfig);
|
|
gpioConfig.hw.gpio = GPIOC;
|
|
gpioConfig.hw.port = PORTC;
|
|
gpioConfig.hw.pin = 16;
|
|
gpioConfig.isInput = true;
|
|
C1_Left = McuGPIO_InitGPIO(&gpioConfig);
|
|
gpioConfig.hw.pin = 17;
|
|
C2_Left = McuGPIO_InitGPIO(&gpioConfig);
|
|
gpioConfig.hw.pin = 10;
|
|
C1_Right = McuGPIO_InitGPIO(&gpioConfig);
|
|
gpioConfig.hw.pin = 11;
|
|
C2_Right = McuGPIO_InitGPIO(&gpioConfig);
|
|
}
|
|
|
|
/*
|
|
** ===================================================================
|
|
** Method : Init (component QuadCounter)
|
|
**
|
|
** Description :
|
|
** Module initialization method
|
|
** Parameters : None
|
|
** Returns : Nothing
|
|
** ===================================================================
|
|
*/
|
|
void QuadCounter_Init(void)
|
|
{
|
|
QuadCounter_PinInit();
|
|
QuadCounter_currPos_Left = 0;
|
|
QuadCounter_currPos_Right = 0;
|
|
QuadCounter_last_quadrature_value_left = QuadCounter_GET_C1_C2_PINS_LEFT();
|
|
QuadCounter_last_quadrature_value_right = QuadCounter_GET_C1_C2_PINS_RIGHT();
|
|
#if QuadCounter_SWAP_PINS_AT_RUNTIME
|
|
QuadCounter_swappedPins_Left = FALSE;
|
|
QuadCounter_swappedPins_Right = FALSE;
|
|
#endif
|
|
}
|
|
|
|
|
|
/* END QuadCounter. */
|
|
|
|
/*!
|
|
** @}
|
|
*/
|
|
|