Added ST_INTERFACES library.
Fork of X_NUCLEO_IHM04A1 by
Components/l6206/l6206_class.cpp@0:2cb6ce8e07bd, 2016-05-03 (annotated)
- Committer:
- brdirais
- Date:
- Tue May 03 11:16:57 2016 +0000
- Revision:
- 0:2cb6ce8e07bd
- Child:
- 1:2597a6165252
Initial version of X-NUCLEO-IHM04A1 library
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
brdirais | 0:2cb6ce8e07bd | 1 | /** |
brdirais | 0:2cb6ce8e07bd | 2 | ****************************************************************************** |
brdirais | 0:2cb6ce8e07bd | 3 | * @file l6206_class.cpp |
brdirais | 0:2cb6ce8e07bd | 4 | * @author IPC Rennes |
brdirais | 0:2cb6ce8e07bd | 5 | * @version V1.1.0 |
brdirais | 0:2cb6ce8e07bd | 6 | * @date March 02, 2016 |
brdirais | 0:2cb6ce8e07bd | 7 | * @brief L6206 driver (dual full bridge driver) |
brdirais | 0:2cb6ce8e07bd | 8 | * @note (C) COPYRIGHT 2015 STMicroelectronics |
brdirais | 0:2cb6ce8e07bd | 9 | ****************************************************************************** |
brdirais | 0:2cb6ce8e07bd | 10 | * @attention |
brdirais | 0:2cb6ce8e07bd | 11 | * |
brdirais | 0:2cb6ce8e07bd | 12 | * <h2><center>© COPYRIGHT(c) 2015 STMicroelectronics</center></h2> |
brdirais | 0:2cb6ce8e07bd | 13 | * |
brdirais | 0:2cb6ce8e07bd | 14 | * Redistribution and use in source and binary forms, with or without modification, |
brdirais | 0:2cb6ce8e07bd | 15 | * are permitted provided that the following conditions are met: |
brdirais | 0:2cb6ce8e07bd | 16 | * 1. Redistributions of source code must retain the above copyright notice, |
brdirais | 0:2cb6ce8e07bd | 17 | * this list of conditions and the following disclaimer. |
brdirais | 0:2cb6ce8e07bd | 18 | * 2. Redistributions in binary form must reproduce the above copyright notice, |
brdirais | 0:2cb6ce8e07bd | 19 | * this list of conditions and the following disclaimer in the documentation |
brdirais | 0:2cb6ce8e07bd | 20 | * and/or other materials provided with the distribution. |
brdirais | 0:2cb6ce8e07bd | 21 | * 3. Neither the name of STMicroelectronics nor the names of its contributors |
brdirais | 0:2cb6ce8e07bd | 22 | * may be used to endorse or promote products derived from this software |
brdirais | 0:2cb6ce8e07bd | 23 | * without specific prior written permission. |
brdirais | 0:2cb6ce8e07bd | 24 | * |
brdirais | 0:2cb6ce8e07bd | 25 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
brdirais | 0:2cb6ce8e07bd | 26 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
brdirais | 0:2cb6ce8e07bd | 27 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
brdirais | 0:2cb6ce8e07bd | 28 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE |
brdirais | 0:2cb6ce8e07bd | 29 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
brdirais | 0:2cb6ce8e07bd | 30 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR |
brdirais | 0:2cb6ce8e07bd | 31 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
brdirais | 0:2cb6ce8e07bd | 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, |
brdirais | 0:2cb6ce8e07bd | 33 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
brdirais | 0:2cb6ce8e07bd | 34 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
brdirais | 0:2cb6ce8e07bd | 35 | * |
brdirais | 0:2cb6ce8e07bd | 36 | ****************************************************************************** |
brdirais | 0:2cb6ce8e07bd | 37 | */ |
brdirais | 0:2cb6ce8e07bd | 38 | |
brdirais | 0:2cb6ce8e07bd | 39 | |
brdirais | 0:2cb6ce8e07bd | 40 | /* Generated with STM32CubeTOO -----------------------------------------------*/ |
brdirais | 0:2cb6ce8e07bd | 41 | |
brdirais | 0:2cb6ce8e07bd | 42 | |
brdirais | 0:2cb6ce8e07bd | 43 | /* Revision ------------------------------------------------------------------*/ |
brdirais | 0:2cb6ce8e07bd | 44 | /* |
brdirais | 0:2cb6ce8e07bd | 45 | Repository: http://svn.x-nucleodev.codex.cro.st.com/svnroot/X-NucleoDev |
brdirais | 0:2cb6ce8e07bd | 46 | Branch/Trunk/Tag: trunk |
brdirais | 0:2cb6ce8e07bd | 47 | Based on: X-CUBE-SPN4/trunk/Drivers/BSP/Components/l6206/l6206.c |
brdirais | 0:2cb6ce8e07bd | 48 | Revision: 0 |
brdirais | 0:2cb6ce8e07bd | 49 | */ |
brdirais | 0:2cb6ce8e07bd | 50 | |
brdirais | 0:2cb6ce8e07bd | 51 | /* Includes ------------------------------------------------------------------*/ |
brdirais | 0:2cb6ce8e07bd | 52 | #include "l6206_class.h" |
brdirais | 0:2cb6ce8e07bd | 53 | #include "l6206.h" |
brdirais | 0:2cb6ce8e07bd | 54 | #include "string.h" |
brdirais | 0:2cb6ce8e07bd | 55 | |
brdirais | 0:2cb6ce8e07bd | 56 | /* Private constants ---------------------------------------------------------*/ |
brdirais | 0:2cb6ce8e07bd | 57 | |
brdirais | 0:2cb6ce8e07bd | 58 | |
brdirais | 0:2cb6ce8e07bd | 59 | /* Private variables ---------------------------------------------------------*/ |
brdirais | 0:2cb6ce8e07bd | 60 | static uint8_t l6206ArrayNbMaxMotorsByConfig[PARALLELING_END_ENUM] = {2,3,3,4,2,3,2,3,2,1,2,1,1}; |
brdirais | 0:2cb6ce8e07bd | 61 | |
brdirais | 0:2cb6ce8e07bd | 62 | /* Private constant ---------------------------------------------------------*/ |
brdirais | 0:2cb6ce8e07bd | 63 | |
brdirais | 0:2cb6ce8e07bd | 64 | /** @defgroup L6206_Private_Constants L6206 Private Constants |
brdirais | 0:2cb6ce8e07bd | 65 | * @{ |
brdirais | 0:2cb6ce8e07bd | 66 | */ |
brdirais | 0:2cb6ce8e07bd | 67 | |
brdirais | 0:2cb6ce8e07bd | 68 | /// Max numbers of supported motors depending of the parraleling bridges configuration |
brdirais | 0:2cb6ce8e07bd | 69 | |
brdirais | 0:2cb6ce8e07bd | 70 | |
brdirais | 0:2cb6ce8e07bd | 71 | /* Function prototypes -----------------------------------------------*/ |
brdirais | 0:2cb6ce8e07bd | 72 | |
brdirais | 0:2cb6ce8e07bd | 73 | /** @defgroup L6206_Exported_Functions L6206 Exported Functions |
brdirais | 0:2cb6ce8e07bd | 74 | * @{ |
brdirais | 0:2cb6ce8e07bd | 75 | */ |
brdirais | 0:2cb6ce8e07bd | 76 | |
brdirais | 0:2cb6ce8e07bd | 77 | /* Private function prototypes -----------------------------------------------*/ |
brdirais | 0:2cb6ce8e07bd | 78 | |
brdirais | 0:2cb6ce8e07bd | 79 | /** @defgroup L6206_Private_functions L6206 Private functions |
brdirais | 0:2cb6ce8e07bd | 80 | * @{ |
brdirais | 0:2cb6ce8e07bd | 81 | */ |
brdirais | 0:2cb6ce8e07bd | 82 | /** @defgroup L6206_Exported_Variables L6206 Exported Variables |
brdirais | 0:2cb6ce8e07bd | 83 | * @{ |
brdirais | 0:2cb6ce8e07bd | 84 | */ |
brdirais | 0:2cb6ce8e07bd | 85 | |
brdirais | 0:2cb6ce8e07bd | 86 | /** @defgroup L6206_Exported_Functions L6206 Exported Functions |
brdirais | 0:2cb6ce8e07bd | 87 | * @{ |
brdirais | 0:2cb6ce8e07bd | 88 | */ |
brdirais | 0:2cb6ce8e07bd | 89 | |
brdirais | 0:2cb6ce8e07bd | 90 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 91 | * @brief Attaches a user callback to the error Handler. |
brdirais | 0:2cb6ce8e07bd | 92 | * The call back will be then called each time the library |
brdirais | 0:2cb6ce8e07bd | 93 | * detects an error |
brdirais | 0:2cb6ce8e07bd | 94 | * @param[in] callback Name of the callback to attach |
brdirais | 0:2cb6ce8e07bd | 95 | * to the error Hanlder |
brdirais | 0:2cb6ce8e07bd | 96 | * @retval None |
brdirais | 0:2cb6ce8e07bd | 97 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 98 | void L6206::L6206_AttachErrorHandler(void (*callback)(uint16_t error)) |
brdirais | 0:2cb6ce8e07bd | 99 | { |
brdirais | 0:2cb6ce8e07bd | 100 | errorHandlerCallback = (void (*)(uint16_t error)) callback; |
brdirais | 0:2cb6ce8e07bd | 101 | } |
brdirais | 0:2cb6ce8e07bd | 102 | |
brdirais | 0:2cb6ce8e07bd | 103 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 104 | * @brief Attaches a user callback to the flag Interrupt |
brdirais | 0:2cb6ce8e07bd | 105 | * The call back will be then called each time the status |
brdirais | 0:2cb6ce8e07bd | 106 | * flag pin will be pulled down due to the occurrence of |
brdirais | 0:2cb6ce8e07bd | 107 | * a programmed alarms ( OCD, thermal alert) |
brdirais | 0:2cb6ce8e07bd | 108 | * @param[in] callback Name of the callback to attach |
brdirais | 0:2cb6ce8e07bd | 109 | * to the Flag Interrupt |
brdirais | 0:2cb6ce8e07bd | 110 | * @retval None |
brdirais | 0:2cb6ce8e07bd | 111 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 112 | void L6206::L6206_AttachFlagInterrupt(void (*callback)(void)) |
brdirais | 0:2cb6ce8e07bd | 113 | { |
brdirais | 0:2cb6ce8e07bd | 114 | flagInterruptCallback = (void (*)(void))callback; |
brdirais | 0:2cb6ce8e07bd | 115 | } |
brdirais | 0:2cb6ce8e07bd | 116 | |
brdirais | 0:2cb6ce8e07bd | 117 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 118 | * @brief Disable the specified bridge |
brdirais | 0:2cb6ce8e07bd | 119 | * @param[in] bridgeId (from 0 for bridge A to 1 for bridge B) |
brdirais | 0:2cb6ce8e07bd | 120 | * @retval None |
brdirais | 0:2cb6ce8e07bd | 121 | * @note When input of different brigdes are parallelized |
brdirais | 0:2cb6ce8e07bd | 122 | * together, the disabling of one bridge leads to the disabling |
brdirais | 0:2cb6ce8e07bd | 123 | * of the second one |
brdirais | 0:2cb6ce8e07bd | 124 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 125 | void L6206::L6206_DisableBridge(uint8_t bridgeId) |
brdirais | 0:2cb6ce8e07bd | 126 | { |
brdirais | 0:2cb6ce8e07bd | 127 | L6206_Board_DisableBridge(bridgeId); |
brdirais | 0:2cb6ce8e07bd | 128 | |
brdirais | 0:2cb6ce8e07bd | 129 | devicePrm.bridgeEnabled[bridgeId] = FALSE; |
brdirais | 0:2cb6ce8e07bd | 130 | if (devicePrm.config >= PARALLELING_IN1A_IN2A__IN1B_IN2B__1_BIDIR_MOTOR) |
brdirais | 0:2cb6ce8e07bd | 131 | { |
brdirais | 0:2cb6ce8e07bd | 132 | if (bridgeId == BRIDGE_A) |
brdirais | 0:2cb6ce8e07bd | 133 | { |
brdirais | 0:2cb6ce8e07bd | 134 | L6206_Board_DisableBridge(BRIDGE_B); |
brdirais | 0:2cb6ce8e07bd | 135 | devicePrm.bridgeEnabled[BRIDGE_B] = FALSE; |
brdirais | 0:2cb6ce8e07bd | 136 | } |
brdirais | 0:2cb6ce8e07bd | 137 | else |
brdirais | 0:2cb6ce8e07bd | 138 | { |
brdirais | 0:2cb6ce8e07bd | 139 | L6206_Board_DisableBridge(BRIDGE_A); |
brdirais | 0:2cb6ce8e07bd | 140 | devicePrm.bridgeEnabled[BRIDGE_A] = FALSE; |
brdirais | 0:2cb6ce8e07bd | 141 | } |
brdirais | 0:2cb6ce8e07bd | 142 | } |
brdirais | 0:2cb6ce8e07bd | 143 | } |
brdirais | 0:2cb6ce8e07bd | 144 | |
brdirais | 0:2cb6ce8e07bd | 145 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 146 | * @brief Enable the specified bridge |
brdirais | 0:2cb6ce8e07bd | 147 | * @param[in] bridgeId (from 0 for bridge A to 1 for bridge B) |
brdirais | 0:2cb6ce8e07bd | 148 | * @retval None |
brdirais | 0:2cb6ce8e07bd | 149 | * @note When input of different brigdes are parallelized |
brdirais | 0:2cb6ce8e07bd | 150 | * together, the enabling of one bridge leads to the enabling |
brdirais | 0:2cb6ce8e07bd | 151 | * of the second one |
brdirais | 0:2cb6ce8e07bd | 152 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 153 | void L6206::L6206_EnableBridge(uint8_t bridgeId) |
brdirais | 0:2cb6ce8e07bd | 154 | { |
brdirais | 0:2cb6ce8e07bd | 155 | devicePrm.bridgeEnabled[bridgeId] = TRUE; |
brdirais | 0:2cb6ce8e07bd | 156 | if (devicePrm.config >= PARALLELING_IN1A_IN2A__IN1B_IN2B__1_BIDIR_MOTOR) |
brdirais | 0:2cb6ce8e07bd | 157 | { |
brdirais | 0:2cb6ce8e07bd | 158 | L6206_Board_EnableBridge(bridgeId, 0); |
brdirais | 0:2cb6ce8e07bd | 159 | if (bridgeId == BRIDGE_A) |
brdirais | 0:2cb6ce8e07bd | 160 | { |
brdirais | 0:2cb6ce8e07bd | 161 | L6206_Board_EnableBridge(BRIDGE_B, 1); |
brdirais | 0:2cb6ce8e07bd | 162 | devicePrm.bridgeEnabled[BRIDGE_B] = TRUE; |
brdirais | 0:2cb6ce8e07bd | 163 | } |
brdirais | 0:2cb6ce8e07bd | 164 | else |
brdirais | 0:2cb6ce8e07bd | 165 | { |
brdirais | 0:2cb6ce8e07bd | 166 | L6206_Board_EnableBridge(BRIDGE_A, 1); |
brdirais | 0:2cb6ce8e07bd | 167 | devicePrm.bridgeEnabled[BRIDGE_A] = TRUE; |
brdirais | 0:2cb6ce8e07bd | 168 | } |
brdirais | 0:2cb6ce8e07bd | 169 | } |
brdirais | 0:2cb6ce8e07bd | 170 | else |
brdirais | 0:2cb6ce8e07bd | 171 | { |
brdirais | 0:2cb6ce8e07bd | 172 | L6206_Board_EnableBridge(bridgeId, 1); |
brdirais | 0:2cb6ce8e07bd | 173 | } |
brdirais | 0:2cb6ce8e07bd | 174 | } |
brdirais | 0:2cb6ce8e07bd | 175 | |
brdirais | 0:2cb6ce8e07bd | 176 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 177 | * @brief Start the L6206 library |
brdirais | 0:2cb6ce8e07bd | 178 | * @param[in] init pointer to the initialization data |
brdirais | 0:2cb6ce8e07bd | 179 | * @retval None |
brdirais | 0:2cb6ce8e07bd | 180 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 181 | Status_t L6206::L6206_Init(void *init) |
brdirais | 0:2cb6ce8e07bd | 182 | { |
brdirais | 0:2cb6ce8e07bd | 183 | deviceInstance++; |
brdirais | 0:2cb6ce8e07bd | 184 | |
brdirais | 0:2cb6ce8e07bd | 185 | /* Initialise the GPIOs */ |
brdirais | 0:2cb6ce8e07bd | 186 | L6206_Board_GpioInit(); |
brdirais | 0:2cb6ce8e07bd | 187 | |
brdirais | 0:2cb6ce8e07bd | 188 | if (init == NULL) |
brdirais | 0:2cb6ce8e07bd | 189 | { |
brdirais | 0:2cb6ce8e07bd | 190 | /* Set context variables to the predefined values from l6206_target_config.h */ |
brdirais | 0:2cb6ce8e07bd | 191 | /* Set GPIO according to these values */ |
brdirais | 0:2cb6ce8e07bd | 192 | L6206_SetDeviceParamsToPredefinedValues(); |
brdirais | 0:2cb6ce8e07bd | 193 | } |
brdirais | 0:2cb6ce8e07bd | 194 | else |
brdirais | 0:2cb6ce8e07bd | 195 | { |
brdirais | 0:2cb6ce8e07bd | 196 | L6206_SetDeviceParamsToGivenValues((L6206_Init_t*) init); |
brdirais | 0:2cb6ce8e07bd | 197 | } |
brdirais | 0:2cb6ce8e07bd | 198 | |
brdirais | 0:2cb6ce8e07bd | 199 | /* Initialise input bridges PWMs */ |
brdirais | 0:2cb6ce8e07bd | 200 | L6206_SetDualFullBridgeConfig(devicePrm.config); |
brdirais | 0:2cb6ce8e07bd | 201 | |
brdirais | 0:2cb6ce8e07bd | 202 | return COMPONENT_OK; |
brdirais | 0:2cb6ce8e07bd | 203 | } |
brdirais | 0:2cb6ce8e07bd | 204 | |
brdirais | 0:2cb6ce8e07bd | 205 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 206 | * @brief Get the PWM frequency of the specified bridge |
brdirais | 0:2cb6ce8e07bd | 207 | * @param[in] bridgeId 0 for bridge A, 1 for bridge B |
brdirais | 0:2cb6ce8e07bd | 208 | * @retval Freq in Hz |
brdirais | 0:2cb6ce8e07bd | 209 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 210 | uint32_t L6206::L6206_GetBridgeInputPwmFreq(uint8_t bridgeId) |
brdirais | 0:2cb6ce8e07bd | 211 | { |
brdirais | 0:2cb6ce8e07bd | 212 | return (devicePrm.pwmFreq[(bridgeId << 1)]); |
brdirais | 0:2cb6ce8e07bd | 213 | } |
brdirais | 0:2cb6ce8e07bd | 214 | |
brdirais | 0:2cb6ce8e07bd | 215 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 216 | * @brief Returns the current speed of the specified motor |
brdirais | 0:2cb6ce8e07bd | 217 | * @param[in] motorId from 0 to MAX_NUMBER_OF_BRUSH_DC_MOTORS |
brdirais | 0:2cb6ce8e07bd | 218 | * @retval current speed in % from 0 to 100 |
brdirais | 0:2cb6ce8e07bd | 219 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 220 | uint16_t L6206::L6206_GetCurrentSpeed(uint8_t motorId) |
brdirais | 0:2cb6ce8e07bd | 221 | { |
brdirais | 0:2cb6ce8e07bd | 222 | uint16_t speed = 0; |
brdirais | 0:2cb6ce8e07bd | 223 | |
brdirais | 0:2cb6ce8e07bd | 224 | if (motorId > l6206ArrayNbMaxMotorsByConfig[devicePrm.config]) |
brdirais | 0:2cb6ce8e07bd | 225 | { |
brdirais | 0:2cb6ce8e07bd | 226 | L6206_ErrorHandler(L6206_ERROR_1); |
brdirais | 0:2cb6ce8e07bd | 227 | } |
brdirais | 0:2cb6ce8e07bd | 228 | else if (devicePrm.motionState[motorId] != INACTIVE) |
brdirais | 0:2cb6ce8e07bd | 229 | { |
brdirais | 0:2cb6ce8e07bd | 230 | speed = devicePrm.speed[motorId]; |
brdirais | 0:2cb6ce8e07bd | 231 | } |
brdirais | 0:2cb6ce8e07bd | 232 | |
brdirais | 0:2cb6ce8e07bd | 233 | return (speed); |
brdirais | 0:2cb6ce8e07bd | 234 | } |
brdirais | 0:2cb6ce8e07bd | 235 | |
brdirais | 0:2cb6ce8e07bd | 236 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 237 | * @brief Returns the device state |
brdirais | 0:2cb6ce8e07bd | 238 | * @param[in] motorId from 0 to MAX_NUMBER_OF_BRUSH_DC_MOTORS |
brdirais | 0:2cb6ce8e07bd | 239 | * @retval State (STEADY or INACTIVE) |
brdirais | 0:2cb6ce8e07bd | 240 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 241 | motorState_t L6206::L6206_GetDeviceState(uint8_t motorId) |
brdirais | 0:2cb6ce8e07bd | 242 | { |
brdirais | 0:2cb6ce8e07bd | 243 | motorState_t state = INACTIVE; |
brdirais | 0:2cb6ce8e07bd | 244 | |
brdirais | 0:2cb6ce8e07bd | 245 | if (motorId > l6206ArrayNbMaxMotorsByConfig[devicePrm.config]) |
brdirais | 0:2cb6ce8e07bd | 246 | { |
brdirais | 0:2cb6ce8e07bd | 247 | L6206_ErrorHandler(L6206_ERROR_1); |
brdirais | 0:2cb6ce8e07bd | 248 | } |
brdirais | 0:2cb6ce8e07bd | 249 | else |
brdirais | 0:2cb6ce8e07bd | 250 | { |
brdirais | 0:2cb6ce8e07bd | 251 | state = devicePrm.motionState[motorId]; |
brdirais | 0:2cb6ce8e07bd | 252 | } |
brdirais | 0:2cb6ce8e07bd | 253 | return (state); |
brdirais | 0:2cb6ce8e07bd | 254 | } |
brdirais | 0:2cb6ce8e07bd | 255 | |
brdirais | 0:2cb6ce8e07bd | 256 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 257 | * @brief Returns the FW version of the library |
brdirais | 0:2cb6ce8e07bd | 258 | * @retval L6206_FW_VERSION |
brdirais | 0:2cb6ce8e07bd | 259 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 260 | uint8_t L6206::L6206_GetFwVersion(void) |
brdirais | 0:2cb6ce8e07bd | 261 | { |
brdirais | 0:2cb6ce8e07bd | 262 | return (L6206_FW_VERSION); |
brdirais | 0:2cb6ce8e07bd | 263 | } |
brdirais | 0:2cb6ce8e07bd | 264 | |
brdirais | 0:2cb6ce8e07bd | 265 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 266 | * @brief Returns the max speed of the specified motor |
brdirais | 0:2cb6ce8e07bd | 267 | * @param[in] motorId from 0 to MAX_NUMBER_OF_BRUSH_DC_MOTORS |
brdirais | 0:2cb6ce8e07bd | 268 | * @retval maxSpeed in % from 0 to 100 |
brdirais | 0:2cb6ce8e07bd | 269 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 270 | uint16_t L6206::L6206_GetMaxSpeed(uint8_t motorId) |
brdirais | 0:2cb6ce8e07bd | 271 | { |
brdirais | 0:2cb6ce8e07bd | 272 | uint16_t speed = 0; |
brdirais | 0:2cb6ce8e07bd | 273 | if (motorId > l6206ArrayNbMaxMotorsByConfig[devicePrm.config]) |
brdirais | 0:2cb6ce8e07bd | 274 | { |
brdirais | 0:2cb6ce8e07bd | 275 | L6206_ErrorHandler(L6206_ERROR_1); |
brdirais | 0:2cb6ce8e07bd | 276 | } |
brdirais | 0:2cb6ce8e07bd | 277 | else |
brdirais | 0:2cb6ce8e07bd | 278 | { |
brdirais | 0:2cb6ce8e07bd | 279 | speed = devicePrm.speed[motorId]; |
brdirais | 0:2cb6ce8e07bd | 280 | } |
brdirais | 0:2cb6ce8e07bd | 281 | return (speed); |
brdirais | 0:2cb6ce8e07bd | 282 | } |
brdirais | 0:2cb6ce8e07bd | 283 | |
brdirais | 0:2cb6ce8e07bd | 284 | |
brdirais | 0:2cb6ce8e07bd | 285 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 286 | * @brief Get the status of the bridge enabling of the corresponding bridge |
brdirais | 0:2cb6ce8e07bd | 287 | * @param[in] bridgeId from 0 for bridge A to 1 for bridge B |
brdirais | 0:2cb6ce8e07bd | 288 | * @retval State of the Enable&Flag pin of the corresponding bridge (1 set, 0 for reset) |
brdirais | 0:2cb6ce8e07bd | 289 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 290 | uint16_t L6206::L6206_GetBridgeStatus(uint8_t bridgeId) |
brdirais | 0:2cb6ce8e07bd | 291 | { |
brdirais | 0:2cb6ce8e07bd | 292 | uint16_t status = L6206_Board_GetFlagPinState(bridgeId); |
brdirais | 0:2cb6ce8e07bd | 293 | |
brdirais | 0:2cb6ce8e07bd | 294 | return (status); |
brdirais | 0:2cb6ce8e07bd | 295 | } |
brdirais | 0:2cb6ce8e07bd | 296 | |
brdirais | 0:2cb6ce8e07bd | 297 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 298 | * @brief Immediatly stops the motor and disable the power bridge |
brdirais | 0:2cb6ce8e07bd | 299 | * @param[in] motorId from 0 to MAX_NUMBER_OF_BRUSH_DC_MOTORS |
brdirais | 0:2cb6ce8e07bd | 300 | * @retval None |
brdirais | 0:2cb6ce8e07bd | 301 | * @note if two motors uses the same power bridge, the |
brdirais | 0:2cb6ce8e07bd | 302 | * power bridge will be disable only if the two motors are |
brdirais | 0:2cb6ce8e07bd | 303 | * stopped |
brdirais | 0:2cb6ce8e07bd | 304 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 305 | void L6206::L6206_HardHiz(uint8_t motorId) |
brdirais | 0:2cb6ce8e07bd | 306 | { |
brdirais | 0:2cb6ce8e07bd | 307 | if (motorId > l6206ArrayNbMaxMotorsByConfig[devicePrm.config]) |
brdirais | 0:2cb6ce8e07bd | 308 | { |
brdirais | 0:2cb6ce8e07bd | 309 | L6206_ErrorHandler(L6206_ERROR_1); |
brdirais | 0:2cb6ce8e07bd | 310 | } |
brdirais | 0:2cb6ce8e07bd | 311 | else |
brdirais | 0:2cb6ce8e07bd | 312 | { |
brdirais | 0:2cb6ce8e07bd | 313 | /* Get bridge Id of the corresponding motor */ |
brdirais | 0:2cb6ce8e07bd | 314 | uint8_t bridgeId = L6206_GetBridgeIdUsedByMotorId(motorId); |
brdirais | 0:2cb6ce8e07bd | 315 | |
brdirais | 0:2cb6ce8e07bd | 316 | if (devicePrm.bridgeEnabled[bridgeId] != FALSE) |
brdirais | 0:2cb6ce8e07bd | 317 | { |
brdirais | 0:2cb6ce8e07bd | 318 | bool skip = FALSE; |
brdirais | 0:2cb6ce8e07bd | 319 | |
brdirais | 0:2cb6ce8e07bd | 320 | /* Check if another motor is currently running by using the same bridge */ |
brdirais | 0:2cb6ce8e07bd | 321 | switch (devicePrm.config) |
brdirais | 0:2cb6ce8e07bd | 322 | { |
brdirais | 0:2cb6ce8e07bd | 323 | case PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 324 | if ((motorId > 0) && (devicePrm.motionState[1] == STEADY) && (devicePrm.motionState[2] == STEADY)) |
brdirais | 0:2cb6ce8e07bd | 325 | { |
brdirais | 0:2cb6ce8e07bd | 326 | skip = TRUE; |
brdirais | 0:2cb6ce8e07bd | 327 | } |
brdirais | 0:2cb6ce8e07bd | 328 | break; |
brdirais | 0:2cb6ce8e07bd | 329 | case PARALLELING_NONE___2_UNDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 330 | if ((motorId < 2) && (devicePrm.motionState[0] == STEADY) && (devicePrm.motionState[1] == STEADY)) |
brdirais | 0:2cb6ce8e07bd | 331 | { |
brdirais | 0:2cb6ce8e07bd | 332 | skip = TRUE; |
brdirais | 0:2cb6ce8e07bd | 333 | } |
brdirais | 0:2cb6ce8e07bd | 334 | break; |
brdirais | 0:2cb6ce8e07bd | 335 | case PARALLELING_NONE___2_UNDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 336 | if (((motorId < 2) && (devicePrm.motionState[0] == STEADY) && (devicePrm.motionState[1] == STEADY))|| |
brdirais | 0:2cb6ce8e07bd | 337 | ((motorId > 1) && (devicePrm.motionState[2] == STEADY) && (devicePrm.motionState[3] == STEADY))) |
brdirais | 0:2cb6ce8e07bd | 338 | { |
brdirais | 0:2cb6ce8e07bd | 339 | skip = TRUE; |
brdirais | 0:2cb6ce8e07bd | 340 | } |
brdirais | 0:2cb6ce8e07bd | 341 | break; |
brdirais | 0:2cb6ce8e07bd | 342 | case PARALLELING_IN1A_IN2A__1_UNDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 343 | if ((motorId > 0) && (devicePrm.motionState[1] == STEADY) && (devicePrm.motionState[2] == STEADY)) |
brdirais | 0:2cb6ce8e07bd | 344 | { |
brdirais | 0:2cb6ce8e07bd | 345 | skip = TRUE; |
brdirais | 0:2cb6ce8e07bd | 346 | } |
brdirais | 0:2cb6ce8e07bd | 347 | break; |
brdirais | 0:2cb6ce8e07bd | 348 | case PARALLELING_IN1B_IN2B__2_UNDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 349 | if ((motorId < 2) && (devicePrm.motionState[0] == STEADY) && (devicePrm.motionState[1] == STEADY)) |
brdirais | 0:2cb6ce8e07bd | 350 | { |
brdirais | 0:2cb6ce8e07bd | 351 | skip = TRUE; |
brdirais | 0:2cb6ce8e07bd | 352 | } |
brdirais | 0:2cb6ce8e07bd | 353 | break; |
brdirais | 0:2cb6ce8e07bd | 354 | case PARALLELING_IN1A_IN1B__IN2A_IN2B__1_UNDIR_MOTOR_BRIDGE_1A__1_UNDIR_MOTOR_BRIDGE_2A: |
brdirais | 0:2cb6ce8e07bd | 355 | if ((devicePrm.motionState[0] == STEADY) && (devicePrm.motionState[1] == STEADY)) |
brdirais | 0:2cb6ce8e07bd | 356 | { |
brdirais | 0:2cb6ce8e07bd | 357 | skip = TRUE; |
brdirais | 0:2cb6ce8e07bd | 358 | } |
brdirais | 0:2cb6ce8e07bd | 359 | break; |
brdirais | 0:2cb6ce8e07bd | 360 | default: |
brdirais | 0:2cb6ce8e07bd | 361 | break; |
brdirais | 0:2cb6ce8e07bd | 362 | } |
brdirais | 0:2cb6ce8e07bd | 363 | |
brdirais | 0:2cb6ce8e07bd | 364 | if (skip == FALSE) |
brdirais | 0:2cb6ce8e07bd | 365 | { |
brdirais | 0:2cb6ce8e07bd | 366 | /* Disable the bridge */ |
brdirais | 0:2cb6ce8e07bd | 367 | L6206_DisableBridge(bridgeId); |
brdirais | 0:2cb6ce8e07bd | 368 | } |
brdirais | 0:2cb6ce8e07bd | 369 | } |
brdirais | 0:2cb6ce8e07bd | 370 | /* Disable the PWM */ |
brdirais | 0:2cb6ce8e07bd | 371 | L6206_HardStop(motorId); |
brdirais | 0:2cb6ce8e07bd | 372 | } |
brdirais | 0:2cb6ce8e07bd | 373 | } |
brdirais | 0:2cb6ce8e07bd | 374 | |
brdirais | 0:2cb6ce8e07bd | 375 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 376 | * @brief Stops the motor without disabling the bridge |
brdirais | 0:2cb6ce8e07bd | 377 | * @param[in] motorId from 0 to MAX_NUMBER_OF_BRUSH_DC_MOTORS |
brdirais | 0:2cb6ce8e07bd | 378 | * @retval none |
brdirais | 0:2cb6ce8e07bd | 379 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 380 | void L6206::L6206_HardStop(uint8_t motorId) |
brdirais | 0:2cb6ce8e07bd | 381 | { |
brdirais | 0:2cb6ce8e07bd | 382 | if (motorId > l6206ArrayNbMaxMotorsByConfig[devicePrm.config]) |
brdirais | 0:2cb6ce8e07bd | 383 | { |
brdirais | 0:2cb6ce8e07bd | 384 | L6206_ErrorHandler(L6206_ERROR_1); |
brdirais | 0:2cb6ce8e07bd | 385 | } |
brdirais | 0:2cb6ce8e07bd | 386 | else if (devicePrm.motionState[motorId] != INACTIVE) |
brdirais | 0:2cb6ce8e07bd | 387 | { |
brdirais | 0:2cb6ce8e07bd | 388 | uint8_t bridgeInput; |
brdirais | 0:2cb6ce8e07bd | 389 | |
brdirais | 0:2cb6ce8e07bd | 390 | /* Get bridge input of the corresponding motor */ |
brdirais | 0:2cb6ce8e07bd | 391 | bridgeInput = L6206_GetBridgeInputUsedByMotorId(motorId); |
brdirais | 0:2cb6ce8e07bd | 392 | |
brdirais | 0:2cb6ce8e07bd | 393 | /* Disable corresponding PWM */ |
brdirais | 0:2cb6ce8e07bd | 394 | L6206_Board_PwmStop(bridgeInput); |
brdirais | 0:2cb6ce8e07bd | 395 | |
brdirais | 0:2cb6ce8e07bd | 396 | /* for bidirectionnal motor, disable second PWM*/ |
brdirais | 0:2cb6ce8e07bd | 397 | if (L6206_IsBidirectionnalMotor(motorId)) |
brdirais | 0:2cb6ce8e07bd | 398 | { |
brdirais | 0:2cb6ce8e07bd | 399 | bridgeInput = L6206_GetSecondBridgeInputUsedByMotorId(motorId); |
brdirais | 0:2cb6ce8e07bd | 400 | L6206_Board_PwmStop(bridgeInput); |
brdirais | 0:2cb6ce8e07bd | 401 | } |
brdirais | 0:2cb6ce8e07bd | 402 | /* Set inactive state */ |
brdirais | 0:2cb6ce8e07bd | 403 | devicePrm.motionState[motorId] = INACTIVE; |
brdirais | 0:2cb6ce8e07bd | 404 | } |
brdirais | 0:2cb6ce8e07bd | 405 | } |
brdirais | 0:2cb6ce8e07bd | 406 | |
brdirais | 0:2cb6ce8e07bd | 407 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 408 | * @brief Read id |
brdirais | 0:2cb6ce8e07bd | 409 | * @retval Id of the l6206 Driver Instance |
brdirais | 0:2cb6ce8e07bd | 410 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 411 | Status_t L6206::L6206_ReadId(uint8_t *id) |
brdirais | 0:2cb6ce8e07bd | 412 | { |
brdirais | 0:2cb6ce8e07bd | 413 | *id = deviceInstance; |
brdirais | 0:2cb6ce8e07bd | 414 | |
brdirais | 0:2cb6ce8e07bd | 415 | return COMPONENT_OK; |
brdirais | 0:2cb6ce8e07bd | 416 | } |
brdirais | 0:2cb6ce8e07bd | 417 | |
brdirais | 0:2cb6ce8e07bd | 418 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 419 | * @brief Runs the motor |
brdirais | 0:2cb6ce8e07bd | 420 | * @param[in] motorId from 0 to MAX_NUMBER_OF_BRUSH_DC_MOTORS |
brdirais | 0:2cb6ce8e07bd | 421 | * @param[in] direction FORWARD or BACKWARD |
brdirais | 0:2cb6ce8e07bd | 422 | * @retval None |
brdirais | 0:2cb6ce8e07bd | 423 | * @note For unidirectionnal motor, direction parameter has |
brdirais | 0:2cb6ce8e07bd | 424 | * no effect |
brdirais | 0:2cb6ce8e07bd | 425 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 426 | void L6206::L6206_Run(uint8_t motorId, motorDir_t direction) |
brdirais | 0:2cb6ce8e07bd | 427 | { |
brdirais | 0:2cb6ce8e07bd | 428 | if (motorId > l6206ArrayNbMaxMotorsByConfig[devicePrm.config]) |
brdirais | 0:2cb6ce8e07bd | 429 | { |
brdirais | 0:2cb6ce8e07bd | 430 | L6206_ErrorHandler(L6206_ERROR_1); |
brdirais | 0:2cb6ce8e07bd | 431 | } |
brdirais | 0:2cb6ce8e07bd | 432 | else if ((devicePrm.motionState[motorId] == INACTIVE) || |
brdirais | 0:2cb6ce8e07bd | 433 | (devicePrm.direction[motorId] != direction)) |
brdirais | 0:2cb6ce8e07bd | 434 | { |
brdirais | 0:2cb6ce8e07bd | 435 | uint8_t bridgeId; |
brdirais | 0:2cb6ce8e07bd | 436 | uint8_t bridgeInput; |
brdirais | 0:2cb6ce8e07bd | 437 | |
brdirais | 0:2cb6ce8e07bd | 438 | /* Eventually deactivate motor */ |
brdirais | 0:2cb6ce8e07bd | 439 | if (devicePrm.motionState[motorId] != INACTIVE) |
brdirais | 0:2cb6ce8e07bd | 440 | { |
brdirais | 0:2cb6ce8e07bd | 441 | L6206_HardStop(motorId); |
brdirais | 0:2cb6ce8e07bd | 442 | } |
brdirais | 0:2cb6ce8e07bd | 443 | |
brdirais | 0:2cb6ce8e07bd | 444 | /* Store new direction */ |
brdirais | 0:2cb6ce8e07bd | 445 | devicePrm.direction[motorId] = direction; |
brdirais | 0:2cb6ce8e07bd | 446 | |
brdirais | 0:2cb6ce8e07bd | 447 | /* Switch to steady state */ |
brdirais | 0:2cb6ce8e07bd | 448 | devicePrm.motionState[motorId] = STEADY; |
brdirais | 0:2cb6ce8e07bd | 449 | |
brdirais | 0:2cb6ce8e07bd | 450 | /* Get bridge Id of the corresponding motor */ |
brdirais | 0:2cb6ce8e07bd | 451 | bridgeId = L6206_GetBridgeIdUsedByMotorId(motorId); |
brdirais | 0:2cb6ce8e07bd | 452 | |
brdirais | 0:2cb6ce8e07bd | 453 | /* Get bridge input of the corresponding motor */ |
brdirais | 0:2cb6ce8e07bd | 454 | bridgeInput = L6206_GetBridgeInputUsedByMotorId(motorId); |
brdirais | 0:2cb6ce8e07bd | 455 | |
brdirais | 0:2cb6ce8e07bd | 456 | /* Enable bridge */ |
brdirais | 0:2cb6ce8e07bd | 457 | L6206_EnableBridge(bridgeId); |
brdirais | 0:2cb6ce8e07bd | 458 | |
brdirais | 0:2cb6ce8e07bd | 459 | /* Set PWM */ |
brdirais | 0:2cb6ce8e07bd | 460 | if (L6206_IsBidirectionnalMotor(motorId)) |
brdirais | 0:2cb6ce8e07bd | 461 | { |
brdirais | 0:2cb6ce8e07bd | 462 | /* for bidirectionnal motor */ |
brdirais | 0:2cb6ce8e07bd | 463 | L6206_Board_PwmSetFreq(bridgeInput, devicePrm.pwmFreq[bridgeInput],(100 - devicePrm.speed[motorId])); |
brdirais | 0:2cb6ce8e07bd | 464 | bridgeInput = L6206_GetSecondBridgeInputUsedByMotorId(motorId); |
brdirais | 0:2cb6ce8e07bd | 465 | L6206_Board_PwmSetFreq(bridgeInput, devicePrm.pwmFreq[bridgeInput],100); |
brdirais | 0:2cb6ce8e07bd | 466 | } |
brdirais | 0:2cb6ce8e07bd | 467 | else |
brdirais | 0:2cb6ce8e07bd | 468 | { |
brdirais | 0:2cb6ce8e07bd | 469 | /* for unidirectionnal motor */ |
brdirais | 0:2cb6ce8e07bd | 470 | L6206_Board_PwmSetFreq(bridgeInput, devicePrm.pwmFreq[bridgeInput],devicePrm.speed[motorId]); |
brdirais | 0:2cb6ce8e07bd | 471 | } |
brdirais | 0:2cb6ce8e07bd | 472 | } |
brdirais | 0:2cb6ce8e07bd | 473 | } |
brdirais | 0:2cb6ce8e07bd | 474 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 475 | * @brief Set dual full bridge parallelling configuration |
brdirais | 0:2cb6ce8e07bd | 476 | * @param[in] newConfig bridge configuration to apply from |
brdirais | 0:2cb6ce8e07bd | 477 | * dualFullBridgeConfig_t enum |
brdirais | 0:2cb6ce8e07bd | 478 | * @retval None |
brdirais | 0:2cb6ce8e07bd | 479 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 480 | void L6206::L6206_SetDualFullBridgeConfig(uint8_t newConfig) |
brdirais | 0:2cb6ce8e07bd | 481 | { |
brdirais | 0:2cb6ce8e07bd | 482 | devicePrm.config = (dualFullBridgeConfig_t)newConfig; |
brdirais | 0:2cb6ce8e07bd | 483 | |
brdirais | 0:2cb6ce8e07bd | 484 | /* Start to reset all else if several inits are used successively */ |
brdirais | 0:2cb6ce8e07bd | 485 | /* they will fail */ |
brdirais | 0:2cb6ce8e07bd | 486 | L6206_Board_PwmDeInit(INPUT_1A); |
brdirais | 0:2cb6ce8e07bd | 487 | L6206_Board_PwmDeInit(INPUT_2A); |
brdirais | 0:2cb6ce8e07bd | 488 | L6206_Board_PwmDeInit(INPUT_1B); |
brdirais | 0:2cb6ce8e07bd | 489 | L6206_Board_PwmDeInit(INPUT_2B); |
brdirais | 0:2cb6ce8e07bd | 490 | |
brdirais | 0:2cb6ce8e07bd | 491 | |
brdirais | 0:2cb6ce8e07bd | 492 | /* Initialise the bridges inputs PWMs --------------------------------------*/ |
brdirais | 0:2cb6ce8e07bd | 493 | switch (devicePrm.config) |
brdirais | 0:2cb6ce8e07bd | 494 | { |
brdirais | 0:2cb6ce8e07bd | 495 | case PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 496 | case PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 497 | case PARALLELING_NONE___2_UNDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 498 | case PARALLELING_NONE___2_UNDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 499 | L6206_Board_PwmInit(INPUT_1A); |
brdirais | 0:2cb6ce8e07bd | 500 | L6206_Board_PwmInit(INPUT_2A); |
brdirais | 0:2cb6ce8e07bd | 501 | L6206_Board_PwmInit(INPUT_1B); |
brdirais | 0:2cb6ce8e07bd | 502 | L6206_Board_PwmInit(INPUT_2B); |
brdirais | 0:2cb6ce8e07bd | 503 | break; |
brdirais | 0:2cb6ce8e07bd | 504 | case PARALLELING_IN1A_IN2A__1_UNDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 505 | case PARALLELING_IN1A_IN2A__1_UNDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 506 | L6206_Board_PwmDeInit(INPUT_2A); |
brdirais | 0:2cb6ce8e07bd | 507 | L6206_Board_PwmInit(INPUT_1A); |
brdirais | 0:2cb6ce8e07bd | 508 | L6206_Board_PwmInit(INPUT_1B); |
brdirais | 0:2cb6ce8e07bd | 509 | L6206_Board_PwmInit(INPUT_2B); |
brdirais | 0:2cb6ce8e07bd | 510 | break; |
brdirais | 0:2cb6ce8e07bd | 511 | case PARALLELING_IN1B_IN2B__1_BIDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 512 | case PARALLELING_IN1B_IN2B__2_UNDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 513 | L6206_Board_PwmDeInit(INPUT_2B); |
brdirais | 0:2cb6ce8e07bd | 514 | L6206_Board_PwmInit(INPUT_1A); |
brdirais | 0:2cb6ce8e07bd | 515 | L6206_Board_PwmInit(INPUT_2A); |
brdirais | 0:2cb6ce8e07bd | 516 | L6206_Board_PwmInit(INPUT_1B); |
brdirais | 0:2cb6ce8e07bd | 517 | break; |
brdirais | 0:2cb6ce8e07bd | 518 | case PARALLELING_IN1A_IN2A__IN1B_IN2B__1_UNDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 519 | case PARALLELING_IN1A_IN2A__IN1B_IN2B__1_BIDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 520 | L6206_Board_PwmDeInit(INPUT_2A); |
brdirais | 0:2cb6ce8e07bd | 521 | L6206_Board_PwmDeInit(INPUT_2B); |
brdirais | 0:2cb6ce8e07bd | 522 | L6206_Board_PwmInit(INPUT_1A); |
brdirais | 0:2cb6ce8e07bd | 523 | L6206_Board_PwmInit(INPUT_1B); |
brdirais | 0:2cb6ce8e07bd | 524 | break; |
brdirais | 0:2cb6ce8e07bd | 525 | case PARALLELING_IN1A_IN1B__IN2A_IN2B__1_UNDIR_MOTOR_BRIDGE_1A__1_UNDIR_MOTOR_BRIDGE_2A: |
brdirais | 0:2cb6ce8e07bd | 526 | case PARALLELING_IN1A_IN1B__IN2A_IN2B__1_BIDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 527 | L6206_Board_PwmDeInit(INPUT_1B); |
brdirais | 0:2cb6ce8e07bd | 528 | L6206_Board_PwmDeInit(INPUT_2B); |
brdirais | 0:2cb6ce8e07bd | 529 | L6206_Board_PwmInit(INPUT_1A); |
brdirais | 0:2cb6ce8e07bd | 530 | L6206_Board_PwmInit(INPUT_2A); |
brdirais | 0:2cb6ce8e07bd | 531 | break; |
brdirais | 0:2cb6ce8e07bd | 532 | case PARALLELING_ALL_WITH_IN1A___1_UNDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 533 | L6206_Board_PwmDeInit(INPUT_2A); |
brdirais | 0:2cb6ce8e07bd | 534 | L6206_Board_PwmDeInit(INPUT_1B); |
brdirais | 0:2cb6ce8e07bd | 535 | L6206_Board_PwmDeInit(INPUT_2B); |
brdirais | 0:2cb6ce8e07bd | 536 | L6206_Board_PwmInit(INPUT_1A); |
brdirais | 0:2cb6ce8e07bd | 537 | break; |
brdirais | 0:2cb6ce8e07bd | 538 | default: |
brdirais | 0:2cb6ce8e07bd | 539 | break; |
brdirais | 0:2cb6ce8e07bd | 540 | } |
brdirais | 0:2cb6ce8e07bd | 541 | } |
brdirais | 0:2cb6ce8e07bd | 542 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 543 | * @brief Changes the max speed of the specified device |
brdirais | 0:2cb6ce8e07bd | 544 | * @param[in] motorId from 0 to MAX_NUMBER_OF_BRUSH_DC_MOTORS |
brdirais | 0:2cb6ce8e07bd | 545 | * @param[in] newMaxSpeed in % from 0 to 100 |
brdirais | 0:2cb6ce8e07bd | 546 | * @retval true if the command is successfully executed, else false |
brdirais | 0:2cb6ce8e07bd | 547 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 548 | bool L6206::L6206_SetMaxSpeed(uint8_t motorId, uint16_t newMaxSpeed) |
brdirais | 0:2cb6ce8e07bd | 549 | { |
brdirais | 0:2cb6ce8e07bd | 550 | bool cmdExecuted = FALSE; |
brdirais | 0:2cb6ce8e07bd | 551 | |
brdirais | 0:2cb6ce8e07bd | 552 | if (motorId > l6206ArrayNbMaxMotorsByConfig[devicePrm.config]) |
brdirais | 0:2cb6ce8e07bd | 553 | { |
brdirais | 0:2cb6ce8e07bd | 554 | L6206_ErrorHandler(L6206_ERROR_1); |
brdirais | 0:2cb6ce8e07bd | 555 | } |
brdirais | 0:2cb6ce8e07bd | 556 | else |
brdirais | 0:2cb6ce8e07bd | 557 | { |
brdirais | 0:2cb6ce8e07bd | 558 | devicePrm.speed[motorId] = newMaxSpeed; |
brdirais | 0:2cb6ce8e07bd | 559 | if (devicePrm.motionState[motorId] != INACTIVE) |
brdirais | 0:2cb6ce8e07bd | 560 | { |
brdirais | 0:2cb6ce8e07bd | 561 | uint8_t bridgeInput; |
brdirais | 0:2cb6ce8e07bd | 562 | |
brdirais | 0:2cb6ce8e07bd | 563 | /* Get Bridge input of the corresponding motor */ |
brdirais | 0:2cb6ce8e07bd | 564 | bridgeInput = L6206_GetBridgeInputUsedByMotorId(motorId); |
brdirais | 0:2cb6ce8e07bd | 565 | |
brdirais | 0:2cb6ce8e07bd | 566 | /* Set PWM frequency*/ |
brdirais | 0:2cb6ce8e07bd | 567 | if (L6206_IsBidirectionnalMotor(motorId)) |
brdirais | 0:2cb6ce8e07bd | 568 | { |
brdirais | 0:2cb6ce8e07bd | 569 | /* for bidirectionnal motor */ |
brdirais | 0:2cb6ce8e07bd | 570 | L6206_Board_PwmSetFreq(bridgeInput, devicePrm.pwmFreq[bridgeInput],(100 - devicePrm.speed[motorId])); |
brdirais | 0:2cb6ce8e07bd | 571 | } |
brdirais | 0:2cb6ce8e07bd | 572 | else |
brdirais | 0:2cb6ce8e07bd | 573 | { |
brdirais | 0:2cb6ce8e07bd | 574 | /* for unidirectionnal motor */ |
brdirais | 0:2cb6ce8e07bd | 575 | L6206_Board_PwmSetFreq(bridgeInput, devicePrm.pwmFreq[bridgeInput],devicePrm.speed[motorId]); |
brdirais | 0:2cb6ce8e07bd | 576 | } |
brdirais | 0:2cb6ce8e07bd | 577 | } |
brdirais | 0:2cb6ce8e07bd | 578 | cmdExecuted = TRUE; |
brdirais | 0:2cb6ce8e07bd | 579 | } |
brdirais | 0:2cb6ce8e07bd | 580 | return cmdExecuted; |
brdirais | 0:2cb6ce8e07bd | 581 | } |
brdirais | 0:2cb6ce8e07bd | 582 | |
brdirais | 0:2cb6ce8e07bd | 583 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 584 | * @brief Changes the PWM frequency of the bridge input |
brdirais | 0:2cb6ce8e07bd | 585 | * @param[in] bridgeId 0 for bridge A, 1 for bridge B |
brdirais | 0:2cb6ce8e07bd | 586 | * @param[in] newFreq in Hz |
brdirais | 0:2cb6ce8e07bd | 587 | * @retval None |
brdirais | 0:2cb6ce8e07bd | 588 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 589 | void L6206::L6206_SetBridgeInputPwmFreq(uint8_t bridgeId, uint32_t newFreq) |
brdirais | 0:2cb6ce8e07bd | 590 | { |
brdirais | 0:2cb6ce8e07bd | 591 | uint8_t loop; |
brdirais | 0:2cb6ce8e07bd | 592 | |
brdirais | 0:2cb6ce8e07bd | 593 | if (newFreq > L6206_MAX_PWM_FREQ) |
brdirais | 0:2cb6ce8e07bd | 594 | { |
brdirais | 0:2cb6ce8e07bd | 595 | newFreq = L6206_MAX_PWM_FREQ; |
brdirais | 0:2cb6ce8e07bd | 596 | } |
brdirais | 0:2cb6ce8e07bd | 597 | for (loop = 0; loop < 2;loop ++) |
brdirais | 0:2cb6ce8e07bd | 598 | { |
brdirais | 0:2cb6ce8e07bd | 599 | uint8_t motorId; |
brdirais | 0:2cb6ce8e07bd | 600 | uint8_t bridgeInput = (bridgeId << 1) + loop; |
brdirais | 0:2cb6ce8e07bd | 601 | devicePrm.pwmFreq[bridgeInput] = newFreq; |
brdirais | 0:2cb6ce8e07bd | 602 | |
brdirais | 0:2cb6ce8e07bd | 603 | /* Get motor Id using this bridge */ |
brdirais | 0:2cb6ce8e07bd | 604 | motorId = L6206_GetMotorIdUsingbridgeInput(bridgeInput); |
brdirais | 0:2cb6ce8e07bd | 605 | |
brdirais | 0:2cb6ce8e07bd | 606 | /* Immediatly update frequency if motor is running */ |
brdirais | 0:2cb6ce8e07bd | 607 | if (devicePrm.motionState[motorId] != INACTIVE) |
brdirais | 0:2cb6ce8e07bd | 608 | { |
brdirais | 0:2cb6ce8e07bd | 609 | /* Test if motor is bidir */ |
brdirais | 0:2cb6ce8e07bd | 610 | if (L6206_IsBidirectionnalMotor(motorId)) |
brdirais | 0:2cb6ce8e07bd | 611 | { |
brdirais | 0:2cb6ce8e07bd | 612 | if (bridgeInput != L6206_GetSecondBridgeInputUsedByMotorId(motorId)) |
brdirais | 0:2cb6ce8e07bd | 613 | { |
brdirais | 0:2cb6ce8e07bd | 614 | /* Set PWM frequency for bidirectionnal motor of the first bridge*/ |
brdirais | 0:2cb6ce8e07bd | 615 | L6206_Board_PwmSetFreq(bridgeInput, devicePrm.pwmFreq[bridgeInput],(100 - devicePrm.speed[motorId])); |
brdirais | 0:2cb6ce8e07bd | 616 | } |
brdirais | 0:2cb6ce8e07bd | 617 | else |
brdirais | 0:2cb6ce8e07bd | 618 | { |
brdirais | 0:2cb6ce8e07bd | 619 | /* Set PWM frequency for bidirectionnal motor of the second bridge */ |
brdirais | 0:2cb6ce8e07bd | 620 | L6206_Board_PwmSetFreq(bridgeInput, devicePrm.pwmFreq[bridgeInput],100); |
brdirais | 0:2cb6ce8e07bd | 621 | } |
brdirais | 0:2cb6ce8e07bd | 622 | } |
brdirais | 0:2cb6ce8e07bd | 623 | else |
brdirais | 0:2cb6ce8e07bd | 624 | { |
brdirais | 0:2cb6ce8e07bd | 625 | /* Set PWM frequency for unidirectionnal motor */ |
brdirais | 0:2cb6ce8e07bd | 626 | L6206_Board_PwmSetFreq(bridgeInput, devicePrm.pwmFreq[bridgeInput],devicePrm.speed[motorId]); |
brdirais | 0:2cb6ce8e07bd | 627 | } |
brdirais | 0:2cb6ce8e07bd | 628 | } |
brdirais | 0:2cb6ce8e07bd | 629 | } |
brdirais | 0:2cb6ce8e07bd | 630 | } |
brdirais | 0:2cb6ce8e07bd | 631 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 632 | * @brief Sets the number of devices to be used |
brdirais | 0:2cb6ce8e07bd | 633 | * @param[in] nbDevices (from 1 to MAX_NUMBER_OF_DEVICES) |
brdirais | 0:2cb6ce8e07bd | 634 | * @retval TRUE if successfull, FALSE if failure, attempt to set a number of |
brdirais | 0:2cb6ce8e07bd | 635 | * devices greater than MAX_NUMBER_OF_DEVICES |
brdirais | 0:2cb6ce8e07bd | 636 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 637 | bool L6206::L6206_SetNbDevices(uint8_t nbDevices) |
brdirais | 0:2cb6ce8e07bd | 638 | { |
brdirais | 0:2cb6ce8e07bd | 639 | if (nbDevices <= MAX_NUMBER_OF_DEVICES) |
brdirais | 0:2cb6ce8e07bd | 640 | { |
brdirais | 0:2cb6ce8e07bd | 641 | return TRUE; |
brdirais | 0:2cb6ce8e07bd | 642 | } |
brdirais | 0:2cb6ce8e07bd | 643 | else |
brdirais | 0:2cb6ce8e07bd | 644 | { |
brdirais | 0:2cb6ce8e07bd | 645 | return FALSE; |
brdirais | 0:2cb6ce8e07bd | 646 | } |
brdirais | 0:2cb6ce8e07bd | 647 | } |
brdirais | 0:2cb6ce8e07bd | 648 | |
brdirais | 0:2cb6ce8e07bd | 649 | |
brdirais | 0:2cb6ce8e07bd | 650 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 651 | * @brief Error handler which calls the user callback (if defined) |
brdirais | 0:2cb6ce8e07bd | 652 | * @param[in] error Number of the error |
brdirais | 0:2cb6ce8e07bd | 653 | * @retval None |
brdirais | 0:2cb6ce8e07bd | 654 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 655 | void L6206::L6206_ErrorHandler(uint16_t error) |
brdirais | 0:2cb6ce8e07bd | 656 | { |
brdirais | 0:2cb6ce8e07bd | 657 | if (errorHandlerCallback != 0) |
brdirais | 0:2cb6ce8e07bd | 658 | { |
brdirais | 0:2cb6ce8e07bd | 659 | (void) errorHandlerCallback(error); |
brdirais | 0:2cb6ce8e07bd | 660 | } |
brdirais | 0:2cb6ce8e07bd | 661 | else |
brdirais | 0:2cb6ce8e07bd | 662 | { |
brdirais | 0:2cb6ce8e07bd | 663 | while(1) |
brdirais | 0:2cb6ce8e07bd | 664 | { |
brdirais | 0:2cb6ce8e07bd | 665 | /* Infinite loop */ |
brdirais | 0:2cb6ce8e07bd | 666 | } |
brdirais | 0:2cb6ce8e07bd | 667 | } |
brdirais | 0:2cb6ce8e07bd | 668 | } |
brdirais | 0:2cb6ce8e07bd | 669 | |
brdirais | 0:2cb6ce8e07bd | 670 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 671 | * @brief Handlers of the flag interrupt which calls the user callback (if defined) |
brdirais | 0:2cb6ce8e07bd | 672 | * @retval None |
brdirais | 0:2cb6ce8e07bd | 673 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 674 | void L6206::L6206_FlagInterruptHandler(void) |
brdirais | 0:2cb6ce8e07bd | 675 | { |
brdirais | 0:2cb6ce8e07bd | 676 | bool status; |
brdirais | 0:2cb6ce8e07bd | 677 | |
brdirais | 0:2cb6ce8e07bd | 678 | status = L6206_GetBridgeStatus(BRIDGE_A); |
brdirais | 0:2cb6ce8e07bd | 679 | if (status != devicePrm.bridgeEnabled[BRIDGE_A]) |
brdirais | 0:2cb6ce8e07bd | 680 | { |
brdirais | 0:2cb6ce8e07bd | 681 | devicePrm.bridgeEnabled[BRIDGE_A] = status; |
brdirais | 0:2cb6ce8e07bd | 682 | } |
brdirais | 0:2cb6ce8e07bd | 683 | |
brdirais | 0:2cb6ce8e07bd | 684 | status = L6206_GetBridgeStatus(BRIDGE_B); |
brdirais | 0:2cb6ce8e07bd | 685 | if (status != devicePrm.bridgeEnabled[BRIDGE_B]) |
brdirais | 0:2cb6ce8e07bd | 686 | { |
brdirais | 0:2cb6ce8e07bd | 687 | devicePrm.bridgeEnabled[BRIDGE_B] = status; |
brdirais | 0:2cb6ce8e07bd | 688 | } |
brdirais | 0:2cb6ce8e07bd | 689 | |
brdirais | 0:2cb6ce8e07bd | 690 | if (flagInterruptCallback != 0) |
brdirais | 0:2cb6ce8e07bd | 691 | { |
brdirais | 0:2cb6ce8e07bd | 692 | flagInterruptCallback(); |
brdirais | 0:2cb6ce8e07bd | 693 | } |
brdirais | 0:2cb6ce8e07bd | 694 | } |
brdirais | 0:2cb6ce8e07bd | 695 | |
brdirais | 0:2cb6ce8e07bd | 696 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 697 | * @brief Get the bridges Id used by a given motor |
brdirais | 0:2cb6ce8e07bd | 698 | * @param motorId from 0 to MAX_NUMBER_OF_BRUSH_DC_MOTORS |
brdirais | 0:2cb6ce8e07bd | 699 | * @retval bridgeId 0 for bridge A , 1 for bridge B |
brdirais | 0:2cb6ce8e07bd | 700 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 701 | uint8_t L6206::L6206_GetBridgeIdUsedByMotorId(uint8_t motorId) |
brdirais | 0:2cb6ce8e07bd | 702 | { |
brdirais | 0:2cb6ce8e07bd | 703 | uint8_t bridgeId; |
brdirais | 0:2cb6ce8e07bd | 704 | |
brdirais | 0:2cb6ce8e07bd | 705 | switch (devicePrm.config) |
brdirais | 0:2cb6ce8e07bd | 706 | { |
brdirais | 0:2cb6ce8e07bd | 707 | case PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 708 | case PARALLELING_IN1A_IN2A__1_UNDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 709 | case PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 710 | case PARALLELING_IN1A_IN2A__1_UNDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 711 | case PARALLELING_IN1B_IN2B__1_BIDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 712 | case PARALLELING_IN1A_IN2A__IN1B_IN2B__1_UNDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 713 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 714 | { |
brdirais | 0:2cb6ce8e07bd | 715 | bridgeId = 0; |
brdirais | 0:2cb6ce8e07bd | 716 | } |
brdirais | 0:2cb6ce8e07bd | 717 | else |
brdirais | 0:2cb6ce8e07bd | 718 | { |
brdirais | 0:2cb6ce8e07bd | 719 | bridgeId = 1; |
brdirais | 0:2cb6ce8e07bd | 720 | } |
brdirais | 0:2cb6ce8e07bd | 721 | break; |
brdirais | 0:2cb6ce8e07bd | 722 | case PARALLELING_NONE___2_UNDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 723 | case PARALLELING_NONE___2_UNDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 724 | case PARALLELING_IN1B_IN2B__2_UNDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 725 | if (motorId < 2) |
brdirais | 0:2cb6ce8e07bd | 726 | { |
brdirais | 0:2cb6ce8e07bd | 727 | bridgeId = 0; |
brdirais | 0:2cb6ce8e07bd | 728 | } |
brdirais | 0:2cb6ce8e07bd | 729 | else |
brdirais | 0:2cb6ce8e07bd | 730 | { |
brdirais | 0:2cb6ce8e07bd | 731 | bridgeId = 1; |
brdirais | 0:2cb6ce8e07bd | 732 | } |
brdirais | 0:2cb6ce8e07bd | 733 | break; |
brdirais | 0:2cb6ce8e07bd | 734 | case PARALLELING_IN1A_IN2A__IN1B_IN2B__1_BIDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 735 | case PARALLELING_IN1A_IN1B__IN2A_IN2B__1_UNDIR_MOTOR_BRIDGE_1A__1_UNDIR_MOTOR_BRIDGE_2A: |
brdirais | 0:2cb6ce8e07bd | 736 | case PARALLELING_IN1A_IN1B__IN2A_IN2B__1_BIDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 737 | case PARALLELING_ALL_WITH_IN1A___1_UNDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 738 | default: |
brdirais | 0:2cb6ce8e07bd | 739 | bridgeId = 0; |
brdirais | 0:2cb6ce8e07bd | 740 | break; |
brdirais | 0:2cb6ce8e07bd | 741 | } |
brdirais | 0:2cb6ce8e07bd | 742 | return (bridgeId); |
brdirais | 0:2cb6ce8e07bd | 743 | } |
brdirais | 0:2cb6ce8e07bd | 744 | |
brdirais | 0:2cb6ce8e07bd | 745 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 746 | * @brief Get the motor Id which is using the specified bridge input |
brdirais | 0:2cb6ce8e07bd | 747 | * @param bridgeInput 0 for bridgeInput 1A, 1 for 2A, 2 for 1B, 3 for 3B |
brdirais | 0:2cb6ce8e07bd | 748 | * @retval bridgeId 0 for bridge A , 1 for bridge B |
brdirais | 0:2cb6ce8e07bd | 749 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 750 | uint8_t L6206::L6206_GetMotorIdUsingbridgeInput(uint8_t bridgeInput) |
brdirais | 0:2cb6ce8e07bd | 751 | { |
brdirais | 0:2cb6ce8e07bd | 752 | uint8_t motorId; |
brdirais | 0:2cb6ce8e07bd | 753 | |
brdirais | 0:2cb6ce8e07bd | 754 | switch (devicePrm.config) |
brdirais | 0:2cb6ce8e07bd | 755 | { |
brdirais | 0:2cb6ce8e07bd | 756 | case PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 757 | case PARALLELING_IN1A_IN2A__1_UNDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 758 | case PARALLELING_IN1B_IN2B__1_BIDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 759 | case PARALLELING_IN1A_IN2A__IN1B_IN2B__1_UNDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 760 | if (bridgeInput >= INPUT_1B) |
brdirais | 0:2cb6ce8e07bd | 761 | { |
brdirais | 0:2cb6ce8e07bd | 762 | motorId = 1; |
brdirais | 0:2cb6ce8e07bd | 763 | } |
brdirais | 0:2cb6ce8e07bd | 764 | else |
brdirais | 0:2cb6ce8e07bd | 765 | { |
brdirais | 0:2cb6ce8e07bd | 766 | motorId = 0; |
brdirais | 0:2cb6ce8e07bd | 767 | } |
brdirais | 0:2cb6ce8e07bd | 768 | break; |
brdirais | 0:2cb6ce8e07bd | 769 | case PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 770 | case PARALLELING_IN1A_IN2A__1_UNDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 771 | if (bridgeInput == INPUT_2B) |
brdirais | 0:2cb6ce8e07bd | 772 | { |
brdirais | 0:2cb6ce8e07bd | 773 | motorId = 2; |
brdirais | 0:2cb6ce8e07bd | 774 | } |
brdirais | 0:2cb6ce8e07bd | 775 | else if (bridgeInput == INPUT_1B) |
brdirais | 0:2cb6ce8e07bd | 776 | { |
brdirais | 0:2cb6ce8e07bd | 777 | motorId = 1; |
brdirais | 0:2cb6ce8e07bd | 778 | } |
brdirais | 0:2cb6ce8e07bd | 779 | else |
brdirais | 0:2cb6ce8e07bd | 780 | { |
brdirais | 0:2cb6ce8e07bd | 781 | motorId = 0; |
brdirais | 0:2cb6ce8e07bd | 782 | } |
brdirais | 0:2cb6ce8e07bd | 783 | break; |
brdirais | 0:2cb6ce8e07bd | 784 | case PARALLELING_NONE___2_UNDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 785 | case PARALLELING_IN1B_IN2B__2_UNDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 786 | if (bridgeInput >= INPUT_1B) |
brdirais | 0:2cb6ce8e07bd | 787 | { |
brdirais | 0:2cb6ce8e07bd | 788 | motorId = 2; |
brdirais | 0:2cb6ce8e07bd | 789 | } |
brdirais | 0:2cb6ce8e07bd | 790 | else if (bridgeInput == INPUT_2A) |
brdirais | 0:2cb6ce8e07bd | 791 | { |
brdirais | 0:2cb6ce8e07bd | 792 | motorId = 1; |
brdirais | 0:2cb6ce8e07bd | 793 | } |
brdirais | 0:2cb6ce8e07bd | 794 | else |
brdirais | 0:2cb6ce8e07bd | 795 | { |
brdirais | 0:2cb6ce8e07bd | 796 | motorId = 0; |
brdirais | 0:2cb6ce8e07bd | 797 | } |
brdirais | 0:2cb6ce8e07bd | 798 | break; |
brdirais | 0:2cb6ce8e07bd | 799 | case PARALLELING_NONE___2_UNDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 800 | if (bridgeInput == INPUT_2B) |
brdirais | 0:2cb6ce8e07bd | 801 | { |
brdirais | 0:2cb6ce8e07bd | 802 | motorId = 3; |
brdirais | 0:2cb6ce8e07bd | 803 | } |
brdirais | 0:2cb6ce8e07bd | 804 | else if (bridgeInput == INPUT_1B) |
brdirais | 0:2cb6ce8e07bd | 805 | { |
brdirais | 0:2cb6ce8e07bd | 806 | motorId = 2; |
brdirais | 0:2cb6ce8e07bd | 807 | } |
brdirais | 0:2cb6ce8e07bd | 808 | else if (bridgeInput == INPUT_2A) |
brdirais | 0:2cb6ce8e07bd | 809 | { |
brdirais | 0:2cb6ce8e07bd | 810 | motorId = 1; |
brdirais | 0:2cb6ce8e07bd | 811 | } |
brdirais | 0:2cb6ce8e07bd | 812 | else |
brdirais | 0:2cb6ce8e07bd | 813 | { |
brdirais | 0:2cb6ce8e07bd | 814 | motorId = 0; |
brdirais | 0:2cb6ce8e07bd | 815 | } |
brdirais | 0:2cb6ce8e07bd | 816 | break; |
brdirais | 0:2cb6ce8e07bd | 817 | case PARALLELING_IN1A_IN1B__IN2A_IN2B__1_UNDIR_MOTOR_BRIDGE_1A__1_UNDIR_MOTOR_BRIDGE_2A: |
brdirais | 0:2cb6ce8e07bd | 818 | if ((bridgeInput == INPUT_2A) || (bridgeInput == INPUT_2B)) |
brdirais | 0:2cb6ce8e07bd | 819 | { |
brdirais | 0:2cb6ce8e07bd | 820 | motorId = 1; |
brdirais | 0:2cb6ce8e07bd | 821 | } |
brdirais | 0:2cb6ce8e07bd | 822 | else |
brdirais | 0:2cb6ce8e07bd | 823 | { |
brdirais | 0:2cb6ce8e07bd | 824 | motorId = 0; |
brdirais | 0:2cb6ce8e07bd | 825 | } |
brdirais | 0:2cb6ce8e07bd | 826 | break; |
brdirais | 0:2cb6ce8e07bd | 827 | case PARALLELING_IN1A_IN2A__IN1B_IN2B__1_BIDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 828 | case PARALLELING_IN1A_IN1B__IN2A_IN2B__1_BIDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 829 | case PARALLELING_ALL_WITH_IN1A___1_UNDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 830 | default: |
brdirais | 0:2cb6ce8e07bd | 831 | motorId = 0; |
brdirais | 0:2cb6ce8e07bd | 832 | break; |
brdirais | 0:2cb6ce8e07bd | 833 | } |
brdirais | 0:2cb6ce8e07bd | 834 | |
brdirais | 0:2cb6ce8e07bd | 835 | return (motorId); |
brdirais | 0:2cb6ce8e07bd | 836 | } |
brdirais | 0:2cb6ce8e07bd | 837 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 838 | * @brief Get the PWM input used by a given motor |
brdirais | 0:2cb6ce8e07bd | 839 | * @param motorId from 0 to MAX_NUMBER_OF_BRUSH_DC_MOTORS |
brdirais | 0:2cb6ce8e07bd | 840 | * @retval PWM input 0 for 1A, 1 for 2A, 2 for 1B, 3 for 3B |
brdirais | 0:2cb6ce8e07bd | 841 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 842 | uint8_t L6206::L6206_GetBridgeInputUsedByMotorId(uint8_t motorId) |
brdirais | 0:2cb6ce8e07bd | 843 | { |
brdirais | 0:2cb6ce8e07bd | 844 | uint8_t bridgeInput; |
brdirais | 0:2cb6ce8e07bd | 845 | |
brdirais | 0:2cb6ce8e07bd | 846 | switch (devicePrm.config) |
brdirais | 0:2cb6ce8e07bd | 847 | { |
brdirais | 0:2cb6ce8e07bd | 848 | case PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 849 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 850 | { |
brdirais | 0:2cb6ce8e07bd | 851 | if (devicePrm.direction[0] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 852 | { |
brdirais | 0:2cb6ce8e07bd | 853 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 854 | } |
brdirais | 0:2cb6ce8e07bd | 855 | else |
brdirais | 0:2cb6ce8e07bd | 856 | { |
brdirais | 0:2cb6ce8e07bd | 857 | bridgeInput = INPUT_2A; |
brdirais | 0:2cb6ce8e07bd | 858 | } |
brdirais | 0:2cb6ce8e07bd | 859 | } |
brdirais | 0:2cb6ce8e07bd | 860 | else |
brdirais | 0:2cb6ce8e07bd | 861 | { |
brdirais | 0:2cb6ce8e07bd | 862 | if (devicePrm.direction[1] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 863 | { |
brdirais | 0:2cb6ce8e07bd | 864 | bridgeInput = INPUT_1B; |
brdirais | 0:2cb6ce8e07bd | 865 | } |
brdirais | 0:2cb6ce8e07bd | 866 | else |
brdirais | 0:2cb6ce8e07bd | 867 | { |
brdirais | 0:2cb6ce8e07bd | 868 | bridgeInput = INPUT_2B; |
brdirais | 0:2cb6ce8e07bd | 869 | } |
brdirais | 0:2cb6ce8e07bd | 870 | } |
brdirais | 0:2cb6ce8e07bd | 871 | break; |
brdirais | 0:2cb6ce8e07bd | 872 | case PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 873 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 874 | { |
brdirais | 0:2cb6ce8e07bd | 875 | if (devicePrm.direction[0] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 876 | { |
brdirais | 0:2cb6ce8e07bd | 877 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 878 | } |
brdirais | 0:2cb6ce8e07bd | 879 | else |
brdirais | 0:2cb6ce8e07bd | 880 | { |
brdirais | 0:2cb6ce8e07bd | 881 | bridgeInput = INPUT_2A; |
brdirais | 0:2cb6ce8e07bd | 882 | } |
brdirais | 0:2cb6ce8e07bd | 883 | } |
brdirais | 0:2cb6ce8e07bd | 884 | else if (motorId == 1) |
brdirais | 0:2cb6ce8e07bd | 885 | { |
brdirais | 0:2cb6ce8e07bd | 886 | |
brdirais | 0:2cb6ce8e07bd | 887 | bridgeInput = INPUT_1B; |
brdirais | 0:2cb6ce8e07bd | 888 | } |
brdirais | 0:2cb6ce8e07bd | 889 | else |
brdirais | 0:2cb6ce8e07bd | 890 | { |
brdirais | 0:2cb6ce8e07bd | 891 | bridgeInput = INPUT_2B; |
brdirais | 0:2cb6ce8e07bd | 892 | } |
brdirais | 0:2cb6ce8e07bd | 893 | break; |
brdirais | 0:2cb6ce8e07bd | 894 | case PARALLELING_NONE___2_UNDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 895 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 896 | { |
brdirais | 0:2cb6ce8e07bd | 897 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 898 | } |
brdirais | 0:2cb6ce8e07bd | 899 | else if (motorId == 1) |
brdirais | 0:2cb6ce8e07bd | 900 | { |
brdirais | 0:2cb6ce8e07bd | 901 | bridgeInput = INPUT_2A; |
brdirais | 0:2cb6ce8e07bd | 902 | } |
brdirais | 0:2cb6ce8e07bd | 903 | else |
brdirais | 0:2cb6ce8e07bd | 904 | { |
brdirais | 0:2cb6ce8e07bd | 905 | if (devicePrm.direction[2] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 906 | { |
brdirais | 0:2cb6ce8e07bd | 907 | bridgeInput = INPUT_1B; |
brdirais | 0:2cb6ce8e07bd | 908 | } |
brdirais | 0:2cb6ce8e07bd | 909 | else |
brdirais | 0:2cb6ce8e07bd | 910 | { |
brdirais | 0:2cb6ce8e07bd | 911 | bridgeInput = INPUT_2B; |
brdirais | 0:2cb6ce8e07bd | 912 | } |
brdirais | 0:2cb6ce8e07bd | 913 | } |
brdirais | 0:2cb6ce8e07bd | 914 | break; |
brdirais | 0:2cb6ce8e07bd | 915 | case PARALLELING_NONE___2_UNDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 916 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 917 | { |
brdirais | 0:2cb6ce8e07bd | 918 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 919 | } |
brdirais | 0:2cb6ce8e07bd | 920 | else if (motorId == 1) |
brdirais | 0:2cb6ce8e07bd | 921 | { |
brdirais | 0:2cb6ce8e07bd | 922 | bridgeInput = INPUT_2A; |
brdirais | 0:2cb6ce8e07bd | 923 | } |
brdirais | 0:2cb6ce8e07bd | 924 | else if (motorId == 2) |
brdirais | 0:2cb6ce8e07bd | 925 | { |
brdirais | 0:2cb6ce8e07bd | 926 | bridgeInput = INPUT_1B; |
brdirais | 0:2cb6ce8e07bd | 927 | } |
brdirais | 0:2cb6ce8e07bd | 928 | else |
brdirais | 0:2cb6ce8e07bd | 929 | { |
brdirais | 0:2cb6ce8e07bd | 930 | bridgeInput = INPUT_2B; |
brdirais | 0:2cb6ce8e07bd | 931 | } |
brdirais | 0:2cb6ce8e07bd | 932 | break; |
brdirais | 0:2cb6ce8e07bd | 933 | case PARALLELING_IN1A_IN2A__1_UNDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 934 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 935 | { |
brdirais | 0:2cb6ce8e07bd | 936 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 937 | } |
brdirais | 0:2cb6ce8e07bd | 938 | else |
brdirais | 0:2cb6ce8e07bd | 939 | { |
brdirais | 0:2cb6ce8e07bd | 940 | if (devicePrm.direction[1] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 941 | { |
brdirais | 0:2cb6ce8e07bd | 942 | bridgeInput = INPUT_1B; |
brdirais | 0:2cb6ce8e07bd | 943 | } |
brdirais | 0:2cb6ce8e07bd | 944 | else |
brdirais | 0:2cb6ce8e07bd | 945 | { |
brdirais | 0:2cb6ce8e07bd | 946 | bridgeInput = INPUT_2B; |
brdirais | 0:2cb6ce8e07bd | 947 | } |
brdirais | 0:2cb6ce8e07bd | 948 | } |
brdirais | 0:2cb6ce8e07bd | 949 | break; |
brdirais | 0:2cb6ce8e07bd | 950 | case PARALLELING_IN1A_IN2A__1_UNDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 951 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 952 | { |
brdirais | 0:2cb6ce8e07bd | 953 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 954 | } |
brdirais | 0:2cb6ce8e07bd | 955 | else if (motorId == 1) |
brdirais | 0:2cb6ce8e07bd | 956 | { |
brdirais | 0:2cb6ce8e07bd | 957 | bridgeInput = INPUT_1B; |
brdirais | 0:2cb6ce8e07bd | 958 | } |
brdirais | 0:2cb6ce8e07bd | 959 | else |
brdirais | 0:2cb6ce8e07bd | 960 | { |
brdirais | 0:2cb6ce8e07bd | 961 | bridgeInput = INPUT_2B; |
brdirais | 0:2cb6ce8e07bd | 962 | } |
brdirais | 0:2cb6ce8e07bd | 963 | break; |
brdirais | 0:2cb6ce8e07bd | 964 | case PARALLELING_IN1B_IN2B__1_BIDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 965 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 966 | { |
brdirais | 0:2cb6ce8e07bd | 967 | if (devicePrm.direction[0] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 968 | { |
brdirais | 0:2cb6ce8e07bd | 969 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 970 | } |
brdirais | 0:2cb6ce8e07bd | 971 | else |
brdirais | 0:2cb6ce8e07bd | 972 | { |
brdirais | 0:2cb6ce8e07bd | 973 | bridgeInput = INPUT_2A; |
brdirais | 0:2cb6ce8e07bd | 974 | } |
brdirais | 0:2cb6ce8e07bd | 975 | } |
brdirais | 0:2cb6ce8e07bd | 976 | else |
brdirais | 0:2cb6ce8e07bd | 977 | { |
brdirais | 0:2cb6ce8e07bd | 978 | bridgeInput = INPUT_1B; |
brdirais | 0:2cb6ce8e07bd | 979 | } |
brdirais | 0:2cb6ce8e07bd | 980 | break; |
brdirais | 0:2cb6ce8e07bd | 981 | case PARALLELING_IN1B_IN2B__2_UNDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 982 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 983 | { |
brdirais | 0:2cb6ce8e07bd | 984 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 985 | } |
brdirais | 0:2cb6ce8e07bd | 986 | else if (motorId == 1) |
brdirais | 0:2cb6ce8e07bd | 987 | { |
brdirais | 0:2cb6ce8e07bd | 988 | bridgeInput = INPUT_2A; |
brdirais | 0:2cb6ce8e07bd | 989 | } |
brdirais | 0:2cb6ce8e07bd | 990 | else |
brdirais | 0:2cb6ce8e07bd | 991 | { |
brdirais | 0:2cb6ce8e07bd | 992 | bridgeInput = INPUT_1B; |
brdirais | 0:2cb6ce8e07bd | 993 | } |
brdirais | 0:2cb6ce8e07bd | 994 | break; |
brdirais | 0:2cb6ce8e07bd | 995 | case PARALLELING_IN1A_IN2A__IN1B_IN2B__1_UNDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 996 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 997 | { |
brdirais | 0:2cb6ce8e07bd | 998 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 999 | } |
brdirais | 0:2cb6ce8e07bd | 1000 | else |
brdirais | 0:2cb6ce8e07bd | 1001 | { |
brdirais | 0:2cb6ce8e07bd | 1002 | bridgeInput = INPUT_1B; |
brdirais | 0:2cb6ce8e07bd | 1003 | } |
brdirais | 0:2cb6ce8e07bd | 1004 | break; |
brdirais | 0:2cb6ce8e07bd | 1005 | case PARALLELING_IN1A_IN2A__IN1B_IN2B__1_BIDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 1006 | if (devicePrm.direction[0] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 1007 | { |
brdirais | 0:2cb6ce8e07bd | 1008 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 1009 | } |
brdirais | 0:2cb6ce8e07bd | 1010 | else |
brdirais | 0:2cb6ce8e07bd | 1011 | { |
brdirais | 0:2cb6ce8e07bd | 1012 | bridgeInput = INPUT_1B; |
brdirais | 0:2cb6ce8e07bd | 1013 | } |
brdirais | 0:2cb6ce8e07bd | 1014 | break; |
brdirais | 0:2cb6ce8e07bd | 1015 | case PARALLELING_IN1A_IN1B__IN2A_IN2B__1_UNDIR_MOTOR_BRIDGE_1A__1_UNDIR_MOTOR_BRIDGE_2A: |
brdirais | 0:2cb6ce8e07bd | 1016 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 1017 | { |
brdirais | 0:2cb6ce8e07bd | 1018 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 1019 | } |
brdirais | 0:2cb6ce8e07bd | 1020 | else |
brdirais | 0:2cb6ce8e07bd | 1021 | { |
brdirais | 0:2cb6ce8e07bd | 1022 | bridgeInput = INPUT_2A; |
brdirais | 0:2cb6ce8e07bd | 1023 | } |
brdirais | 0:2cb6ce8e07bd | 1024 | break; |
brdirais | 0:2cb6ce8e07bd | 1025 | case PARALLELING_IN1A_IN1B__IN2A_IN2B__1_BIDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 1026 | if (devicePrm.direction[0] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 1027 | { |
brdirais | 0:2cb6ce8e07bd | 1028 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 1029 | } |
brdirais | 0:2cb6ce8e07bd | 1030 | else |
brdirais | 0:2cb6ce8e07bd | 1031 | { |
brdirais | 0:2cb6ce8e07bd | 1032 | bridgeInput = INPUT_2A; |
brdirais | 0:2cb6ce8e07bd | 1033 | } |
brdirais | 0:2cb6ce8e07bd | 1034 | break; |
brdirais | 0:2cb6ce8e07bd | 1035 | case PARALLELING_ALL_WITH_IN1A___1_UNDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 1036 | default: |
brdirais | 0:2cb6ce8e07bd | 1037 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 1038 | break; |
brdirais | 0:2cb6ce8e07bd | 1039 | } |
brdirais | 0:2cb6ce8e07bd | 1040 | return (bridgeInput); |
brdirais | 0:2cb6ce8e07bd | 1041 | } |
brdirais | 0:2cb6ce8e07bd | 1042 | |
brdirais | 0:2cb6ce8e07bd | 1043 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 1044 | * @brief Get the second PWM input used by a given bidirectionnal motor |
brdirais | 0:2cb6ce8e07bd | 1045 | * @param motorId from 0 to MAX_NUMBER_OF_BRUSH_DC_MOTORS |
brdirais | 0:2cb6ce8e07bd | 1046 | * @retval PWM input 0 for 1A, 1 for 2A, 2 for 1B, 3 for 3B |
brdirais | 0:2cb6ce8e07bd | 1047 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 1048 | uint8_t L6206::L6206_GetSecondBridgeInputUsedByMotorId(uint8_t motorId) |
brdirais | 0:2cb6ce8e07bd | 1049 | { |
brdirais | 0:2cb6ce8e07bd | 1050 | uint8_t bridgeInput = 0xFF; |
brdirais | 0:2cb6ce8e07bd | 1051 | |
brdirais | 0:2cb6ce8e07bd | 1052 | switch (devicePrm.config) |
brdirais | 0:2cb6ce8e07bd | 1053 | { |
brdirais | 0:2cb6ce8e07bd | 1054 | case PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 1055 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 1056 | { |
brdirais | 0:2cb6ce8e07bd | 1057 | if (devicePrm.direction[0] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 1058 | { |
brdirais | 0:2cb6ce8e07bd | 1059 | bridgeInput = INPUT_2A; |
brdirais | 0:2cb6ce8e07bd | 1060 | } |
brdirais | 0:2cb6ce8e07bd | 1061 | else |
brdirais | 0:2cb6ce8e07bd | 1062 | { |
brdirais | 0:2cb6ce8e07bd | 1063 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 1064 | } |
brdirais | 0:2cb6ce8e07bd | 1065 | } |
brdirais | 0:2cb6ce8e07bd | 1066 | else |
brdirais | 0:2cb6ce8e07bd | 1067 | { |
brdirais | 0:2cb6ce8e07bd | 1068 | if (devicePrm.direction[1] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 1069 | { |
brdirais | 0:2cb6ce8e07bd | 1070 | bridgeInput = INPUT_2B; |
brdirais | 0:2cb6ce8e07bd | 1071 | } |
brdirais | 0:2cb6ce8e07bd | 1072 | else |
brdirais | 0:2cb6ce8e07bd | 1073 | { |
brdirais | 0:2cb6ce8e07bd | 1074 | bridgeInput = INPUT_1B; |
brdirais | 0:2cb6ce8e07bd | 1075 | } |
brdirais | 0:2cb6ce8e07bd | 1076 | } |
brdirais | 0:2cb6ce8e07bd | 1077 | break; |
brdirais | 0:2cb6ce8e07bd | 1078 | case PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 1079 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 1080 | { |
brdirais | 0:2cb6ce8e07bd | 1081 | if (devicePrm.direction[0] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 1082 | { |
brdirais | 0:2cb6ce8e07bd | 1083 | bridgeInput = INPUT_2A; |
brdirais | 0:2cb6ce8e07bd | 1084 | } |
brdirais | 0:2cb6ce8e07bd | 1085 | else |
brdirais | 0:2cb6ce8e07bd | 1086 | { |
brdirais | 0:2cb6ce8e07bd | 1087 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 1088 | } |
brdirais | 0:2cb6ce8e07bd | 1089 | } |
brdirais | 0:2cb6ce8e07bd | 1090 | break; |
brdirais | 0:2cb6ce8e07bd | 1091 | case PARALLELING_NONE___2_UNDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 1092 | if (motorId == 2) |
brdirais | 0:2cb6ce8e07bd | 1093 | { |
brdirais | 0:2cb6ce8e07bd | 1094 | if (devicePrm.direction[2] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 1095 | { |
brdirais | 0:2cb6ce8e07bd | 1096 | bridgeInput = INPUT_2B; |
brdirais | 0:2cb6ce8e07bd | 1097 | } |
brdirais | 0:2cb6ce8e07bd | 1098 | else |
brdirais | 0:2cb6ce8e07bd | 1099 | { |
brdirais | 0:2cb6ce8e07bd | 1100 | bridgeInput = INPUT_1B; |
brdirais | 0:2cb6ce8e07bd | 1101 | } |
brdirais | 0:2cb6ce8e07bd | 1102 | } |
brdirais | 0:2cb6ce8e07bd | 1103 | break; |
brdirais | 0:2cb6ce8e07bd | 1104 | |
brdirais | 0:2cb6ce8e07bd | 1105 | case PARALLELING_IN1A_IN2A__1_UNDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 1106 | if (motorId == 1) |
brdirais | 0:2cb6ce8e07bd | 1107 | { |
brdirais | 0:2cb6ce8e07bd | 1108 | if (devicePrm.direction[1] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 1109 | { |
brdirais | 0:2cb6ce8e07bd | 1110 | bridgeInput = INPUT_2B; |
brdirais | 0:2cb6ce8e07bd | 1111 | } |
brdirais | 0:2cb6ce8e07bd | 1112 | else |
brdirais | 0:2cb6ce8e07bd | 1113 | { |
brdirais | 0:2cb6ce8e07bd | 1114 | bridgeInput = INPUT_1B; |
brdirais | 0:2cb6ce8e07bd | 1115 | } |
brdirais | 0:2cb6ce8e07bd | 1116 | } |
brdirais | 0:2cb6ce8e07bd | 1117 | break; |
brdirais | 0:2cb6ce8e07bd | 1118 | |
brdirais | 0:2cb6ce8e07bd | 1119 | case PARALLELING_IN1B_IN2B__1_BIDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 1120 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 1121 | { |
brdirais | 0:2cb6ce8e07bd | 1122 | if (devicePrm.direction[0] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 1123 | { |
brdirais | 0:2cb6ce8e07bd | 1124 | bridgeInput = INPUT_2A; |
brdirais | 0:2cb6ce8e07bd | 1125 | } |
brdirais | 0:2cb6ce8e07bd | 1126 | else |
brdirais | 0:2cb6ce8e07bd | 1127 | { |
brdirais | 0:2cb6ce8e07bd | 1128 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 1129 | } |
brdirais | 0:2cb6ce8e07bd | 1130 | } |
brdirais | 0:2cb6ce8e07bd | 1131 | break; |
brdirais | 0:2cb6ce8e07bd | 1132 | case PARALLELING_IN1A_IN2A__IN1B_IN2B__1_BIDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 1133 | if (devicePrm.direction[0] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 1134 | { |
brdirais | 0:2cb6ce8e07bd | 1135 | bridgeInput = INPUT_1B; |
brdirais | 0:2cb6ce8e07bd | 1136 | } |
brdirais | 0:2cb6ce8e07bd | 1137 | else |
brdirais | 0:2cb6ce8e07bd | 1138 | { |
brdirais | 0:2cb6ce8e07bd | 1139 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 1140 | } |
brdirais | 0:2cb6ce8e07bd | 1141 | break; |
brdirais | 0:2cb6ce8e07bd | 1142 | |
brdirais | 0:2cb6ce8e07bd | 1143 | case PARALLELING_IN1A_IN1B__IN2A_IN2B__1_BIDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 1144 | if (devicePrm.direction[0] == FORWARD) |
brdirais | 0:2cb6ce8e07bd | 1145 | { |
brdirais | 0:2cb6ce8e07bd | 1146 | bridgeInput = INPUT_2A; |
brdirais | 0:2cb6ce8e07bd | 1147 | } |
brdirais | 0:2cb6ce8e07bd | 1148 | else |
brdirais | 0:2cb6ce8e07bd | 1149 | { |
brdirais | 0:2cb6ce8e07bd | 1150 | bridgeInput = INPUT_1A; |
brdirais | 0:2cb6ce8e07bd | 1151 | } |
brdirais | 0:2cb6ce8e07bd | 1152 | break; |
brdirais | 0:2cb6ce8e07bd | 1153 | default: |
brdirais | 0:2cb6ce8e07bd | 1154 | bridgeInput = 0XFF; |
brdirais | 0:2cb6ce8e07bd | 1155 | break; |
brdirais | 0:2cb6ce8e07bd | 1156 | } |
brdirais | 0:2cb6ce8e07bd | 1157 | if (bridgeInput == 0XFF) |
brdirais | 0:2cb6ce8e07bd | 1158 | { |
brdirais | 0:2cb6ce8e07bd | 1159 | L6206_ErrorHandler(L6206_ERROR_2); |
brdirais | 0:2cb6ce8e07bd | 1160 | } |
brdirais | 0:2cb6ce8e07bd | 1161 | |
brdirais | 0:2cb6ce8e07bd | 1162 | return (bridgeInput); |
brdirais | 0:2cb6ce8e07bd | 1163 | } |
brdirais | 0:2cb6ce8e07bd | 1164 | |
brdirais | 0:2cb6ce8e07bd | 1165 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 1166 | * @brief Test if motor is bidirectionnal |
brdirais | 0:2cb6ce8e07bd | 1167 | * @param motorId from 0 to MAX_NUMBER_OF_BRUSH_DC_MOTORS |
brdirais | 0:2cb6ce8e07bd | 1168 | * @retval True if motor is bidirectionnal, else false |
brdirais | 0:2cb6ce8e07bd | 1169 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 1170 | bool L6206::L6206_IsBidirectionnalMotor(uint8_t motorId) |
brdirais | 0:2cb6ce8e07bd | 1171 | { |
brdirais | 0:2cb6ce8e07bd | 1172 | bool isBiDir = FALSE; |
brdirais | 0:2cb6ce8e07bd | 1173 | |
brdirais | 0:2cb6ce8e07bd | 1174 | switch (devicePrm.config) |
brdirais | 0:2cb6ce8e07bd | 1175 | { |
brdirais | 0:2cb6ce8e07bd | 1176 | case PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 1177 | case PARALLELING_IN1A_IN2A__IN1B_IN2B__1_BIDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 1178 | case PARALLELING_IN1A_IN1B__IN2A_IN2B__1_BIDIR_MOTOR: |
brdirais | 0:2cb6ce8e07bd | 1179 | isBiDir = TRUE; |
brdirais | 0:2cb6ce8e07bd | 1180 | break; |
brdirais | 0:2cb6ce8e07bd | 1181 | |
brdirais | 0:2cb6ce8e07bd | 1182 | case PARALLELING_NONE___1_BIDIR_MOTOR_BRIDGE_A__2_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 1183 | case PARALLELING_IN1B_IN2B__1_BIDIR_MOTOR_BRIDGE_A__1_UNDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 1184 | if (motorId == 0) |
brdirais | 0:2cb6ce8e07bd | 1185 | { |
brdirais | 0:2cb6ce8e07bd | 1186 | isBiDir = TRUE; |
brdirais | 0:2cb6ce8e07bd | 1187 | } |
brdirais | 0:2cb6ce8e07bd | 1188 | break; |
brdirais | 0:2cb6ce8e07bd | 1189 | case PARALLELING_NONE___2_UNDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 1190 | if (motorId == 2) |
brdirais | 0:2cb6ce8e07bd | 1191 | { |
brdirais | 0:2cb6ce8e07bd | 1192 | isBiDir = TRUE; |
brdirais | 0:2cb6ce8e07bd | 1193 | } |
brdirais | 0:2cb6ce8e07bd | 1194 | break; |
brdirais | 0:2cb6ce8e07bd | 1195 | case PARALLELING_IN1A_IN2A__1_UNDIR_MOTOR_BRIDGE_A__1_BIDIR_MOTOR_BRIDGE_B: |
brdirais | 0:2cb6ce8e07bd | 1196 | if (motorId == 1) |
brdirais | 0:2cb6ce8e07bd | 1197 | { |
brdirais | 0:2cb6ce8e07bd | 1198 | isBiDir = TRUE; |
brdirais | 0:2cb6ce8e07bd | 1199 | } |
brdirais | 0:2cb6ce8e07bd | 1200 | break; |
brdirais | 0:2cb6ce8e07bd | 1201 | |
brdirais | 0:2cb6ce8e07bd | 1202 | default: |
brdirais | 0:2cb6ce8e07bd | 1203 | break; |
brdirais | 0:2cb6ce8e07bd | 1204 | } |
brdirais | 0:2cb6ce8e07bd | 1205 | |
brdirais | 0:2cb6ce8e07bd | 1206 | return (isBiDir); |
brdirais | 0:2cb6ce8e07bd | 1207 | } |
brdirais | 0:2cb6ce8e07bd | 1208 | |
brdirais | 0:2cb6ce8e07bd | 1209 | |
brdirais | 0:2cb6ce8e07bd | 1210 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 1211 | * @brief Sets the parameters of the device to predefined values |
brdirais | 0:2cb6ce8e07bd | 1212 | * from l6206_target_config.h |
brdirais | 0:2cb6ce8e07bd | 1213 | * @retval None |
brdirais | 0:2cb6ce8e07bd | 1214 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 1215 | void L6206::L6206_SetDeviceParamsToPredefinedValues(void) |
brdirais | 0:2cb6ce8e07bd | 1216 | { |
brdirais | 0:2cb6ce8e07bd | 1217 | uint32_t i; |
brdirais | 0:2cb6ce8e07bd | 1218 | |
brdirais | 0:2cb6ce8e07bd | 1219 | memset(&devicePrm, 0, sizeof(devicePrm)); |
brdirais | 0:2cb6ce8e07bd | 1220 | |
brdirais | 0:2cb6ce8e07bd | 1221 | devicePrm.config = L6206_CONF_PARAM_PARALLE_BRIDGES; |
brdirais | 0:2cb6ce8e07bd | 1222 | |
brdirais | 0:2cb6ce8e07bd | 1223 | devicePrm.pwmFreq[INPUT_1A] = L6206_CONF_PARAM_FREQ_PWM1A; |
brdirais | 0:2cb6ce8e07bd | 1224 | devicePrm.pwmFreq[INPUT_2A] = L6206_CONF_PARAM_FREQ_PWM2A; |
brdirais | 0:2cb6ce8e07bd | 1225 | devicePrm.pwmFreq[INPUT_1B] = L6206_CONF_PARAM_FREQ_PWM1B; |
brdirais | 0:2cb6ce8e07bd | 1226 | devicePrm.pwmFreq[INPUT_2B] = L6206_CONF_PARAM_FREQ_PWM2B; |
brdirais | 0:2cb6ce8e07bd | 1227 | |
brdirais | 0:2cb6ce8e07bd | 1228 | for (i = 0; i < MAX_NUMBER_OF_BRUSH_DC_MOTORS; i++) |
brdirais | 0:2cb6ce8e07bd | 1229 | { |
brdirais | 0:2cb6ce8e07bd | 1230 | devicePrm.speed[i] = 100; |
brdirais | 0:2cb6ce8e07bd | 1231 | devicePrm.direction[i] = FORWARD; |
brdirais | 0:2cb6ce8e07bd | 1232 | devicePrm.motionState[i] = INACTIVE; |
brdirais | 0:2cb6ce8e07bd | 1233 | } |
brdirais | 0:2cb6ce8e07bd | 1234 | for (i = 0; i < L6206_NB_MAX_BRIDGES; i++) |
brdirais | 0:2cb6ce8e07bd | 1235 | { |
brdirais | 0:2cb6ce8e07bd | 1236 | devicePrm.bridgeEnabled[i] = FALSE; |
brdirais | 0:2cb6ce8e07bd | 1237 | } |
brdirais | 0:2cb6ce8e07bd | 1238 | } |
brdirais | 0:2cb6ce8e07bd | 1239 | |
brdirais | 0:2cb6ce8e07bd | 1240 | |
brdirais | 0:2cb6ce8e07bd | 1241 | /******************************************************//** |
brdirais | 0:2cb6ce8e07bd | 1242 | * @brief Set the parameters of the device to values of initDevicePrm structure |
brdirais | 0:2cb6ce8e07bd | 1243 | * Set GPIO according to these values |
brdirais | 0:2cb6ce8e07bd | 1244 | * @param initDevicePrm structure containing values to initialize the device |
brdirais | 0:2cb6ce8e07bd | 1245 | * parameters |
brdirais | 0:2cb6ce8e07bd | 1246 | * @retval None |
brdirais | 0:2cb6ce8e07bd | 1247 | **********************************************************/ |
brdirais | 0:2cb6ce8e07bd | 1248 | void L6206::L6206_SetDeviceParamsToGivenValues(L6206_Init_t* initDevicePrm) |
brdirais | 0:2cb6ce8e07bd | 1249 | { |
brdirais | 0:2cb6ce8e07bd | 1250 | memcpy(&devicePrm, initDevicePrm, sizeof(devicePrm)); |
brdirais | 0:2cb6ce8e07bd | 1251 | } |
brdirais | 0:2cb6ce8e07bd | 1252 | |
brdirais | 0:2cb6ce8e07bd | 1253 | |
brdirais | 0:2cb6ce8e07bd | 1254 | |
brdirais | 0:2cb6ce8e07bd | 1255 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ |
brdirais | 0:2cb6ce8e07bd | 1256 | |
brdirais | 0:2cb6ce8e07bd | 1257 | |
brdirais | 0:2cb6ce8e07bd | 1258 |