/* * 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. */ /*! ** @} */